Subversion Repositories FlightCtrl

#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() {
}