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 setFailsafeFlightParameters(void) {
  flight_setParameters(0, 90, 120, 90, 120);
}

void setNormalFlightParameters(void) {
  flight_setParameters(staticParams.IFactor, dynamicParams.gyroP,
      staticParams.bitConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.gyroI,
      dynamicParams.gyroP, staticParams.yawIFactor);
}

void FC_setNeutral(void) {
  setNormalFlightParameters();
}

void FC_periodicTaskAndPRTY(uint16_t* PRTY) {
  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
        setFailsafeFlightParameters();
        // Set the time in whole seconds.
        if (staticParams.emergencyFlightDuration > (65535 - F_MAINLOOP) / F_MAINLOOP)
          emergencyFlightTime = 0xffff;
        else
          emergencyFlightTime = (uint16_t) staticParams.emergencyFlightDuration * F_MAINLOOP;
      }
    } else {
      if (emergencyFlightTime) {
        emergencyFlightTime--;
      } else {
        // stop motors but stay in emergency flight.
        MKFlags &= ~(MKFLAG_MOTOR_RUN);
      }
    }

    // In either case, use e. throttle.
    PRTY[CONTROL_THROTTLE] = staticParams.emergencyThrottle; // Set emergency throttle
  } else {
    // Signal is OK.
    if (MKFlags & MKFLAG_EMERGENCY_FLIGHT) {
      MKFlags &= ~MKFLAG_EMERGENCY_FLIGHT; // Clear flag for emergency landing
      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
  }
}