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 |