Rev 2073 | Details | Compare with Previous | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 2056 | - | 1 | #include "controlMixer.h" |
| 2 | #include "timer0.h" |
||
| 3 | #include "configuration.h" |
||
| 2189 | - | 4 | //#include "twimaster.h" |
| 2056 | - | 5 | #include "flight.h" |
| 2189 | - | 6 | //#include "output.h" |
| 7 | #include "definitions.h" |
||
| 8 | #include "beeper.h" |
||
| 2056 | - | 9 | |
| 2189 | - | 10 | uint32_t emergencyFlightEndTimeMillis; |
| 2056 | - | 11 | |
| 2059 | - | 12 | void FC_setNeutral(void) { |
| 2189 | - | 13 | configuration_setNormalFlightMode(); |
| 2056 | - | 14 | } |
| 15 | |||
| 2189 | - | 16 | void FC_periodicTaskAndRPTY(int16_t* RPTY) { |
| 2056 | - | 17 | if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy |
| 18 | if (controlMixer_didReceiveSignal) |
||
| 19 | beepRCAlarm(); // Only make alarm if a control signal was received before the signal loss. |
||
| 20 | |||
| 21 | // There are the possibilities: We are not yet in emergency flight, we are already in emergency flight. |
||
| 22 | if (!(MKFlags & MKFLAG_EMERGENCY_FLIGHT)) { |
||
| 23 | if (isFlying > 256) { |
||
| 24 | MKFlags |= MKFLAG_EMERGENCY_FLIGHT; // Set flag for emergency landing |
||
| 2189 | - | 25 | configuration_setFailsafeFlightMode(); |
| 2056 | - | 26 | // Set the time in whole seconds. |
| 2189 | - | 27 | emergencyFlightEndTimeMillis = millisClock + (staticParams.emergencyFlightDuration * 1000L); |
| 2056 | - | 28 | } |
| 29 | } else { |
||
| 2189 | - | 30 | if (millisClock > emergencyFlightEndTimeMillis) { |
| 2056 | - | 31 | // stop motors but stay in emergency flight. |
| 32 | MKFlags &= ~(MKFLAG_MOTOR_RUN); |
||
| 33 | } |
||
| 34 | } |
||
| 35 | |||
| 2058 | - | 36 | // 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. |
| 2189 | - | 37 | RPTY[CONTROL_THROTTLE] = staticParams.emergencyThrottle << 3; // Set emergency throttle |
| 38 | RPTY[CONTROL_PITCH] = RPTY[CONTROL_ROLL] = RPTY[CONTROL_YAW] = 0; |
||
| 2056 | - | 39 | } else { |
| 40 | // Signal is OK. |
||
| 41 | if (MKFlags & MKFLAG_EMERGENCY_FLIGHT) { |
||
| 42 | MKFlags &= ~MKFLAG_EMERGENCY_FLIGHT; // Clear flag for emergency landing |
||
| 2189 | - | 43 | // configuration_setNormalFlightParameters(); |
| 2056 | - | 44 | } |
| 2189 | - | 45 | // A hack (permenent?): Keep re-setting the parameters, in order to support dynamic params. |
| 46 | configuration_setNormalFlightMode(); |
||
| 2056 | - | 47 | } |
| 48 | |||
| 49 | /* |
||
| 50 | * If a Bl-Ctrl is missing, prevent takeoff. |
||
| 2189 | - | 51 | * Feature unnecessary, removed. |
| 2056 | - | 52 | */ |
| 2189 | - | 53 | /* |
| 54 | if ((MKFlags & MKFLAG_MOTOR_RUN) && missingMotor) { |
||
| 2056 | - | 55 | // if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway??? |
| 2189 | - | 56 | if (isFlying > 1 && isFlying < 50 && RPTY[CONTROL_THROTTLE] > 0) isFlying = 1; // keep within lift off condition |
| 57 | RPTY[CONTROL_THROTTLE] = staticParams.minThrottle << 3; // reduce throttle to min to prevent takeoff |
||
| 2056 | - | 58 | } |
| 2189 | - | 59 | */ |
| 2056 | - | 60 | } |