Rev 2069 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2069 | Rev 2073 | ||
|---|---|---|---|
| 1 | #include "controlMixer.h" |
1 | #include "controlMixer.h" |
| 2 | #include "timer0.h" |
2 | #include "timer0.h" |
| 3 | #include "configuration.h" |
3 | #include "configuration.h" |
| 4 | #include "twimaster.h" |
4 | #include "twimaster.h" |
| 5 | #include "flight.h" |
5 | #include "flight.h" |
| 6 | #include "output.h" |
6 | #include "output.h" |
| 7 | 7 | ||
| 8 | uint16_t emergencyFlightTime; |
8 | uint16_t emergencyFlightTime; |
| 9 | 9 | ||
| 10 | void FC_setNeutral(void) { |
10 | void FC_setNeutral(void) { |
| 11 | configuration_setNormalFlightParameters(); |
11 | configuration_setNormalFlightParameters(); |
| 12 | } |
12 | } |
| 13 | 13 | ||
| 14 | void FC_periodicTaskAndPRTY(uint16_t* PRTY) { |
14 | void FC_periodicTaskAndPRTY(uint16_t* PRTY) { |
| 15 | 15 | ||
| 16 | debugOut.analog[25] = emergencyFlightTime; |
16 | debugOut.analog[25] = emergencyFlightTime; |
| 17 | 17 | ||
| 18 | debugOut.analog[30] = controlMixer_getSignalQuality(); |
18 | // debugOut.analog[30] = controlMixer_getSignalQuality(); |
| 19 | 19 | ||
| 20 | if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy |
20 | if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy |
| 21 | if (controlMixer_didReceiveSignal) |
21 | if (controlMixer_didReceiveSignal) |
| 22 | beepRCAlarm(); // Only make alarm if a control signal was received before the signal loss. |
22 | beepRCAlarm(); // Only make alarm if a control signal was received before the signal loss. |
| 23 | 23 | ||
| 24 | // There are the possibilities: We are not yet in emergency flight, we are already in emergency flight. |
24 | // There are the possibilities: We are not yet in emergency flight, we are already in emergency flight. |
| 25 | if (!(MKFlags & MKFLAG_EMERGENCY_FLIGHT)) { |
25 | if (!(MKFlags & MKFLAG_EMERGENCY_FLIGHT)) { |
| 26 | if (isFlying > 256) { |
26 | if (isFlying > 256) { |
| 27 | MKFlags |= MKFLAG_EMERGENCY_FLIGHT; // Set flag for emergency landing |
27 | MKFlags |= MKFLAG_EMERGENCY_FLIGHT; // Set flag for emergency landing |
| 28 | configuration_setFailsafeFlightParameters(); |
28 | configuration_setFailsafeFlightParameters(); |
| 29 | // Set the time in whole seconds. |
29 | // Set the time in whole seconds. |
| 30 | if (staticParams.emergencyFlightDuration > (65535 - (uint16_t)F_MAINLOOP) / (uint16_t)F_MAINLOOP) |
30 | if (staticParams.emergencyFlightDuration > (65535 - (uint16_t)F_MAINLOOP) / (uint16_t)F_MAINLOOP) |
| 31 | emergencyFlightTime = 0xffff; |
31 | emergencyFlightTime = 0xffff; |
| 32 | else |
32 | else |
| 33 | emergencyFlightTime = (uint16_t)staticParams.emergencyFlightDuration * (uint16_t)F_MAINLOOP; |
33 | emergencyFlightTime = (uint16_t)staticParams.emergencyFlightDuration * (uint16_t)F_MAINLOOP; |
| 34 | } |
34 | } |
| 35 | } else { |
35 | } else { |
| 36 | if (emergencyFlightTime) { |
36 | if (emergencyFlightTime) { |
| 37 | emergencyFlightTime--; |
37 | emergencyFlightTime--; |
| 38 | } else { |
38 | } else { |
| 39 | // stop motors but stay in emergency flight. |
39 | // stop motors but stay in emergency flight. |
| 40 | MKFlags &= ~(MKFLAG_MOTOR_RUN); |
40 | MKFlags &= ~(MKFLAG_MOTOR_RUN); |
| 41 | } |
41 | } |
| 42 | } |
42 | } |
| 43 | 43 | ||
| 44 | // In either case, use e. throttle and neutral controls. TODO: If there is supposed to be a navi come-home, this should affect RC control only and not navi. |
44 | // In either case, use e. throttle and neutral controls. TODO: If there is supposed to be a navi come-home, this should affect RC control only and not navi. |
| 45 | PRTY[CONTROL_THROTTLE] = staticParams.emergencyThrottle; // Set emergency throttle |
45 | PRTY[CONTROL_THROTTLE] = staticParams.emergencyThrottle; // Set emergency throttle |
| 46 | PRTY[CONTROL_PITCH] = PRTY[CONTROL_ROLL] = PRTY[CONTROL_YAW] = 0; |
46 | PRTY[CONTROL_PITCH] = PRTY[CONTROL_ROLL] = PRTY[CONTROL_YAW] = 0; |
| 47 | } else { |
47 | } else { |
| 48 | // Signal is OK. |
48 | // Signal is OK. |
| 49 | if (MKFlags & MKFLAG_EMERGENCY_FLIGHT) { |
49 | if (MKFlags & MKFLAG_EMERGENCY_FLIGHT) { |
| 50 | MKFlags &= ~MKFLAG_EMERGENCY_FLIGHT; // Clear flag for emergency landing |
50 | MKFlags &= ~MKFLAG_EMERGENCY_FLIGHT; // Clear flag for emergency landing |
| 51 | configuration_setNormalFlightParameters(); |
51 | configuration_setNormalFlightParameters(); |
| 52 | } |
52 | } |
| 53 | } |
53 | } |
| 54 | 54 | ||
| 55 | /* |
55 | /* |
| 56 | * If a Bl-Ctrl is missing, prevent takeoff. |
56 | * If a Bl-Ctrl is missing, prevent takeoff. |
| 57 | */ |
57 | */ |
| 58 | if (missingMotor) { |
58 | if (missingMotor) { |
| 59 | // if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway??? |
59 | // if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway??? |
| 60 | if (isFlying > 1 && isFlying < 50 && PRTY[CONTROL_THROTTLE] > 0) isFlying = 1; // keep within lift off condition |
60 | if (isFlying > 1 && isFlying < 50 && PRTY[CONTROL_THROTTLE] > 0) isFlying = 1; // keep within lift off condition |
| 61 | PRTY[CONTROL_THROTTLE] = staticParams.minThrottle; // reduce throttle to min to prevent takeoff |
61 | PRTY[CONTROL_THROTTLE] = staticParams.minThrottle; // reduce throttle to min to prevent takeoff |
| 62 | } |
62 | } |
| 63 | } |
63 | } |
| 64 | 64 | ||