Rev 2099 | Rev 2103 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 1910 | - | 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 | |||
| 10 | #include "timer2.h" |
||
| 11 | #include "attitude.h" |
||
| 12 | #include "controlMixer.h" |
||
| 13 | |||
| 14 | #define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
||
| 15 | |||
| 16 | /* |
||
| 2099 | - | 17 | * target-directions integrals. |
| 1910 | - | 18 | */ |
| 2099 | - | 19 | int32_t target[3]; |
| 1910 | - | 20 | |
| 2099 | - | 21 | /* |
| 22 | * Error integrals. |
||
| 23 | */ |
||
| 24 | int32_t error[3]; |
||
| 1910 | - | 25 | |
| 2099 | - | 26 | uint8_t pFactor[3]; |
| 27 | uint8_t dFactor[3]; |
||
| 28 | uint8_t iFactor[3]; |
||
| 29 | uint8_t reverse[3]; |
||
| 30 | int32_t IPart[3] = { 0, 0, 0 }; |
||
| 1910 | - | 31 | |
| 2102 | - | 32 | int16_t controlServos[MAX_CONTROL_SERVOS]; |
| 1910 | - | 33 | |
| 34 | /************************************************************************/ |
||
| 35 | /* Neutral Readings */ |
||
| 36 | /************************************************************************/ |
||
| 37 | #define CONTROL_CONFIG_SCALE 10 |
||
| 38 | |||
| 2099 | - | 39 | void flight_setGround(void) { |
| 2102 | - | 40 | IPart[PITCH] = IPart[ROLL] = IPart[YAW] = 0; |
| 41 | target[PITCH] = attitude[PITCH]; |
||
| 42 | target[ROLL] = attitude[ROLL]; |
||
| 43 | target[YAW] = attitude[YAW]; |
||
| 1910 | - | 44 | } |
| 45 | |||
| 2102 | - | 46 | // this should be followed by a call to switchToFlightMode!! |
| 47 | void updateFlightParametersToFlightMode(uint8_t flightMode) { |
||
| 48 | debugOut.analog[15] = flightMode; |
||
| 1910 | - | 49 | |
| 2102 | - | 50 | reverse[CONTROL_ELEVATOR] = staticParams.controlServosReverse |
| 51 | & CONTROL_SERVO_REVERSE_ELEVATOR; |
||
| 52 | reverse[CONTROL_AILERONS] = staticParams.controlServosReverse |
||
| 53 | & CONTROL_SERVO_REVERSE_AILERONS; |
||
| 54 | reverse[CONTROL_RUDDER] = staticParams.controlServosReverse |
||
| 55 | & CONTROL_SERVO_REVERSE_RUDDER; |
||
| 1910 | - | 56 | |
| 2102 | - | 57 | for (uint8_t i = 0; i < 3; i++) { |
| 58 | if (flightMode == FLIGHT_MODE_MANUAL) { |
||
| 59 | pFactor[i] = 0; |
||
| 60 | dFactor[i] = 0; |
||
| 61 | } else if(flightMode == FLIGHT_MODE_RATE || flightMode == FLIGHT_MODE_ANGLES) { |
||
| 62 | pFactor[i] = staticParams.gyroPID[i].P; |
||
| 63 | dFactor[i] = staticParams.gyroPID[i].D; |
||
| 64 | } |
||
| 65 | |||
| 66 | if (flightMode == FLIGHT_MODE_ANGLES) { |
||
| 67 | iFactor[i] = staticParams.gyroPID[i].I; |
||
| 68 | } else if(flightMode == FLIGHT_MODE_RATE || flightMode == FLIGHT_MODE_MANUAL) { |
||
| 69 | iFactor[i] = 0; |
||
| 70 | } |
||
| 71 | } |
||
| 1910 | - | 72 | } |
| 73 | |||
| 74 | /************************************************************************/ |
||
| 75 | /* Main Flight Control */ |
||
| 76 | /************************************************************************/ |
||
| 77 | void flight_control(void) { |
||
| 2102 | - | 78 | // Mixer Fractions that are combined for Motor Control |
| 79 | int16_t throttleTerm, term[3]; |
||
| 2099 | - | 80 | |
| 2102 | - | 81 | // PID controller variables |
| 82 | int16_t PDPart[3]; |
||
| 1910 | - | 83 | |
| 2102 | - | 84 | static int8_t debugDataTimer = 1; |
| 1910 | - | 85 | |
| 2102 | - | 86 | // High resolution motor values for smoothing of PID motor outputs |
| 87 | // static int16_t outputFilters[MAX_OUTPUTS]; |
||
| 1910 | - | 88 | |
| 2102 | - | 89 | uint8_t axis; |
| 1910 | - | 90 | |
| 2102 | - | 91 | // TODO: Check modern version. |
| 92 | // calculateFlightAttitude(); |
||
| 93 | // TODO: Check modern version. |
||
| 94 | // controlMixer_update(); |
||
| 95 | throttleTerm = controls[CONTROL_THROTTLE]; |
||
| 1910 | - | 96 | |
| 2102 | - | 97 | // These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway. |
| 98 | target[PITCH] += controls[CONTROL_ELEVATOR] * staticParams.stickIElevator; |
||
| 99 | target[ROLL] += controls[CONTROL_AILERONS] * staticParams.stickIAilerons; |
||
| 100 | target[YAW] += controls[CONTROL_RUDDER] * staticParams.stickIRudder; |
||
| 1910 | - | 101 | |
| 2102 | - | 102 | for (axis = PITCH; axis <= YAW; axis++) { |
| 103 | if (target[axis] > OVER180) { |
||
| 104 | target[axis] -= OVER360; |
||
| 105 | } else if (attitude[axis] <= -OVER180) { |
||
| 106 | attitude[axis] += OVER360; |
||
| 107 | } |
||
| 1910 | - | 108 | |
| 2102 | - | 109 | if (reverse[axis]) |
| 110 | error[axis] = attitude[axis] + target[axis]; |
||
| 111 | else |
||
| 112 | error[axis] = attitude[axis] - target[axis]; |
||
| 113 | if (error[axis] > OVER180) { |
||
| 114 | error[axis] -= OVER360; |
||
| 115 | } else if (error[axis] <= -OVER180) { |
||
| 116 | error[axis] += OVER360; |
||
| 117 | } |
||
| 1910 | - | 118 | |
| 2102 | - | 119 | /************************************************************************/ |
| 120 | /* Calculate control feedback from angle (gyro integral) */ |
||
| 121 | /* and angular velocity (gyro signal) */ |
||
| 122 | /************************************************************************/ |
||
| 123 | PDPart[axis] = (((int32_t) rate_PID[axis] * pFactor[axis]) >> 6) |
||
| 124 | + ((differential[axis] * (int16_t) dFactor[axis]) >> 4); |
||
| 125 | if (reverse[axis]) |
||
| 126 | PDPart[axis] = -PDPart[axis]; |
||
| 1910 | - | 127 | |
| 2102 | - | 128 | int16_t anglePart = (error[axis] * iFactor[axis]) >> 10; |
| 129 | if (reverse[axis]) |
||
| 130 | PDPart[axis] += anglePart; |
||
| 131 | else |
||
| 132 | PDPart[axis] -= anglePart; |
||
| 1910 | - | 133 | |
| 2102 | - | 134 | // Add I parts here... these are integrated errors. |
| 135 | // When an error wraps, actually its I part should be negated or something... |
||
| 1910 | - | 136 | |
| 2102 | - | 137 | term[axis] = controls[axis] + PDPart[axis] + IPart[axis]; |
| 138 | } |
||
| 1910 | - | 139 | |
| 2102 | - | 140 | debugOut.analog[12] = term[PITCH]; |
| 141 | debugOut.analog[13] = term[ROLL]; |
||
| 142 | debugOut.analog[14] = throttleTerm; |
||
| 143 | debugOut.analog[15] = term[YAW]; |
||
| 2099 | - | 144 | |
| 2102 | - | 145 | for (uint8_t i = 0; i < MAX_CONTROL_SERVOS; i++) { |
| 146 | int16_t tmp; |
||
| 147 | if (servoTestActive) { |
||
| 148 | controlServos[i] = ((int16_t) servoTest[i] - 128) * 4; |
||
| 149 | } else { |
||
| 150 | // Follow the normal order of servos: Ailerons, elevator, throttle, rudder. |
||
| 151 | switch (i) { |
||
| 152 | case 0: |
||
| 153 | tmp = term[ROLL]; |
||
| 154 | break; |
||
| 155 | case 1: |
||
| 156 | tmp = term[PITCH]; |
||
| 157 | break; |
||
| 158 | case 2: |
||
| 159 | tmp = throttleTerm; |
||
| 160 | break; |
||
| 161 | case 3: |
||
| 162 | tmp = term[YAW]; |
||
| 163 | break; |
||
| 164 | default: |
||
| 165 | tmp = 0; |
||
| 166 | } |
||
| 167 | // These are all signed and in the same units as the RC stuff in rc.c. |
||
| 168 | controlServos[i] = tmp; |
||
| 169 | } |
||
| 170 | } |
||
| 1910 | - | 171 | |
| 2102 | - | 172 | calculateControlServoValues; |
| 1910 | - | 173 | |
| 2102 | - | 174 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 175 | // Debugging |
||
| 176 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
||
| 177 | if (!(--debugDataTimer)) { |
||
| 178 | debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz. |
||
| 179 | debugOut.analog[0] = rate_PID[PITCH]; // in 0.1 deg |
||
| 180 | debugOut.analog[1] = rate_PID[ROLL]; // in 0.1 deg |
||
| 181 | debugOut.analog[2] = rate_PID[YAW]; |
||
| 1910 | - | 182 | |
| 2102 | - | 183 | debugOut.analog[3] = attitude[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
| 184 | debugOut.analog[4] = attitude[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
||
| 185 | debugOut.analog[5] = attitude[YAW] / (GYRO_DEG_FACTOR / 10); |
||
| 2099 | - | 186 | |
| 2102 | - | 187 | debugOut.analog[6] = target[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
| 188 | debugOut.analog[7] = target[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
||
| 189 | debugOut.analog[8] = target[YAW] / (GYRO_DEG_FACTOR / 10); |
||
| 190 | |||
| 191 | debugOut.analog[9] = error[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
||
| 192 | debugOut.analog[10] = error[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
||
| 193 | debugOut.analog[11] = error[YAW] / (GYRO_DEG_FACTOR / 10); |
||
| 194 | |||
| 195 | debugOut.analog[12] = term[PITCH]; |
||
| 196 | debugOut.analog[13] = term[ROLL]; |
||
| 197 | debugOut.analog[14] = term[YAW]; |
||
| 198 | |||
| 199 | //DebugOut.Analog[18] = (10 * controlIntegrals[CONTROL_ELEVATOR]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
||
| 200 | //DebugOut.Analog[19] = (10 * controlIntegrals[CONTROL_AILERONS]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
||
| 201 | //debugOut.analog[22] = (10 * IPart[PITCH]) / GYRO_DEG_FACTOR; // in 0.1 deg |
||
| 202 | //debugOut.analog[23] = (10 * IPart[ROLL]) / GYRO_DEG_FACTOR; // in 0.1 deg |
||
| 203 | } |
||
| 1910 | - | 204 | } |