Subversion Repositories FlightCtrl

Rev

Rev 2101 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 2101 Rev 2102
Line 2... Line 2...
2 
#include "timer0.h"
 2 
#include "timer0.h"
3 
#include "configuration.h"
 3 
#include "configuration.h"
4 
#include "flight.h"
 4 
#include "flight.h"
5 
#include "output.h"
 5 
#include "output.h"
Line 6... Line -...
6 
 
 - 
 
7 
uint16_t emergencyFlightTime;
 - 
 
8 
 
 6 
 
9 
void FC_periodicTaskAndPRTY(uint16_t* PRTY) {
 - 
 
10 
 
 - 
 
11 
  debugOut.analog[25] = emergencyFlightTime;
 - 
 
12 
 
 - 
 
13 
  // debugOut.analog[30] = controlMixer_getSignalQuality();
 - 
 
14 
 
 7 
void FC_periodicTaskAndPRTY(uint16_t* PRTY) {
15 
  if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy
 8 
  if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy
16 
    if (controlMixer_didReceiveSignal)
 9 
    if (controlMixer_didReceiveSignal) {
17 
      beepRCAlarm(); // Only make alarm if a control signal was received before the signal loss.
 - 
 
18 
 
 - 
 
19 
    // There are the possibilities: We are not yet in emergency flight, we are already in emergency flight.
 - 
 
20 
    if (!(MKFlags & MKFLAG_EMERGENCY_FLIGHT)) {
 - 
 
21 
      if (isFlying > 256) {
 - 
 
22 
        MKFlags |= MKFLAG_EMERGENCY_FLIGHT; // Set flag for emergency landing
 - 
 
23 
        // configuration_setFailsafeFlightParameters();
 - 
 
24 
        // Set the time in whole seconds.
 - 
 
25 
        if (staticParams.emergencyFlightDuration
 - 
 
26 
            > (65535 - (uint16_t)F_MAINLOOP) / (uint16_t)F_MAINLOOP)
 - 
 
27 
          emergencyFlightTime = 0xffff;
 - 
 
28 
        else
 - 
 
29 
          emergencyFlightTime = (uint16_t) staticParams.emergencyFlightDuration
 - 
 
30 
              * (uint16_t)F_MAINLOOP;
 - 
 
31 
      }
 - 
 
32 
    } else {
 - 
 
33 
      if (emergencyFlightTime) {
 - 
 
34 
        emergencyFlightTime--;
 - 
 
35 
      } else {
 - 
 
36 
        // stop motors but stay in emergency flight.
 - 
 
37 
        MKFlags &= ~(MKFLAG_MOTOR_RUN);
 - 
 
38 
      }
 - 
 
39 
    }
 - 
 
40 
 
 - 
 
41 
    // 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.
 - 
 
42 
    PRTY[CONTROL_THROTTLE] = staticParams.emergencyThrottle; // Set emergency throttle
 - 
 
43 
    PRTY[CONTROL_ELEVATOR] = PRTY[CONTROL_AILERONS] = PRTY[CONTROL_RUDDER] = 0;
 - 
 
44 
  } else {
 - 
 
45 
    // Signal is OK.
 - 
 
46 
    if (MKFlags & MKFLAG_EMERGENCY_FLIGHT) {
 - 
 
47 
      MKFlags &= ~MKFLAG_EMERGENCY_FLIGHT; // Clear flag for emergency landing
 - 
 
48 
      // configuration_setNormalFlightParameters();
 10 
      beepRCAlarm(); // Only make alarm if a control signal was received before the signal loss.
49 
    }
 11 
    }
50 
  }
 12 
  }
Line 51... Line -...
51 
}
 - 
 
52 
 
 13 
}
53 
 
 14