| 1,22 → 1,19 |
|---|
| #include "controlMixer.h" |
| #include "timer0.h" |
| #include "configuration.h" |
| #include "twimaster.h" |
| //#include "twimaster.h" |
| #include "flight.h" |
| #include "output.h" |
| //#include "output.h" |
| #include "definitions.h" |
| #include "beeper.h" |
| |
| uint16_t emergencyFlightTime; |
| uint32_t emergencyFlightEndTimeMillis; |
| |
| void FC_setNeutral(void) { |
| configuration_setNormalFlightParameters(); |
| configuration_setNormalFlightMode(); |
| } |
| |
| void FC_periodicTaskAndPRTY(uint16_t* PRTY) { |
| |
| debugOut.analog[25] = emergencyFlightTime; |
| |
| // debugOut.analog[30] = controlMixer_getSignalQuality(); |
| |
| void FC_periodicTaskAndRPTY(int16_t* RPTY) { |
| if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy |
| if (controlMixer_didReceiveSignal) |
| beepRCAlarm(); // Only make alarm if a control signal was received before the signal loss. |
| 25,17 → 22,12 |
|---|
| if (!(MKFlags & MKFLAG_EMERGENCY_FLIGHT)) { |
| if (isFlying > 256) { |
| MKFlags |= MKFLAG_EMERGENCY_FLIGHT; // Set flag for emergency landing |
| configuration_setFailsafeFlightParameters(); |
| configuration_setFailsafeFlightMode(); |
| // Set the time in whole seconds. |
| if (staticParams.emergencyFlightDuration > (65535 - (uint16_t)F_MAINLOOP) / (uint16_t)F_MAINLOOP) |
| emergencyFlightTime = 0xffff; |
| else |
| emergencyFlightTime = (uint16_t)staticParams.emergencyFlightDuration * (uint16_t)F_MAINLOOP; |
| emergencyFlightEndTimeMillis = millisClock + (staticParams.emergencyFlightDuration * 1000L); |
| } |
| } else { |
| if (emergencyFlightTime) { |
| emergencyFlightTime--; |
| } else { |
| if (millisClock > emergencyFlightEndTimeMillis) { |
| // stop motors but stay in emergency flight. |
| MKFlags &= ~(MKFLAG_MOTOR_RUN); |
| } |
| 42,22 → 34,27 |
|---|
| } |
| |
| // 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. |
| PRTY[CONTROL_THROTTLE] = staticParams.emergencyThrottle; // Set emergency throttle |
| PRTY[CONTROL_PITCH] = PRTY[CONTROL_ROLL] = PRTY[CONTROL_YAW] = 0; |
| RPTY[CONTROL_THROTTLE] = staticParams.emergencyThrottle << 3; // Set emergency throttle |
| RPTY[CONTROL_PITCH] = RPTY[CONTROL_ROLL] = RPTY[CONTROL_YAW] = 0; |
| } else { |
| // Signal is OK. |
| if (MKFlags & MKFLAG_EMERGENCY_FLIGHT) { |
| MKFlags &= ~MKFLAG_EMERGENCY_FLIGHT; // Clear flag for emergency landing |
| configuration_setNormalFlightParameters(); |
| // configuration_setNormalFlightParameters(); |
| } |
| // A hack (permenent?): Keep re-setting the parameters, in order to support dynamic params. |
| configuration_setNormalFlightMode(); |
| } |
| |
| /* |
| * If a Bl-Ctrl is missing, prevent takeoff. |
| * Feature unnecessary, removed. |
| */ |
| if (missingMotor) { |
| /* |
| if ((MKFlags & MKFLAG_MOTOR_RUN) && missingMotor) { |
| // if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway??? |
| if (isFlying > 1 && isFlying < 50 && PRTY[CONTROL_THROTTLE] > 0) isFlying = 1; // keep within lift off condition |
| PRTY[CONTROL_THROTTLE] = staticParams.minThrottle; // reduce throttle to min to prevent takeoff |
| if (isFlying > 1 && isFlying < 50 && RPTY[CONTROL_THROTTLE] > 0) isFlying = 1; // keep within lift off condition |
| RPTY[CONTROL_THROTTLE] = staticParams.minThrottle << 3; // reduce throttle to min to prevent takeoff |
| } |
| */ |
| } |