Subversion Repositories FlightCtrl

Rev

Rev 2073 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 2073 Rev 2189
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"
7 
 
 9 
 
8 
uint16_t emergencyFlightTime;
 10 
uint32_t emergencyFlightEndTimeMillis;
9 
 
 11 
 
10 
void FC_setNeutral(void) {
 12 
void FC_setNeutral(void) {
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) {
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) {
 23 
      if (isFlying > 256) {
27 
        MKFlags |= MKFLAG_EMERGENCY_FLIGHT; // Set flag for emergency landing
 24 
        MKFlags |= MKFLAG_EMERGENCY_FLIGHT; // Set flag for emergency landing
28 
        configuration_setFailsafeFlightParameters();
 25 
        configuration_setFailsafeFlightMode();
29 
        // Set the time in whole seconds.
 26 
        // Set the time in whole seconds.
30 
        if (staticParams.emergencyFlightDuration > (65535 - (uint16_t)F_MAINLOOP) / (uint16_t)F_MAINLOOP)
 - 
 
31 
          emergencyFlightTime = 0xffff;
 - 
 
32 
        else
 - 
 
33 
          emergencyFlightTime = (uint16_t)staticParams.emergencyFlightDuration * (uint16_t)F_MAINLOOP;
 27 
        emergencyFlightEndTimeMillis = millisClock + (staticParams.emergencyFlightDuration  * 1000L);
34 
      }
 28 
      }
35 
    } else {
 29 
    } else {
36 
      if (emergencyFlightTime) {
 30 
      if (millisClock > emergencyFlightEndTimeMillis) {
37 
        emergencyFlightTime--;
 - 
 
38 
      } else {
 - 
 
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 {
48 
    // Signal is OK.
 40 
    // Signal is OK.
49 
    if (MKFlags & MKFLAG_EMERGENCY_FLIGHT) {
 41 
    if (MKFlags & MKFLAG_EMERGENCY_FLIGHT) {
50 
      MKFlags &= ~MKFLAG_EMERGENCY_FLIGHT; // Clear flag for emergency landing
 42 
      MKFlags &= ~MKFLAG_EMERGENCY_FLIGHT; // Clear flag for emergency landing
51 
      configuration_setNormalFlightParameters();
 43 
      // configuration_setNormalFlightParameters();
52 
    }
 44 
    }
- 
 
 45 
    // A hack (permenent?): Keep re-setting the parameters, in order to support dynamic params.
- 
 
 46 
    configuration_setNormalFlightMode();
53 
  }
 47 
  }
54 
 
 48 
 
55 
  /*
 49 
  /*
56 
   * If a Bl-Ctrl is missing, prevent takeoff.
 50 
   * If a Bl-Ctrl is missing, prevent takeoff.
- 
 
 51 
   * Feature unnecessary, removed.
57 
   */
 52 
   */
- 
 
 53 
  /*
58 
  if (missingMotor) {
 54 
  if ((MKFlags & MKFLAG_MOTOR_RUN) && missingMotor) {
59 
    // if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway???
 55 
    // if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway???
60 
    if (isFlying > 1 && isFlying < 50 && PRTY[CONTROL_THROTTLE] > 0) isFlying = 1; // keep within lift off condition
 56 
    if (isFlying > 1 && isFlying < 50 && RPTY[CONTROL_THROTTLE] > 0) isFlying = 1; // keep within lift off condition
61 
    PRTY[CONTROL_THROTTLE] = staticParams.minThrottle; // reduce throttle to min to prevent takeoff
 57 
    RPTY[CONTROL_THROTTLE] = staticParams.minThrottle << 3; // reduce throttle to min to prevent takeoff
62 
  }
 58 
  }
- 
 
 59 
  */
63 
}
 60 
}
64 
 
 61