Subversion Repositories FlightCtrl

Rev

Rev 2073 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
2056 - 1
#include "controlMixer.h"
2
#include "timer0.h"
3
#include "configuration.h"
2189 - 4
//#include "twimaster.h"
2056 - 5
#include "flight.h"
2189 - 6
//#include "output.h"
7
#include "definitions.h"
8
#include "beeper.h"
2056 - 9
 
2189 - 10
uint32_t emergencyFlightEndTimeMillis;
2056 - 11
 
2059 - 12
void FC_setNeutral(void) {
2189 - 13
  configuration_setNormalFlightMode();
2056 - 14
}
15
 
2189 - 16
void FC_periodicTaskAndRPTY(int16_t* RPTY) {
2056 - 17
  if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy
18
    if (controlMixer_didReceiveSignal)
19
      beepRCAlarm(); // Only make alarm if a control signal was received before the signal loss.
20
 
21
    // There are the possibilities: We are not yet in emergency flight, we are already in emergency flight.
22
    if (!(MKFlags & MKFLAG_EMERGENCY_FLIGHT)) {
23
      if (isFlying > 256) {
24
        MKFlags |= MKFLAG_EMERGENCY_FLIGHT; // Set flag for emergency landing
2189 - 25
        configuration_setFailsafeFlightMode();
2056 - 26
        // Set the time in whole seconds.
2189 - 27
        emergencyFlightEndTimeMillis = millisClock + (staticParams.emergencyFlightDuration  * 1000L);
2056 - 28
      }
29
    } else {
2189 - 30
      if (millisClock > emergencyFlightEndTimeMillis) {
2056 - 31
        // stop motors but stay in emergency flight.
32
        MKFlags &= ~(MKFLAG_MOTOR_RUN);
33
      }
34
    }
35
 
2058 - 36
    // 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.
2189 - 37
    RPTY[CONTROL_THROTTLE] = staticParams.emergencyThrottle << 3; // Set emergency throttle
38
    RPTY[CONTROL_PITCH] = RPTY[CONTROL_ROLL] = RPTY[CONTROL_YAW] = 0;
2056 - 39
  } else {
40
    // Signal is OK.
41
    if (MKFlags & MKFLAG_EMERGENCY_FLIGHT) {
42
      MKFlags &= ~MKFLAG_EMERGENCY_FLIGHT; // Clear flag for emergency landing
2189 - 43
      // configuration_setNormalFlightParameters();
2056 - 44
    }
2189 - 45
    // A hack (permenent?): Keep re-setting the parameters, in order to support dynamic params.
46
    configuration_setNormalFlightMode();
2056 - 47
  }
48
 
49
  /*
50
   * If a Bl-Ctrl is missing, prevent takeoff.
2189 - 51
   * Feature unnecessary, removed.
2056 - 52
   */
2189 - 53
  /*
54
  if ((MKFlags & MKFLAG_MOTOR_RUN) && missingMotor) {
2056 - 55
    // if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway???
2189 - 56
    if (isFlying > 1 && isFlying < 50 && RPTY[CONTROL_THROTTLE] > 0) isFlying = 1; // keep within lift off condition
57
    RPTY[CONTROL_THROTTLE] = staticParams.minThrottle << 3; // reduce throttle to min to prevent takeoff
2056 - 58
  }
2189 - 59
  */
2056 - 60
}