0,0 → 1,55 |
#include "controlMixer.h" |
#include "timer0.h" |
#include "configuration.h" |
#include "flight.h" |
#include "output.h" |
|
uint16_t emergencyFlightTime; |
|
void FC_periodicTaskAndPRTY(uint16_t* PRTY) { |
|
debugOut.analog[25] = emergencyFlightTime; |
|
// debugOut.analog[30] = controlMixer_getSignalQuality(); |
|
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. |
|
// There are the possibilities: We are not yet in emergency flight, we are already in emergency flight. |
if (!(MKFlags & MKFLAG_EMERGENCY_FLIGHT)) { |
if (isFlying > 256) { |
MKFlags |= MKFLAG_EMERGENCY_FLIGHT; // Set flag for emergency landing |
// configuration_setFailsafeFlightParameters(); |
// 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; |
} |
} else { |
if (emergencyFlightTime) { |
emergencyFlightTime--; |
} else { |
// stop motors but stay in emergency flight. |
MKFlags &= ~(MKFLAG_MOTOR_RUN); |
} |
} |
|
// 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_ELEVATOR] = PRTY[CONTROL_AILERONS] = PRTY[CONTROL_RUDDER] = 0; |
} else { |
// Signal is OK. |
if (MKFlags & MKFLAG_EMERGENCY_FLIGHT) { |
MKFlags &= ~MKFLAG_EMERGENCY_FLIGHT; // Clear flag for emergency landing |
// configuration_setNormalFlightParameters(); |
} |
} |
} |
|
|
void FC_setNeutral() { |
} |