Subversion Repositories FlightCtrl

Rev

Rev 2073 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

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