Rev 2102 | Go to most recent revision | Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 2101 | - | 1 | #include "controlMixer.h" |
| 2 | #include "timer0.h" |
||
| 3 | #include "configuration.h" |
||
| 4 | #include "flight.h" |
||
| 5 | #include "output.h" |
||
| 6 | |||
| 7 | uint16_t emergencyFlightTime; |
||
| 8 | |||
| 9 | void FC_periodicTaskAndPRTY(uint16_t* PRTY) { |
||
| 10 | |||
| 11 | debugOut.analog[25] = emergencyFlightTime; |
||
| 12 | |||
| 13 | // debugOut.analog[30] = controlMixer_getSignalQuality(); |
||
| 14 | |||
| 15 | if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy |
||
| 16 | if (controlMixer_didReceiveSignal) |
||
| 17 | beepRCAlarm(); // Only make alarm if a control signal was received before the signal loss. |
||
| 18 | |||
| 19 | // There are the possibilities: We are not yet in emergency flight, we are already in emergency flight. |
||
| 20 | if (!(MKFlags & MKFLAG_EMERGENCY_FLIGHT)) { |
||
| 21 | if (isFlying > 256) { |
||
| 22 | MKFlags |= MKFLAG_EMERGENCY_FLIGHT; // Set flag for emergency landing |
||
| 23 | // configuration_setFailsafeFlightParameters(); |
||
| 24 | // Set the time in whole seconds. |
||
| 25 | if (staticParams.emergencyFlightDuration |
||
| 26 | > (65535 - (uint16_t)F_MAINLOOP) / (uint16_t)F_MAINLOOP) |
||
| 27 | emergencyFlightTime = 0xffff; |
||
| 28 | else |
||
| 29 | emergencyFlightTime = (uint16_t) staticParams.emergencyFlightDuration |
||
| 30 | * (uint16_t)F_MAINLOOP; |
||
| 31 | } |
||
| 32 | } else { |
||
| 33 | if (emergencyFlightTime) { |
||
| 34 | emergencyFlightTime--; |
||
| 35 | } else { |
||
| 36 | // stop motors but stay in emergency flight. |
||
| 37 | MKFlags &= ~(MKFLAG_MOTOR_RUN); |
||
| 38 | } |
||
| 39 | } |
||
| 40 | |||
| 41 | // 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. |
||
| 42 | PRTY[CONTROL_THROTTLE] = staticParams.emergencyThrottle; // Set emergency throttle |
||
| 43 | PRTY[CONTROL_ELEVATOR] = PRTY[CONTROL_AILERONS] = PRTY[CONTROL_RUDDER] = 0; |
||
| 44 | } else { |
||
| 45 | // Signal is OK. |
||
| 46 | if (MKFlags & MKFLAG_EMERGENCY_FLIGHT) { |
||
| 47 | MKFlags &= ~MKFLAG_EMERGENCY_FLIGHT; // Clear flag for emergency landing |
||
| 48 | // configuration_setNormalFlightParameters(); |
||
| 49 | } |
||
| 50 | } |
||
| 51 | } |
||
| 52 | |||
| 53 | |||
| 54 | void FC_setNeutral() { |
||
| 55 | } |