Rev 2118 | Rev 2122 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2118 | Rev 2119 | ||
|---|---|---|---|
| Line 43... | Line 43... | ||
| 43 | target[YAW] = attitude[YAW]; |
43 | target[YAW] = attitude[YAW]; |
| 44 | } |
44 | } |
| Line 45... | Line 45... | ||
| 45 | 45 | ||
| 46 | void flight_updateFlightParametersToFlightMode(void) { |
46 | void flight_updateFlightParametersToFlightMode(void) { |
| 47 | debugOut.analog[16] = currentFlightMode; |
47 | debugOut.analog[16] = currentFlightMode; |
| 48 | reverse[PITCH] = staticParams.controlServosReverse & CONTROL_SERVO_REVERSE_ELEVATOR; |
48 | reverse[PITCH] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_ELEVATOR; |
| 49 | reverse[ROLL] = staticParams.controlServosReverse & CONTROL_SERVO_REVERSE_AILERONS; |
49 | reverse[ROLL] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_AILERONS; |
| Line 50... | Line 50... | ||
| 50 | reverse[YAW] = staticParams.controlServosReverse & CONTROL_SERVO_REVERSE_RUDDER; |
50 | reverse[YAW] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_RUDDER; |
| 51 | 51 | ||
| 52 | // At a switch to angles, we want to kill errors first. |
52 | // At a switch to angles, we want to kill errors first. |
| 53 | // This should be triggered only once per mode change! |
53 | // This should be triggered only once per mode change! |
| Line 62... | Line 62... | ||
| 62 | } |
62 | } |
| 63 | } |
63 | } |
| Line 64... | Line 64... | ||
| 64 | 64 | ||
| 65 | // Normal at airspeed = 10. |
65 | // Normal at airspeed = 10. |
| 66 | uint8_t calcAirspeedPID(uint8_t pid) { |
66 | uint8_t calcAirspeedPID(uint8_t pid) { |
| 67 | if (staticParams.bitConfig & CFG_USE_AIRSPEED_PID) { |
67 | if (!(staticParams.bitConfig & CFG_USE_AIRSPEED_PID)) { |
| 68 | return pid; |
68 | return pid; |
| Line 69... | Line 69... | ||
| 69 | } |
69 | } |
| Line 83... | Line 83... | ||
| 83 | airspeedPID[axis].I = calcAirspeedPID(dynamicParams.gyroPID[axis].I); // Should this be??? |
83 | airspeedPID[axis].I = calcAirspeedPID(dynamicParams.gyroPID[axis].I); // Should this be??? |
| 84 | airspeedPID[axis].D = dynamicParams.gyroPID[axis].D; |
84 | airspeedPID[axis].D = dynamicParams.gyroPID[axis].D; |
| 85 | } |
85 | } |
| 86 | } |
86 | } |
| Line -... | Line 87... | ||
| - | 87 | ||
| - | 88 | #define LOG_STICK_SCALE 8 |
|
| - | 89 | #define LOG_P_SCALE 6 |
|
| - | 90 | #define LOG_I_SCALE 10 |
|
| - | 91 | #define LOG_D_SCALE 6 |
|
| 87 | 92 | ||
| 88 | /************************************************************************/ |
93 | /************************************************************************/ |
| 89 | /* Main Flight Control */ |
94 | /* Main Flight Control */ |
| 90 | /************************************************************************/ |
95 | /************************************************************************/ |
| 91 | void flight_control(void) { |
96 | void flight_control(void) { |
| Line 105... | Line 110... | ||
| 105 | setAirspeedPIDs(); |
110 | setAirspeedPIDs(); |
| Line 106... | Line 111... | ||
| 106 | 111 | ||
| Line 107... | Line 112... | ||
| 107 | term[CONTROL_THROTTLE] = controls[CONTROL_THROTTLE]; |
112 | term[CONTROL_THROTTLE] = controls[CONTROL_THROTTLE]; |
| 108 | 113 | ||
| 109 | // These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway. |
114 | // These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway. |
| 110 | target[PITCH] += ((int32_t)controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> 7; |
115 | target[PITCH] += ((int32_t)controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> LOG_STICK_SCALE; |
| Line 111... | Line 116... | ||
| 111 | target[ROLL] += ((int32_t)controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> 7; |
116 | target[ROLL] += ((int32_t)controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> LOG_STICK_SCALE; |
| 112 | target[YAW] += ((int32_t)controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> 7; |
117 | target[YAW] += ((int32_t)controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> LOG_STICK_SCALE; |
| 113 | 118 | ||
| 114 | for (axis = PITCH; axis <= YAW; axis++) { |
119 | for (axis = PITCH; axis <= YAW; axis++) { |
| 115 | if (target[axis] > OVER180) { |
120 | if (target[axis] > OVER180) { |
| 116 | target[axis] -= OVER360; |
121 | target[axis] -= OVER360; |
| Line 117... | Line 122... | ||
| 117 | } else if (target[axis] <= -OVER180) { |
122 | } else if (target[axis] <= -OVER180) { |
| 118 | target[axis] += OVER360; |
123 | target[axis] += OVER360; |
| 119 | } |
124 | } |
| 120 | 125 | ||
| Line 121... | Line 126... | ||
| 121 | if (reverse[axis]) |
126 | if (reverse[axis]) |
| 122 | error[axis] = attitude[axis] + target[axis]; |
127 | error[axis] = target[axis] - attitude[axis]; |
| 123 | else |
128 | else |
| 124 | error[axis] = attitude[axis] - target[axis]; |
129 | error[axis] = attitude[axis] - target[axis]; |
| 125 | 130 | ||
| 126 | if (error[axis] > maxError[axis]) { |
131 | if (error[axis] > maxError[axis]) { |
| 127 | error[axis] = maxError[axis]; |
132 | error[axis] = maxError[axis]; |
| Line 128... | Line -... | ||
| 128 | } else if (error[axis] < -maxError[axis]) { |
- | |
| 129 | error[axis] =- maxError[axis]; |
- | |
| 130 | } else { |
- | |
| 131 | // update I parts here for angles mode. Ĩ parts in rate mode is something different. |
- | |
| 132 | } |
133 | } else if (error[axis] < -maxError[axis]) { |
| 133 | 134 | error[axis] =- maxError[axis]; |
|
| 134 | #define LOG_P_SCALE 6 |
135 | } else { |
| 135 | #define LOG_I_SCALE 6 |
136 | // update I parts here for angles mode. Ĩ parts in rate mode is something different. |
| 136 | #define LOG_D_SCALE 4 |
137 | } |
| 137 | - | ||
| 138 | /************************************************************************/ |
- | |
| 139 | /* Calculate control feedback from angle (gyro integral) */ |
138 | |
| 140 | /* and angular velocity (gyro signal) */ |
139 | /************************************************************************/ |
| 141 | /************************************************************************/ |
- | |
| 142 | if (currentFlightMode == FLIGHT_MODE_ANGLES || currentFlightMode |
140 | /* Calculate control feedback from angle (gyro integral) */ |
| 143 | == FLIGHT_MODE_RATE) { |
141 | /* and angular velocity (gyro signal) */ |
| 144 | PDPart[axis] = (((int32_t) gyro_PID[axis] |
142 | /************************************************************************/ |
| 145 | * (int16_t) airspeedPID[axis].P) >> LOG_P_SCALE) |
143 | if (currentFlightMode == FLIGHT_MODE_ANGLES || currentFlightMode == FLIGHT_MODE_RATE) { |
| 146 | + ((gyroD[axis] * (int16_t) airspeedPID[axis].D) |
144 | PDPart[axis] = (((int32_t) gyro_PID[axis] * (int32_t) airspeedPID[axis].P) >> LOG_P_SCALE) |
| Line 147... | Line 145... | ||
| 147 | >> LOG_D_SCALE); |
145 | + (((int16_t)gyroD[axis] * (int16_t) airspeedPID[axis].D) >> LOG_D_SCALE); |
| 148 | if (reverse[axis]) |
- | |
| 149 | PDPart[axis] = -PDPart[axis]; |
146 | if (reverse[axis]) |
| 150 | } else { |
- | |
| 151 | PDPart[axis] = 0; |
147 | PDPart[axis] = -PDPart[axis]; |
| 152 | } |
148 | } else { |
| 153 | 149 | PDPart[axis] = 0; |
|
| 154 | if (currentFlightMode == FLIGHT_MODE_ANGLES) { |
150 | } |
| 155 | int16_t anglePart = (int32_t)( |
151 | |
| Line 156... | Line 152... | ||
| 156 | error[axis] * (int32_t) airspeedPID[axis].I) |
152 | if (currentFlightMode == FLIGHT_MODE_ANGLES) { |
| 157 | >> LOG_I_SCALE; |
153 | int16_t anglePart = (int32_t)(error[axis] * (int32_t) airspeedPID[axis].I) >> LOG_I_SCALE; |
| 158 | if (reverse[axis]) |
154 | if (reverse[axis]) |
| Line 159... | Line 155... | ||
| 159 | PDPart[axis] += anglePart; |
155 | PDPart[axis] += anglePart; |
| 160 | else |
156 | else |
| 161 | PDPart[axis] -= anglePart; |
157 | PDPart[axis] -= anglePart; |
| 162 | } |
158 | } |
| Line 163... | Line 159... | ||
| 163 | 159 | ||
| 164 | // Add I parts here... these are integrated errors. |
160 | // Add I parts here... these are integrated errors. |
| 165 | term[axis] = controls[axis] + PDPart[axis] + IPart[axis]; |
161 | term[axis] = controls[axis] + PDPart[axis]; // + IPart[axis]; |
| 166 | } |
162 | } |
| 167 | 163 | ||
| 168 | debugOut.analog[12] = term[PITCH]; |
164 | debugOut.analog[12] = term[PITCH]; |
| 169 | debugOut.analog[13] = term[ROLL]; |
165 | debugOut.analog[13] = term[ROLL]; |
| 170 | debugOut.analog[14] = term[YAW]; |
166 | debugOut.analog[14] = term[YAW]; |
| 171 | debugOut.analog[15] = term[THROTTLE]; |
167 | debugOut.analog[15] = term[THROTTLE]; |