Rev 2051 | Rev 2053 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2051 | Rev 2052 | ||
---|---|---|---|
Line 52... | Line 52... | ||
52 | #include <stdlib.h> |
52 | #include <stdlib.h> |
53 | #include <avr/io.h> |
53 | #include <avr/io.h> |
54 | #include "eeprom.h" |
54 | #include "eeprom.h" |
55 | #include "flight.h" |
55 | #include "flight.h" |
56 | #include "output.h" |
56 | #include "output.h" |
- | 57 | #include "uart0.h" |
|
Line 57... | Line 58... | ||
57 | 58 | ||
58 | // Necessary for external control and motor test |
- | |
59 | #include "uart0.h" |
59 | // Necessary for external control and motor test |
60 | #include "twimaster.h" |
60 | #include "twimaster.h" |
61 | #include "attitude.h" |
61 | #include "attitude.h" |
62 | #include "controlMixer.h" |
62 | #include "controlMixer.h" |
- | 63 | #include "commands.h" |
|
- | 64 | #include "heightControl.h" |
|
- | 65 | ||
- | 66 | #ifdef USE_MK3MAG |
|
- | 67 | #include "mk3mag.h" |
|
- | 68 | #include "compassControl.h" |
|
Line 63... | Line 69... | ||
63 | #include "commands.h" |
69 | #endif |
Line 64... | Line 70... | ||
64 | 70 | ||
65 | #define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
71 | #define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
Line 145... | Line 151... | ||
145 | // Mixer Fractions that are combined for Motor Control |
151 | // Mixer Fractions that are combined for Motor Control |
146 | int16_t yawTerm, throttleTerm, term[2]; |
152 | int16_t yawTerm, throttleTerm, term[2]; |
Line 147... | Line 153... | ||
147 | 153 | ||
148 | // PID controller variables |
154 | // PID controller variables |
149 | int16_t PDPart[2],/* DPart[2],*/ PDPartYaw /*, DPartYaw */; |
155 | int16_t PDPart[2],/* DPart[2],*/ PDPartYaw /*, DPartYaw */; |
150 | static int32_t IPart[2] = { 0, 0 }; |
156 | static int32_t IPart[2] = {0, 0}; |
151 | static uint16_t emergencyFlightTime; |
157 | static uint16_t emergencyFlightTime; |
Line 152... | Line 158... | ||
152 | static int8_t debugDataTimer = 1; |
158 | static int8_t debugDataTimer = 1; |
153 | 159 | ||
Line 169... | Line 175... | ||
169 | /* This part could be abstracted, as having yet another control input */ |
175 | /* This part could be abstracted, as having yet another control input */ |
170 | /* to the control mixer: An emergency autopilot control. */ |
176 | /* to the control mixer: An emergency autopilot control. */ |
171 | /************************************************************************/ |
177 | /************************************************************************/ |
Line 172... | Line 178... | ||
172 | 178 | ||
173 | if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy |
179 | if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy |
174 | if (controlMixer_didReceiveSignal) beepRCAlarm(); |
180 | if (controlMixer_didReceiveSignal) beepRCAlarm(); // Only make alarm if a control signal was received before the signal loss. |
175 | if (emergencyFlightTime) { |
181 | if (emergencyFlightTime) { |
176 | // continue emergency flight |
182 | // continue emergency flight |
177 | emergencyFlightTime--; |
183 | emergencyFlightTime--; |
178 | if (isFlying > 256) { |
184 | if (isFlying > 256) { |
Line 191... | Line 197... | ||
191 | // signal is acceptable |
197 | // signal is acceptable |
192 | if (controlMixer_getSignalQuality() > SIGNAL_BAD) { |
198 | if (controlMixer_getSignalQuality() > SIGNAL_BAD) { |
193 | // Reset emergency landing control variables. |
199 | // Reset emergency landing control variables. |
194 | MKFlags &= ~(MKFLAG_EMERGENCY_FLIGHT); // clear flag for emergency landing |
200 | MKFlags &= ~(MKFLAG_EMERGENCY_FLIGHT); // clear flag for emergency landing |
195 | // The time is in whole seconds. |
201 | // The time is in whole seconds. |
- | 202 | if (staticParams.emergencyFlightDuration > (65535-F_MAINLOOP)/F_MAINLOOP) |
|
- | 203 | emergencyFlightTime = 0xffff; |
|
- | 204 | else |
|
196 | emergencyFlightTime = (uint16_t) staticParams.emergencyFlightDuration * 488; |
205 | emergencyFlightTime = (uint16_t)staticParams.emergencyFlightDuration * F_MAINLOOP; |
197 | } |
206 | } |
Line 198... | Line 207... | ||
198 | 207 | ||
199 | // If some throttle is given, and the motor-run flag is on, increase the probability that we are flying. |
208 | // If some throttle is given, and the motor-run flag is on, increase the probability that we are flying. |
200 | if (throttleTerm > 40 && (MKFlags & MKFLAG_MOTOR_RUN)) { |
209 | if (throttleTerm > 40 && (MKFlags & MKFLAG_MOTOR_RUN)) { |
Line 209... | Line 218... | ||
209 | */ |
218 | */ |
210 | if (isFlying < 256) { |
219 | if (isFlying < 256) { |
211 | IPart[PITCH] = IPart[ROLL] = 0; |
220 | IPart[PITCH] = IPart[ROLL] = 0; |
212 | PDPartYaw = 0; |
221 | PDPartYaw = 0; |
213 | if (isFlying == 250) { |
222 | if (isFlying == 250) { |
214 | // HC_setGround(); |
223 | HC_setGround(); |
- | 224 | #ifdef USE_MK3MAG |
|
215 | attitude_resetHeadingToMagnetic(); |
225 | attitude_resetHeadingToMagnetic(); |
- | 226 | compass_setTakeoffHeading(heading); |
|
- | 227 | #endif |
|
216 | // Set target heading to the one just gotten off compass. |
228 | // Set target heading to the one just gotten off compass. |
217 | // targetHeading = heading; |
229 | // targetHeading = heading; |
218 | } |
230 | } |
219 | } else { |
231 | } else { |
220 | // Set fly flag. TODO: Hmmm what can we trust - the isFlying counter or the flag? |
232 | // Set fly flag. TODO: Hmmm what can we trust - the isFlying counter or the flag? |
Line 224... | Line 236... | ||
224 | 236 | ||
225 | commands_handleCommands(); |
237 | commands_handleCommands(); |
226 | setNormalFlightParameters(); |
238 | setNormalFlightParameters(); |
Line 227... | Line -... | ||
227 | } // end else (not bad signal case) |
- | |
228 | - | ||
229 | /************************************************************************/ |
- | |
230 | /* Yawing */ |
- | |
231 | /************************************************************************/ |
- | |
232 | /* |
- | |
233 | if (abs(controls[CONTROL_YAW]) > 4 * staticParams.stickYawP) { // yaw stick is activated |
- | |
234 | ignoreCompassTimer = 1000; |
- | |
235 | if (!(staticParams.bitConfig & CFG_COMPASS_FIX)) { |
- | |
236 | //targetHeading = heading; |
- | |
237 | // YGBSM!!! |
- | |
238 | } |
- | |
239 | } |
- | |
240 | */ |
239 | } // end else (not bad signal case) |
241 | 240 | ||
242 | #if defined (USE_NAVICTRL) |
241 | #if defined (USE_NAVICTRL) |
243 | /************************************************************************/ |
242 | /************************************************************************/ |
244 | /* GPS is currently not supported. */ |
243 | /* GPS is currently not supported. */ |
245 | /************************************************************************/ |
244 | /************************************************************************/ |
246 | if(staticParams.GlobalConfig & CFG_GPS_ACTIVE) { |
245 | if(staticParams.GlobalConfig & CFG_GPS_ENABLED) { |
247 | GPS_Main(); |
246 | GPS_Main(); |
248 | MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START); |
247 | MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START); |
249 | } else { |
248 | } else { |
Line 259... | Line 258... | ||
259 | // The P-part is the P of the PID controller. That's the angle integrals (not rates). |
258 | // The P-part is the P of the PID controller. That's the angle integrals (not rates). |
260 | for (axis = PITCH; axis <= ROLL; axis++) { |
259 | for (axis = PITCH; axis <= ROLL; axis++) { |
261 | PDPart[axis] = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2); // P-Part - Proportional to Integral |
260 | PDPart[axis] = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2); // P-Part - Proportional to Integral |
262 | PDPart[axis] += (int32_t)rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 5); |
261 | PDPart[axis] += (int32_t)rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 5); |
263 | PDPart[axis] += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16; |
262 | PDPart[axis] += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16; |
- | 263 | ||
264 | CHECK_MIN_MAX(PDPart[axis], -6L*GYRO_DEG_FACTOR_PITCHROLL, 6L*GYRO_DEG_FACTOR_PITCHROLL); |
264 | //CHECK_MIN_MAX(PDPart[axis], -6L*GYRO_DEG_FACTOR_PITCHROLL, 6L*GYRO_DEG_FACTOR_PITCHROLL); |
- | 265 | if (PDPart[axis] < -6L*GYRO_DEG_FACTOR_PITCHROLL) { |
|
- | 266 | PDPart[axis] =- 6L*GYRO_DEG_FACTOR_PITCHROLL; |
|
- | 267 | debugOut.digital[0] |= DEBUG_FLIGHTCLIP; |
|
- | 268 | } else if (PDPart[axis] > 6L*GYRO_DEG_FACTOR_PITCHROLL) { |
|
- | 269 | PDPart[axis] = 6L*GYRO_DEG_FACTOR_PITCHROLL; |
|
- | 270 | debugOut.digital[0] |= DEBUG_FLIGHTCLIP; |
|
- | 271 | } |
|
265 | } |
272 | } |
Line 266... | Line 273... | ||
266 | 273 | ||
267 | #define YAW_I_LIMIT (45L * GYRO_DEG_FACTOR_YAW) |
274 | #define YAW_I_LIMIT (45L * GYRO_DEG_FACTOR_YAW) |
268 | // This is where control affects the target heading. It also (later) directly controls yaw. |
275 | // This is where control affects the target heading. It also (later) directly controls yaw. |
Line 348... | Line 355... | ||
348 | } |
355 | } |
Line 349... | Line 356... | ||
349 | 356 | ||
350 | tmp_int = (int32_t) ((int32_t) dynamicParams.dynamicStability |
357 | tmp_int = (int32_t) ((int32_t) dynamicParams.dynamicStability |
Line -... | Line 358... | ||
- | 358 | * (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64; |
|
- | 359 | ||
- | 360 | //CHECK_MIN_MAX(IPart[axis], -25L*GYRO_DEG_FACTOR_PITCHROLL, 25L*GYRO_DEG_FACTOR_PITCHROLL); |
|
351 | * (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64; |
361 | if (IPart[axis] < -25L*GYRO_DEG_FACTOR_PITCHROLL) { |
352 | 362 | IPart[axis] =- 25L*GYRO_DEG_FACTOR_PITCHROLL; |
|
- | 363 | debugOut.digital[1] |= DEBUG_FLIGHTCLIP; |
|
- | 364 | } else if (PDPart[axis] > 25L*GYRO_DEG_FACTOR_PITCHROLL) { |
|
- | 365 | PDPart[axis] = 25L*GYRO_DEG_FACTOR_PITCHROLL; |
|
- | 366 | debugOut.digital[1] |= DEBUG_FLIGHTCLIP; |
|
353 | // TODO: From which planet comes the 16000? |
367 | } |
354 | CHECK_MIN_MAX(IPart[axis], -(CONTROL_SCALING * 16000L), (CONTROL_SCALING * 16000L)); |
368 | |
355 | // Add (P, D) parts minus stick pos. to the scaled-down I part. |
369 | // Add (P, D) parts minus stick pos. to the scaled-down I part. |
356 | term[axis] = PDPart[axis] - controls[axis] + IPart[axis] / Ki; // PID-controller for pitch |
370 | term[axis] = PDPart[axis] - controls[axis] + IPart[axis] / Ki; // PID-controller for pitch |
357 | term[axis] += (dynamicParams.levelCorrection[axis] - 128); |
371 | term[axis] += (dynamicParams.levelCorrection[axis] - 128); |