Title: Anything wrong with my code ? Post by: anilkunchalaece on July 14, 2018, 12:35:48 AM Hi,
I am trying to tune quad copter (single axis) via string setup. For pitch axis it is some what ok (I think), quad stabilized with P alone (is it weird ?). But for Roll - i'm unable to tune it. it just rolls as soon small throttle applied. (I dont even see oscillations) I changed X config to + Config but same reaction. I am thinking there may be some bug in my code for roll (or pitch ) PID calculation (I'm not sure) which is not visible to me. So If you guys found anything weird in following code let me know /* ref - http://ardupilot.org/dev/docs/apmcopter-programming-attitude-control-2.html and ymfc-al code angle error (difference between target error and actual angle) is converted in to desired rotation rate followed by previous rate pid from ksrmQuadRateMode The input pulses from RC receiver are converted to the +/- rate of rotation. The pilot input acts as the SetPoint to roll,pitch and yaw rate PID's. Measured input to PID comes from gyroscope Motor directions for Quad + configuration 5 (cw) | | (ccw) 6 ------|------ (ccw)4 | | 7 (cw) Arduino Pin m0 right - ccw 4 m1 top - cw 5 m2 left - ccw 6 m3 bottom - cw 7 MPU6050 Connections SCL - A5 SDA - A4 Rx connections Enable PCMSK0 Register - PCMSK0 - portB (D8-D13) (PCINT0-PCINT6) ArduinoPin PCINTx ch1 - 8 PCINT0 - Roll ch2 - 9 PCINT1 - Pitch ch3 - 10 PCINT2 - Throttle ch4 - 11 PCINT3 - YAW Arm and Disarm mechanism To Arm - Throttle Min && Yaw Max To DisArm - Throttle Min && Roll Max */ // Dont use servo - it is limited to 50Hz and conflict with Software serial // If you are planning for more serial devices go for arduino Mega // I use blutooth to get flight parameters - it will be used with hardware serial #include<PID_v1.h> #include<Wire.h> #include<math.h> //used to send debug info via bluetooth //#define DEBUG #define DATA_INTERVAL 200 //send debug info every 200 milli seconds unsigned long btDataStartMillis; //motor connections #define m0 4 #define m1 5 #define m2 6 #define m3 7 //receiver connections #define ch0 8 #define ch1 9 #define ch2 10 #define ch3 11 #define motorMinValue 1150 #define motorMaxValue 1800 //variables used for MPU6050 Angle calculation const float GYRO_SENSITIVITY_SCALE_FACTOR = 65.5; //for 500 degrees/sec full scale //MPU6050 I2C address const int MPU6050_ADDR = 0b1101000; //MPU6050 Register Addresses const int PWR_REG_ADDR = 0x6B; const int GYRO_CONFIG_REG_ADDR = 0x1B; const int GYRO_CONFIG_REG_VALUE = 0x08;//for 500 degrees / sec const int ACCR_CONFIG_REG_ADDR = 0x1C; const int ACCR_CONFIG_REG_VALUE = 0x10; //for +/- 8g const int ACCR_READ_START_ADDR = 0x3B; //variables used for angular rate and angle calculations const float radToDegreeConvert = 180.0 / PI; int16_t accX, accY, accZ, gyroX, gyroY, gyroZ, tmp; //used to store the data from MPU6050 registers float rotX, rotY, rotZ; //used to store the angular rotation from gyro float accAngleX = 0.0, accAngleY = 0.0, accrAngleZ = 0.0; float compAngleX = 0.0, compAngleY = 0.0; float pitchAngle = 0.0, rollAngle = 0.0; float pitchAngleAdjust = 0.0, rollAngleAdjust = 0.0; unsigned long prevTime = 0, currentTime = 0; //used to calculate the dt for gyro integration unsigned long loopTimer = 0, now = 0, difference = 0; //used to calculate the loop execution time //gyro offset variables float gyroOffsetValX = 0, gyroOffsetValY = 0, gyroOffsetValZ = 0; float accrOffsetValX = 0, accrOffsetValY = 0; const int noOfSamplesForOffset = 400; //variables used in pinChange ISR volatile boolean armMotors = false; //used to arm and disarm quad volatile int pwmDuration[4]; //used to store pwm duration volatile unsigned long pwmStart[4]; //port status volatile byte prevPortState[] = {0, 0, 0, 0}; volatile byte presentPortState[4]; //pinDeclarations for Rx const byte rxCh[] = {ch0, ch1, ch2, ch3}; const byte noOfChannels = sizeof(rxCh); //PID Constants //0.5 const float pitchRatePGain = 1.0; const float pitchRateIGain = 0.0; const float pitchRateDGain = 0.0; const float rollRatePGain = 1.0; const float rollRateIGain = 0.0; const float rollRateDGain = 0.0; const float yawRatePGain = 0.0; const float yawRateIGain = 0.0; const float yawRateDGain = 0.0; const int pidOutMax = 400; // PID variables float pidPitchRateIn = 0.0, pidPitchRateOut = 0.0, pidPitchRateSetPoint = 0.0; float pidRollRateIn = 0.0, pidRollRateOut = 0.0, pidRollRateSetPoint = 0.0; float pidYawRateIn = 0.0, pidYawRateOut = 0.0, pidYawRateSetPoint = 0.0; float pitchError = 0.0, rollError = 0.0, yawError = 0.0; float pitchPrevError = 0.0, rollPrevError = 0.0, yawPrevError = 0.0; float pitchErrorSum = 0.0, rollErrorSum = 0.0, yawErrorSum = 0.0; //for integral coefficient float pitchErrorDelta = 0.0 , rollErrorDelta = 0.0 , yawErrorDelta = 0.0; //for differntial coeffcient const int pidMax = 400; //variables to store the motor values calculated using pid and throttle int m0Value, m1Value, m2Value, m3Value; //Interrupt Service Routine will fire when for PinChange in PortB ISR(PCINT0_vect) { for (int ch = 0; ch < noOfChannels ; ch++) { presentPortState[ch] = digitalRead(rxCh[ch]); }//end of for looptr for (int c = 0; c < noOfChannels ; c++) { if (prevPortState[c] == 0 & presentPortState[c] == 1) { //if previous state is 0 and present state is 1 (Raising Edge) then take the time stamp pwmStart[c] = micros(); prevPortState[c] = 1; //update the prevPort State } else if (prevPortState[c] == 1 & presentPortState[c] == 0) { //if previous state is 1 and present state is 0 (Falling Edge) then calculate the width based on the change pwmDuration[c] = micros() - pwmStart[c]; prevPortState[c] = 0; //update Present PortState } }//end of for loop //ARM motors if (pwmDuration[2] < 1250 && pwmDuration[3] > 1700) { //Throttle Min And Yaw Max armMotors = true; } //DISARM Motors if (pwmDuration[2] < 1250 && pwmDuration[0] > 1600) { //Throttle Min and Roll Max armMotors = false; } }//end of ISR Fcn void initReceiver() { cli(); //Clear all interrupts PCICR |= 1 << PCIE0; //Enable port B Registers i.e D8-D13 PCMSK0 |= 1 << PCINT3 | 1 << PCINT2 | 1 << PCINT1 | 1 << PCINT0 ; // Pin11,10,9,8 sei(); //enable all interrupts for (int i = 0 ; i < noOfChannels ; i++) { pinMode(rxCh, INPUT); digitalWrite(rxCh, HIGH); //enable pullup }//end of for loop }//end of initReceiver Fcn void updateMotors() { int throttle = pwmDuration[2]; m0Value = throttle - pidRollRateOut + pidYawRateOut; //right m1Value = throttle - pidPitchRateOut - pidYawRateOut; //top m2Value = throttle + pidRollRateOut + pidYawRateOut; //left m3Value = throttle + pidPitchRateOut - pidYawRateOut; //bottom //dont send values more than motorMaxValue - ESC may get into trouble if (m0Value > motorMaxValue) m0Value = motorMaxValue; if (m1Value > motorMaxValue) m1Value = motorMaxValue; if (m2Value > motorMaxValue) m2Value = motorMaxValue; if (m3Value > motorMaxValue) m3Value = motorMaxValue; //dont send values less than motorMinValue - Keep motors running if (m0Value < motorMinValue) m0Value = motorMinValue + 10; if (m1Value < motorMinValue) m1Value = motorMinValue + 10; if (m2Value < motorMinValue) m2Value = motorMinValue + 10; if (m3Value < motorMinValue) m3Value = motorMinValue + 10; // Refresh rate is 250Hz: send ESC pulses every 4000µs while ((now = micros()) - loopTimer < 4000); // Update loop timer loopTimer = now; // Set pins #4 #5 #6 #7 HIGH PORTD |= B11110000; // Wait until all pins #4 #5 #6 #7 are LOW while (PORTD >= 16) { now = micros(); difference = now - loopTimer; if (difference >= m0Value) PORTD &= B11101111; // Set pin #4 LOW if (difference >= m1Value) PORTD &= B11011111; // Set pin #5 LOW if (difference >= m2Value) PORTD &= B10111111; // Set pin #6 LOW if (difference >= m3Value) PORTD &= B01111111; // Set pin #7 LOW }//end of while loop }//end of updateMotors Fcn void initializeMotors() { pinMode(m0, OUTPUT); pinMode(m1, OUTPUT); pinMode(m2, OUTPUT); pinMode(m3, OUTPUT); }//end of initializeMotors Fcn /* This function will convert Rx inputs into angular rate which are used as SetPoints for PID */ void setPointUpdate() { pidRollRateSetPoint = 0.0; pidPitchRateSetPoint = 0.0; pidYawRateSetPoint = 0.0; if (pwmDuration[0] > 1508) { pidRollRateSetPoint = pwmDuration[0] - 1508; } else if (pwmDuration[0] < 1492) { pidRollRateSetPoint = pwmDuration[0] - 1492; } rollAngleAdjust = rollAngle * 15; // convert 0 - 30 to 0 -450 pidRollRateSetPoint = pidRollRateSetPoint - rollAngleAdjust; //calculate angleError pidRollRateSetPoint = pidRollRateSetPoint / 3.0 ;// the max roll rate is aprox 164 degrees per second (500/3 = 165 d/s ) if (pwmDuration[1] > 1508) { pidPitchRateSetPoint = pwmDuration[1] - 1508; } else if (pwmDuration[1] < 1492) { pidPitchRateSetPoint = pwmDuration[1] - 1492; } pitchAngleAdjust = pitchAngle * 15; pidPitchRateSetPoint = pidPitchRateSetPoint - pitchAngleAdjust; pidPitchRateSetPoint = pidPitchRateSetPoint / 3.0; if (pwmDuration[3] > 1508) { pidYawRateSetPoint = pwmDuration[3] - 1508; } else if (pwmDuration[3] < 1492) { pidYawRateSetPoint = pwmDuration[3] - 1492; } pidYawRateSetPoint = pidYawRateSetPoint / 3.0; }//end of SetPointUpdate Fcn void initPID() { //nothing to initialize here }//end of initPID Fcn /* In this fuction we configure mpu6050 and calculate offsets i.e calibration */ void initMPU6050() { configureMPU(); calculateOffsets(); }//end of initMPU6050 Fcn void calculateOffsets() { float gyroTotalValX = 0, gyroTotalValY = 0, gyroTotalValZ = 0; float accrTotalValX = 0, accrTotalValY = 0; for (int i = 0 ; i < noOfSamplesForOffset ; i++) { readMPU(); delayMicroseconds(10); gyroTotalValX = gyroTotalValX + rotX; gyroTotalValY = gyroTotalValY + rotY; gyroTotalValZ = gyroTotalValZ + rotZ; accAngleY = atan(accX / sqrt(pow(accY, 2) + pow(accZ, 2))) * radToDegreeConvert; accAngleX = atan(accY / sqrt(pow(accX, 2) + pow(accZ, 2))) * radToDegreeConvert; accrTotalValX = accrTotalValX + accAngleX; accrTotalValY = accrTotalValY + accAngleY; // Serial.println(".........."); }//end of for loop gyroOffsetValX = gyroTotalValX / noOfSamplesForOffset; gyroOffsetValY = gyroTotalValY / noOfSamplesForOffset; gyroOffsetValZ = gyroTotalValZ / noOfSamplesForOffset; accrOffsetValX = accrTotalValX / noOfSamplesForOffset; accrOffsetValY = accrTotalValY / noOfSamplesForOffset; //read again to get current values readMPU(); accAngleY = atan(accX / sqrt(pow(accY, 2) + pow(accZ, 2))) * radToDegreeConvert; accAngleX = atan(accY / sqrt(pow(accX, 2) + pow(accZ, 2))) * radToDegreeConvert; //apply offsets to accr values accAngleX = accAngleX - accrOffsetValX; accAngleY = accAngleY - accrOffsetValY; //set initial values for gyro and complementary filter rotX = accAngleX; rotY = accAngleY; compAngleX = accAngleX; compAngleY = accAngleY; }//end of calculateGyroOffsets Fcn void readMPU() { Wire.beginTransmission(MPU6050_ADDR); Wire.write(ACCR_READ_START_ADDR); // starting with register 0x3B (ACCEL_XOUT_H) Wire.endTransmission(false); Wire.requestFrom(MPU6050_ADDR, 14, true); while (Wire.available() < 14) { //wait until data is available #ifdef DEBUG Serial.println(F("MPU6050 waiting")); #endif } accX = Wire.read() << 8 | Wire.read(); accY = Wire.read() << 8 | Wire.read(); accZ = Wire.read() << 8 | Wire.read(); tmp = Wire.read() << 8 | Wire.read(); gyroX = Wire.read() << 8 | Wire.read(); gyroY = Wire.read() << 8 | Wire.read(); gyroZ = Wire.read() << 8 | Wire.read(); //apply scale factor for gyro reading rotX = gyroX / GYRO_SENSITIVITY_SCALE_FACTOR; rotY = gyroY / GYRO_SENSITIVITY_SCALE_FACTOR; rotZ = gyroZ / GYRO_SENSITIVITY_SCALE_FACTOR; }//end of readGyroX Fcn /* This function will apply complementary filer to gyro angular rates and assign them as PID inputs */ void calculateRotationRates() { //apply the offset rotX = rotX - gyroOffsetValX; rotY = rotY - gyroOffsetValY; rotZ = rotZ - gyroOffsetValZ; //complemenrary filter for gyro readings pidPitchRateIn = (pidPitchRateIn * 0.8) + (rotX * 0.2); pidRollRateIn = (pidRollRateIn * 0.8) + (rotY * 0.2); pidYawRateIn = (pidYawRateIn * 0.8) + (rotZ * 0.2); }//end of calculateRotationRates Fcn /* This Fcn will calculate the angles using complementary filter */ void calculateAngles() { //angles from accr accAngleY = atan(accX / sqrt(pow(accY, 2) + pow(accZ, 2))) * radToDegreeConvert; accAngleX = atan(accY / sqrt(pow(accX, 2) + pow(accZ, 2))) * radToDegreeConvert; //apply offsets to accr values accAngleX = accAngleX - accrOffsetValX; accAngleY = accAngleY - accrOffsetValY; //double dt = 0.004 = 1/250 //The Mighty Complementary filter compAngleX = 0.98 * (compAngleX + rotX * 0.004) + 0.02 * accAngleX; compAngleY = 0.98 * (compAngleY + rotY * 0.004) + 0.02 * accAngleY; // assigning pitch and roll pitchAngle = compAngleX; rollAngle = compAngleY; // //if imu yawed convert roll to pitch //0.004 = 1/250 //copied from ymfc-al code pitchAngle = pitchAngle - rollAngle * sin(rotZ * 0.004 * ((3.142 * PI) / 180)); rollAngle = rollAngle + pitchAngle * sin(rotZ * 0.004 * ((3.142 * PI) / 180)); }//end of calculateAngles Fcn //get register values from mpu6050 and calculate angles based on complementary filter void updateAnglesFromMPU() { readMPU(); calculateRotationRates(); calculateAngles(); }//end of updateAnglesFromMPU Fcn void configureMPU() { //Power Register Wire.beginTransmission(MPU6050_ADDR); Wire.write(PWR_REG_ADDR);//Access the power register Wire.write(0b00000000);//check datasheet Wire.endTransmission(); //Gyro Config Wire.beginTransmission(MPU6050_ADDR); Wire.write(ACCR_CONFIG_REG_ADDR); Wire.write(ACCR_CONFIG_REG_VALUE);//check data sheet for more info Wire.endTransmission(); //Accr Config Wire.beginTransmission(MPU6050_ADDR); Wire.write(GYRO_CONFIG_REG_ADDR); Wire.write(GYRO_CONFIG_REG_VALUE);//check datasheet for more info Wire.endTransmission(); }//end of setUpMPU Fcn void computePID() { //input updated in updateAngles Fcn //update SetPoint according to throttle setPointUpdate(); //calculate errors pitchError = pidPitchRateIn - pidPitchRateSetPoint; rollError = pidRollRateIn - pidRollRateSetPoint; yawError = pidYawRateIn - pidYawRateSetPoint; //calculate sum of errors pitchErrorSum = pitchError + pitchPrevError; rollErrorSum = rollError + rollPrevError; yawErrorSum = yawError + yawPrevError; //calculate delta of errors pitchErrorDelta = pitchError - pitchPrevError; rollErrorDelta = rollError - rollPrevError; yawErrorDelta = yawError - yawPrevError; //save current error as prev error for next iteration pitchPrevError = pitchError; rollPrevError = rollError; yawPrevError = yawError; //PID Calculation pidPitchRateOut = (pitchError * pitchRatePGain) + (pitchErrorSum * pitchRateIGain) + (pitchErrorDelta * pitchRateDGain); pidRollRateOut = (rollError * rollRatePGain) + (rollErrorSum * rollRateIGain) + (rollErrorDelta * rollRateDGain); pidYawRateOut = (yawError * yawRatePGain) + (yawErrorSum * yawRateIGain) + (yawErrorDelta * yawRateDGain); if (pidPitchRateOut > pidMax) pidPitchRateOut = pidMax; else if (pidPitchRateOut < -pidMax) pidPitchRateOut = pidMax * -1; if (pidRollRateOut > pidMax) pidRollRateOut = pidMax; else if (pidRollRateOut < -pidMax) pidRollRateOut = pidMax * -1; if (pidYawRateOut > pidMax) pidYawRateOut = pidMax; else if (pidYawRateOut < -pidMax ) pidYawRateOut = pidMax * -1; }//end of computePID Fcn void resetPID() { }//end of resetPID Fcn void sendBTOutput() { if (millis() - btDataStartMillis > DATA_INTERVAL) { btDataStartMillis = millis(); Serial.print(F("y")); Serial.print(pidYawRateIn); Serial.print(F(">")); Serial.print(F("p")); Serial.print(pitchAngle); Serial.print(F(">")); Serial.print(F("r")); Serial.print(rollAngle); Serial.print(F(">")); Serial.print(F("ps")); Serial.print(pidPitchRateSetPoint); Serial.print(F(">")); Serial.print(F("rs")); Serial.print(pidRollRateSetPoint); Serial.print(F(">")); Serial.print(F("ys")); Serial.print(pidYawRateSetPoint); Serial.print(F(">")); Serial.print(F("po")); Serial.print(pidPitchRateOut); Serial.print(F(">")); Serial.print(F("ro")); Serial.print(pidRollRateOut); Serial.print(F(">")); Serial.print(F("yo")); Serial.print(pidYawRateOut); Serial.print(F(">")); Serial.print(F("m0-")); Serial.print(m0Value); Serial.print(F(">")); Serial.print(F("m1-")); Serial.print(m1Value); Serial.print(F(">")); Serial.print(F("m2-")); Serial.print(m2Value); Serial.print(F(">")); Serial.print(F("m3-")); Serial.print(m3Value); Serial.print(F(">")); Serial.print(F("c0")); Serial.print(pwmDuration[0]); Serial.print(F(">")); Serial.print(F("c1")); Serial.print(pwmDuration[1]); Serial.print(F(">")); Serial.print(F("c2")); Serial.print(pwmDuration[2]); Serial.print(F(">")); Serial.print(F("c3")); Serial.print(pwmDuration[3]); Serial.print(F(">")); Serial.println(""); }//end of IF }//end of sendBTOutput Fcn void setup() { pinMode(13, OUTPUT); digitalWrite(13, LOW); //used to indicate FC is ready initReceiver(); #ifdef DEBUG Serial.begin(115200); //HC-05 is using hardware serial, 38400 is HC-05 default baud rate btDataStartMillis = millis(); Serial.println("DEBUG mode ON"); #endif initMPU6050(); initPID(); initializeMotors(); //prevTime = millis(); //used to calculate dt for gyro integration loopTimer = micros();//used to keep track of loop time digitalWrite(13, HIGH); //FC is Ready }//end of setup Fcn void loop() { updateAnglesFromMPU(); if (armMotors == true) { computePID(); updateMotors(); }//end of armMotors #ifdef DEBUG sendBTOutput(); #endif }//end of loop Fcn Title: Re: Anything wrong with my code ? Post by: anilkunchalaece on July 14, 2018, 03:08:02 PM Update :
Roll and Roll Rate directions are inverted. For Angle Measurement : For Angular Rate Measurement Roll Right Down --> -ve Right Down --> + ve Left Down --> +ve Left Down --> - ve Pitch Nose Up --> +ve Nose Up --> + ve Node Down --> -ve Nose Down --> -ve are these directions are ok ? Or DId I mess up something in the code ? Title: Re: Anything wrong with my code ? Post by: anilkunchalaece on July 18, 2018, 11:09:39 AM Hi,
Roll rotation is the problem. Changing that seems to solve it. |