Rev 2138 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 2108 | - | 1 | #include <stdlib.h> |
| 2 | #include <avr/io.h> |
||
| 3 | #include "eeprom.h" |
||
| 4 | #include "flight.h" |
||
| 5 | #include "output.h" |
||
| 6 | |||
| 7 | // Necessary for external control and motor test |
||
| 8 | #include "uart0.h" |
||
| 9 | #include "timer2.h" |
||
| 10 | #include "analog.h" |
||
| 11 | #include "attitude.h" |
||
| 12 | #include "controlMixer.h" |
||
| 13 | #include "configuration.h" |
||
| 14 | |||
| 15 | #define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
||
| 16 | |||
| 17 | /* |
||
| 18 | * target-directions integrals. |
||
| 19 | */ |
||
| 20 | int32_t target[3]; |
||
| 21 | |||
| 22 | /* |
||
| 23 | * Error integrals. |
||
| 24 | */ |
||
| 25 | |||
| 26 | int32_t maxError[3]; |
||
| 27 | int32_t IPart[3] = { 0, 0, 0 }; |
||
| 28 | PID_t airspeedPID[3]; |
||
| 29 | |||
| 30 | int16_t controlServos[NUM_CONTROL_SERVOS]; |
||
| 31 | |||
| 32 | /************************************************************************/ |
||
| 33 | /* Neutral Readings */ |
||
| 34 | /************************************************************************/ |
||
| 35 | #define CONTROL_CONFIG_SCALE 10 |
||
| 36 | |||
| 37 | void flight_setGround(void) { |
||
| 38 | IPart[PITCH] = IPart[ROLL] = IPart[YAW] = 0; |
||
| 39 | target[PITCH] = attitude[PITCH]; |
||
| 40 | target[ROLL] = attitude[ROLL]; |
||
| 41 | target[YAW] = attitude[YAW]; |
||
| 42 | } |
||
| 43 | |||
| 44 | void flight_updateFlightParametersToFlightMode(void) { |
||
| 45 | debugOut.analog[16] = currentFlightMode; |
||
| 2135 | - | 46 | |
| 2108 | - | 47 | // At a switch to angles, we want to kill errors first. |
| 48 | // This should be triggered only once per mode change! |
||
| 49 | if (currentFlightMode == FLIGHT_MODE_ANGLES) { |
||
| 50 | target[PITCH] = attitude[PITCH]; |
||
| 51 | target[ROLL] = attitude[ROLL]; |
||
| 52 | target[YAW] = attitude[YAW]; |
||
| 53 | } |
||
| 54 | |||
| 55 | for (uint8_t axis=0; axis<3; axis++) { |
||
| 56 | maxError[axis] = (int32_t)staticParams.gyroPID[axis].iMax * GYRO_DEG_FACTOR; |
||
| 57 | } |
||
| 58 | } |
||
| 59 | |||
| 60 | // Normal at airspeed = 10. |
||
| 61 | uint8_t calcAirspeedPID(uint8_t pid) { |
||
| 2132 | - | 62 | //if (!(staticParams.bitConfig & CFG_USE_AIRSPEED_PID)) { |
| 2108 | - | 63 | return pid; |
| 2132 | - | 64 | //} |
| 65 | } |
||
| 2108 | - | 66 | |
| 67 | void setAirspeedPIDs(void) { |
||
| 68 | for (uint8_t axis = 0; axis<3; axis++) { |
||
| 69 | airspeedPID[axis].P = calcAirspeedPID(dynamicParams.gyroPID[axis].P); |
||
| 70 | airspeedPID[axis].I = calcAirspeedPID(dynamicParams.gyroPID[axis].I); // Should this be??? |
||
| 71 | airspeedPID[axis].D = dynamicParams.gyroPID[axis].D; |
||
| 72 | } |
||
| 73 | } |
||
| 74 | |||
| 75 | /************************************************************************/ |
||
| 76 | /* Main Flight Control */ |
||
| 77 | /************************************************************************/ |
||
| 78 | void flight_control(void) { |
||
| 79 | // Mixer Fractions that are combined for Motor Control |
||
| 80 | int16_t term[4]; |
||
| 81 | |||
| 82 | // PID controller variables |
||
| 83 | int16_t PDPart[3]; |
||
| 84 | |||
| 85 | static int8_t debugDataTimer = 1; |
||
| 86 | |||
| 87 | // High resolution motor values for smoothing of PID motor outputs |
||
| 88 | // static int16_t outputFilters[MAX_OUTPUTS]; |
||
| 89 | |||
| 90 | uint8_t axis; |
||
| 91 | |||
| 92 | setAirspeedPIDs(); |
||
| 93 | |||
| 94 | term[CONTROL_THROTTLE] = controls[CONTROL_THROTTLE]; |
||
| 95 | |||
| 96 | // These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway. |
||
| 2132 | - | 97 | int32_t tmp; |
| 2108 | - | 98 | |
| 2132 | - | 99 | tmp = ((int32_t)controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> LOG_STICK_SCALE; |
| 2139 | - | 100 | if (staticParams.servos[PITCH].reverse) target[PITCH] += tmp; else target[PITCH] -= tmp; |
| 2132 | - | 101 | |
| 102 | tmp = ((int32_t)controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> LOG_STICK_SCALE; |
||
| 2139 | - | 103 | if (staticParams.servos[ROLL].reverse) target[ROLL] += tmp; else target[ROLL] -= tmp; |
| 2132 | - | 104 | |
| 105 | tmp = ((int32_t)controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> LOG_STICK_SCALE; |
||
| 2139 | - | 106 | if (staticParams.servos[YAW].reverse) target[YAW] += tmp; else target[YAW] -= tmp; |
| 2132 | - | 107 | |
| 2108 | - | 108 | for (axis = PITCH; axis <= YAW; axis++) { |
| 109 | if (target[axis] > OVER180) { |
||
| 110 | target[axis] -= OVER360; |
||
| 111 | } else if (target[axis] <= -OVER180) { |
||
| 112 | target[axis] += OVER360; |
||
| 113 | } |
||
| 114 | |||
| 2135 | - | 115 | int32_t error = attitude[axis] - target[axis]; |
| 2108 | - | 116 | |
| 2132 | - | 117 | #define ROTATETARGET 1 |
| 118 | // #define TRUNCATEERROR 1 |
||
| 119 | |||
| 120 | #ifdef ROTATETARGET |
||
| 2135 | - | 121 | //if(abs(error) > OVER180) { // doesnt work!!! |
| 122 | if(error > OVER180 || error < -OVER180) { |
||
| 2132 | - | 123 | // The shortest way from attitude to target crosses -180. |
| 124 | // Well there are 2 possibilities: A is >0 and T is < 0, that makes E a (too) large positive number. It should be wrapped to negative. |
||
| 125 | // Or A is <0 and T is >0, that makes E a (too) large negative number. It should be wrapped to positive. |
||
| 2135 | - | 126 | if (error > 0) { |
| 127 | if (error < OVER360 - maxError[axis]) { |
||
| 2132 | - | 128 | // too much err. |
| 2135 | - | 129 | error = -maxError[axis]; |
| 2132 | - | 130 | target[axis] = attitude[axis] + maxError[axis]; |
| 131 | if (target[axis] > OVER180) target[axis] -= OVER360; |
||
| 132 | } else { |
||
| 133 | // Normal case, we just need to correct for the wrap. Error will be negative. |
||
| 2135 | - | 134 | error -= OVER360; |
| 2132 | - | 135 | } |
| 136 | } else { |
||
| 2135 | - | 137 | if (error > maxError[axis] - OVER360) { |
| 2132 | - | 138 | // too much err. |
| 2135 | - | 139 | error = maxError[axis]; |
| 2132 | - | 140 | target[axis] = attitude[axis] - maxError[axis]; |
| 141 | if (target[axis] < -OVER180) target[axis] += OVER360; |
||
| 142 | } else { |
||
| 143 | // Normal case, we just need to correct for the wrap. Error will be negative. |
||
| 2135 | - | 144 | error += OVER360; |
| 2132 | - | 145 | } |
| 146 | } |
||
| 147 | } else { |
||
| 148 | // Simple case, linear range. |
||
| 2135 | - | 149 | if (error > maxError[axis]) { |
| 150 | error = maxError[axis]; |
||
| 2132 | - | 151 | target[axis] = attitude[axis] - maxError[axis]; |
| 2135 | - | 152 | } else if (error < -maxError[axis]) { |
| 153 | error = -maxError[axis]; |
||
| 2132 | - | 154 | target[axis] = attitude[axis] + maxError[axis]; |
| 155 | } |
||
| 156 | } |
||
| 157 | #endif |
||
| 158 | #ifdef TUNCATEERROR |
||
| 2135 | - | 159 | if (error > maxError[axis]) { |
| 160 | error = maxError[axis]; |
||
| 161 | } else if (error < -maxError[axis]) { |
||
| 162 | error = -maxError[axis]; |
||
| 2132 | - | 163 | } else { |
| 164 | // update I parts here for angles mode. I parts in rate mode is something different. |
||
| 2108 | - | 165 | } |
| 2132 | - | 166 | #endif |
| 2108 | - | 167 | |
| 2135 | - | 168 | debugOut.analog[9+axis] = error / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
| 169 | |||
| 2108 | - | 170 | /************************************************************************/ |
| 171 | /* Calculate control feedback from angle (gyro integral) */ |
||
| 172 | /* and angular velocity (gyro signal) */ |
||
| 173 | /************************************************************************/ |
||
| 174 | if (currentFlightMode == FLIGHT_MODE_ANGLES || currentFlightMode |
||
| 175 | == FLIGHT_MODE_RATE) { |
||
| 176 | PDPart[axis] = (((int32_t) gyro_PID[axis] |
||
| 177 | * (int16_t) airspeedPID[axis].P) >> LOG_P_SCALE) |
||
| 178 | + ((gyroD[axis] * (int16_t) airspeedPID[axis].D) |
||
| 179 | >> LOG_D_SCALE); |
||
| 180 | } else { |
||
| 181 | PDPart[axis] = 0; |
||
| 182 | } |
||
| 183 | |||
| 184 | if (currentFlightMode == FLIGHT_MODE_ANGLES) { |
||
| 2135 | - | 185 | int16_t anglePart = (int32_t)(error * (int32_t) airspeedPID[axis].I) >> LOG_I_SCALE; |
| 2132 | - | 186 | PDPart[axis] += anglePart; |
| 2108 | - | 187 | } |
| 2132 | - | 188 | |
| 2108 | - | 189 | // Add I parts here... these are integrated errors. |
| 2139 | - | 190 | if (staticParams.servos[axis].reverse) |
| 2132 | - | 191 | term[axis] = controls[axis] - PDPart[axis]; // + IPart[axis]; |
| 192 | else |
||
| 193 | term[axis] = controls[axis] + PDPart[axis]; // + IPart[axis]; |
||
| 2108 | - | 194 | } |
| 195 | |||
| 196 | for (uint8_t i = 0; i < NUM_CONTROL_SERVOS; i++) { |
||
| 197 | int16_t tmp; |
||
| 198 | if (servoTestActive) { |
||
| 2132 | - | 199 | controlServos[i] = ((int16_t) servoTest[i] - 128) * 8; |
| 2108 | - | 200 | } else { |
| 201 | // Follow the normal order of servos: Ailerons, elevator, throttle, rudder. |
||
| 202 | switch (i) { |
||
| 203 | case 0: |
||
| 204 | tmp = term[ROLL]; |
||
| 205 | break; |
||
| 206 | case 1: |
||
| 207 | tmp = term[PITCH]; |
||
| 208 | break; |
||
| 209 | case 2: |
||
| 210 | tmp = term[THROTTLE]; |
||
| 211 | break; |
||
| 212 | case 3: |
||
| 213 | tmp = term[YAW]; |
||
| 214 | break; |
||
| 215 | default: |
||
| 216 | tmp = 0; |
||
| 217 | } |
||
| 218 | // These are all signed and in the same units as the RC stuff in rc.c. |
||
| 219 | controlServos[i] = tmp; |
||
| 220 | } |
||
| 221 | } |
||
| 222 | |||
| 223 | calculateControlServoValues(); |
||
| 224 | |||
| 225 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
| 226 | // Debugging |
||
| 227 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
| 228 | if (!(--debugDataTimer)) { |
||
| 229 | debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz. |
||
| 230 | debugOut.analog[0] = gyro_PID[PITCH]; // in 0.1 deg |
||
| 231 | debugOut.analog[1] = gyro_PID[ROLL]; // in 0.1 deg |
||
| 232 | debugOut.analog[2] = gyro_PID[YAW]; |
||
| 233 | |||
| 234 | debugOut.analog[3] = attitude[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
||
| 235 | debugOut.analog[4] = attitude[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
||
| 236 | debugOut.analog[5] = attitude[YAW] / (GYRO_DEG_FACTOR / 10); |
||
| 237 | |||
| 238 | debugOut.analog[6] = target[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
||
| 239 | debugOut.analog[7] = target[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
||
| 240 | debugOut.analog[8] = target[YAW] / (GYRO_DEG_FACTOR / 10); |
||
| 241 | |||
| 2135 | - | 242 | debugOut.analog[12] = term[PITCH]; |
| 243 | debugOut.analog[13] = term[ROLL]; |
||
| 244 | debugOut.analog[14] = term[YAW]; |
||
| 245 | debugOut.analog[15] = term[THROTTLE]; |
||
| 2108 | - | 246 | |
| 247 | //DebugOut.Analog[18] = (10 * controlIntegrals[CONTROL_ELEVATOR]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
||
| 248 | //DebugOut.Analog[19] = (10 * controlIntegrals[CONTROL_AILERONS]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
||
| 249 | //debugOut.analog[22] = (10 * IPart[PITCH]) / GYRO_DEG_FACTOR; // in 0.1 deg |
||
| 250 | //debugOut.analog[23] = (10 * IPart[ROLL]) / GYRO_DEG_FACTOR; // in 0.1 deg |
||
| 251 | } |
||
| 252 | } |