Subversion Repositories FlightCtrl

#include "controlMixer.h"
#include "timer0.h"
#include "configuration.h"
#include "twimaster.h"
#include "flight.h"
#include "output.h"

uint16_t emergencyFlightTime;

void FC_setNeutral(void) {
  configuration_setNormalFlightParameters();
}

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_PITCH] = PRTY[CONTROL_ROLL] = PRTY[CONTROL_YAW] = 0;
  } else {
    // Signal is OK.
    if (MKFlags & MKFLAG_EMERGENCY_FLIGHT) {
      MKFlags &= ~MKFLAG_EMERGENCY_FLIGHT; // Clear flag for emergency landing
      configuration_setNormalFlightParameters();
    }
  }

  /*
   * If a Bl-Ctrl is missing, prevent takeoff.
   */
  if (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
  }
}