Rev 2056 | Rev 2059 | 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" |
||
4 | #include "twimaster.h" |
||
5 | #include "flight.h" |
||
6 | #include "output.h" |
||
7 | |||
8 | uint16_t emergencyFlightTime; |
||
9 | |||
10 | void setFailsafeFlightParameters(void) { |
||
11 | flight_setParameters(0, 90, 120, 90, 120); |
||
12 | } |
||
13 | |||
14 | void setNormalFlightParameters(void) { |
||
15 | flight_setParameters(staticParams.IFactor, dynamicParams.gyroP, |
||
16 | staticParams.bitConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.gyroI, |
||
17 | dynamicParams.gyroP, staticParams.yawIFactor); |
||
18 | } |
||
19 | |||
20 | void FC_setNeutral(void) { |
||
21 | setNormalFlightParameters(); |
||
22 | } |
||
23 | |||
24 | void FC_periodicTaskAndPRTY(uint16_t* PRTY) { |
||
25 | if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy |
||
26 | if (controlMixer_didReceiveSignal) |
||
27 | beepRCAlarm(); // Only make alarm if a control signal was received before the signal loss. |
||
28 | |||
29 | // There are the possibilities: We are not yet in emergency flight, we are already in emergency flight. |
||
30 | if (!(MKFlags & MKFLAG_EMERGENCY_FLIGHT)) { |
||
31 | if (isFlying > 256) { |
||
32 | MKFlags |= MKFLAG_EMERGENCY_FLIGHT; // Set flag for emergency landing |
||
33 | setFailsafeFlightParameters(); |
||
34 | // Set the time in whole seconds. |
||
35 | if (staticParams.emergencyFlightDuration > (65535 - F_MAINLOOP) / F_MAINLOOP) |
||
36 | emergencyFlightTime = 0xffff; |
||
37 | else |
||
38 | emergencyFlightTime = (uint16_t) staticParams.emergencyFlightDuration * F_MAINLOOP; |
||
39 | } |
||
40 | } else { |
||
41 | if (emergencyFlightTime) { |
||
42 | emergencyFlightTime--; |
||
43 | } else { |
||
44 | // stop motors but stay in emergency flight. |
||
45 | MKFlags &= ~(MKFLAG_MOTOR_RUN); |
||
46 | } |
||
47 | } |
||
48 | |||
2058 | - | 49 | // 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. |
2056 | - | 50 | PRTY[CONTROL_THROTTLE] = staticParams.emergencyThrottle; // Set emergency throttle |
2058 | - | 51 | PRTY[CONTROL_PITCH] = PRTY[CONTROL_ROLL] = PRTY[CONTROL_YAW] = 0; |
2056 | - | 52 | } else { |
53 | // Signal is OK. |
||
54 | if (MKFlags & MKFLAG_EMERGENCY_FLIGHT) { |
||
55 | MKFlags &= ~MKFLAG_EMERGENCY_FLIGHT; // Clear flag for emergency landing |
||
56 | setNormalFlightParameters(); |
||
57 | } |
||
58 | } |
||
59 | |||
60 | /* |
||
61 | * If a Bl-Ctrl is missing, prevent takeoff. |
||
62 | */ |
||
63 | if (missingMotor) { |
||
64 | // if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway??? |
||
65 | if (isFlying > 1 && isFlying < 50 && PRTY[CONTROL_THROTTLE] > 0) isFlying = 1; // keep within lift off condition |
||
66 | PRTY[CONTROL_THROTTLE] = staticParams.minThrottle; // reduce throttle to min to prevent takeoff |
||
67 | } |
||
68 | } |