forked from ArduPilot/ardupilot
-
Notifications
You must be signed in to change notification settings - Fork 6
/
is_flying.cpp
342 lines (295 loc) · 12.9 KB
/
is_flying.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
#include "Plane.h"
/*
is_flying and crash detection logic
*/
#define CRASH_DETECTION_DELAY_MS 500
#define IS_FLYING_IMPACT_TIMER_MS 3000
#define GPS_IS_FLYING_SPEED_CMS 150
/*
Do we think we are flying?
Probabilistic method where a bool is low-passed and considered a probability.
*/
void Plane::update_is_flying_5Hz(void)
{
float aspeed=0;
bool is_flying_bool = false;
uint32_t now_ms = AP_HAL::millis();
uint32_t ground_speed_thresh_cm = (aparm.min_gndspeed_cm > 0) ? ((uint32_t)(aparm.min_gndspeed_cm*0.9f)) : GPS_IS_FLYING_SPEED_CMS;
bool gps_confirmed_movement = (gps.status() >= AP_GPS::GPS_OK_FIX_3D) &&
(gps.ground_speed_cm() >= ground_speed_thresh_cm);
// airspeed at least 75% of stall speed?
const float airspeed_threshold = MAX(aparm.airspeed_min,2)*0.75f;
bool airspeed_movement = ahrs.airspeed_estimate(aspeed) && (aspeed >= airspeed_threshold);
if (gps.status() < AP_GPS::GPS_OK_FIX_2D && arming.is_armed() && !airspeed_movement && isFlyingProbability > 0.3) {
// when flying with no GPS, use the last airspeed estimate to
// determine if we think we have airspeed movement. This
// prevents the crash detector from triggering when
// dead-reckoning under long GPS loss
airspeed_movement = aspeed >= airspeed_threshold;
}
#if HAL_QUADPLANE_ENABLED
is_flying_bool = quadplane.is_flying();
#endif
if (is_flying_bool) {
// no need to look further
} else if(arming.is_armed()) {
// when armed assuming flying and we need overwhelming evidence that we ARE NOT flying
// short drop-outs of GPS are common during flight due to banking which points the antenna in different directions
bool gps_lost_recently = (gps.last_fix_time_ms() > 0) && // we have locked to GPS before
(gps.status() < AP_GPS::GPS_OK_FIX_2D) && // and it's lost now
(now_ms - gps.last_fix_time_ms() < 5000); // but it wasn't that long ago (<5s)
if ((auto_state.last_flying_ms > 0) && gps_lost_recently) {
// we've flown before, remove GPS constraints temporarily and only use airspeed
is_flying_bool = airspeed_movement; // moving through the air
} else {
// Because ahrs.airspeed_estimate can return a continued high value after landing if flying in
// strong winds above stall speed it is necessary to include the IMU based movement check.
is_flying_bool = (airspeed_movement && !AP::ins().is_still()) || // moving through the air
gps_confirmed_movement; // locked and we're moving
}
if (control_mode == &mode_auto) {
/*
make is_flying() more accurate during various auto modes
*/
// Detect X-axis deceleration for probable ground impacts.
// Limit the max probability so it can decay faster. This
// will not change the is_flying state, anything above 0.1
// is "true", it just allows it to decay faster once we decide we
// aren't flying using the normal schemes
if (g.crash_accel_threshold == 0) {
crash_state.impact_detected = false;
} else if (ins.get_accel_peak_hold_neg_x() < -(g.crash_accel_threshold)) {
// large deceleration detected, lets lower confidence VERY quickly
crash_state.impact_detected = true;
crash_state.impact_timer_ms = now_ms;
if (isFlyingProbability > 0.2f) {
isFlyingProbability = 0.2f;
}
} else if (crash_state.impact_detected &&
(now_ms - crash_state.impact_timer_ms > IS_FLYING_IMPACT_TIMER_MS)) {
// no impacts seen in a while, clear the flag so we stop clipping isFlyingProbability
crash_state.impact_detected = false;
}
switch (flight_stage)
{
case AP_Vehicle::FixedWing::FLIGHT_TAKEOFF:
break;
case AP_Vehicle::FixedWing::FLIGHT_NORMAL:
if (in_preLaunch_flight_stage()) {
// while on the ground, an uncalibrated airspeed sensor can drift to 7m/s so
// ensure we aren't showing a false positive.
is_flying_bool = false;
crash_state.is_crashed = false;
auto_state.started_flying_in_auto_ms = 0;
}
break;
case AP_Vehicle::FixedWing::FLIGHT_VTOL:
// TODO: detect ground impacts
break;
case AP_Vehicle::FixedWing::FLIGHT_LAND:
if (landing.is_on_approach() && auto_state.sink_rate > 0.2f) {
is_flying_bool = true;
}
break;
case AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND:
if (auto_state.sink_rate < -0.5f) {
// steep climb
is_flying_bool = true;
}
break;
default:
break;
} // switch
}
} else {
// when disarmed assume not flying and need overwhelming evidence that we ARE flying
is_flying_bool = airspeed_movement && gps_confirmed_movement;
if ((flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF) || landing.is_flaring()) {
is_flying_bool = false;
}
}
if (!crash_state.impact_detected || !is_flying_bool) {
// when impact is detected, enforce a clip. Only allow isFlyingProbability to go down, not up.
// low-pass the result.
// coef=0.15f @ 5Hz takes 3.0s to go from 100% down to 10% (or 0% up to 90%)
isFlyingProbability = (0.85f * isFlyingProbability) + (0.15f * (float)is_flying_bool);
}
/*
update last_flying_ms so we always know how long we have not
been flying for. This helps for crash detection and auto-disarm
*/
bool new_is_flying = is_flying();
// we are flying, note the time
if (new_is_flying) {
auto_state.last_flying_ms = now_ms;
if (!previous_is_flying) {
// just started flying in any mode
started_flying_ms = now_ms;
}
if ((control_mode == &mode_auto) &&
((auto_state.started_flying_in_auto_ms == 0) || !previous_is_flying) ) {
// We just started flying, note that time also
auto_state.started_flying_in_auto_ms = now_ms;
}
}
previous_is_flying = new_is_flying;
#if HAL_ADSB_ENABLED
adsb.set_is_flying(new_is_flying);
#endif
#if PARACHUTE == ENABLED
parachute.set_is_flying(new_is_flying);
#endif
#if STATS_ENABLED == ENABLED
g2.stats.set_flying(new_is_flying);
#endif
AP_Notify::flags.flying = new_is_flying;
crash_detection_update();
Log_Write_Status();
// tell AHRS flying state
set_likely_flying(new_is_flying);
// conservative ground mode value for rate D suppression
ground_mode = !is_flying() && !hal.util->get_soft_armed();
}
/*
return true if we think we are flying. This is a probabilistic
estimate, and needs to be used very carefully. Each use case needs
to be thought about individually.
*/
bool Plane::is_flying(void)
{
if (hal.util->get_soft_armed()) {
#if HAL_QUADPLANE_ENABLED
if (quadplane.is_flying_vtol()) {
return true;
}
#endif
// when armed, assume we're flying unless we probably aren't
return (isFlyingProbability >= 0.1f);
}
// when disarmed, assume we're not flying unless we probably are
return (isFlyingProbability >= 0.9f);
}
/*
* Determine if we have crashed
*/
void Plane::crash_detection_update(void)
{
if (control_mode != &mode_auto || !aparm.crash_detection_enable)
{
// crash detection is only available in AUTO mode
crash_state.debounce_timer_ms = 0;
crash_state.is_crashed = false;
return;
}
uint32_t now_ms = AP_HAL::millis();
bool crashed_near_land_waypoint = false;
bool crashed = false;
bool been_auto_flying = (auto_state.started_flying_in_auto_ms > 0) &&
(now_ms - auto_state.started_flying_in_auto_ms >= 2500);
if (!is_flying() && arming.is_armed())
{
if (landing.is_expecting_impact()) {
// We should be nice and level-ish in this flight stage. If not, we most
// likely had a crazy landing. Throttle is inhibited already at the flare
// but go ahead and notify GCS and perform any additional post-crash actions.
// Declare a crash if we are oriented more that 60deg in pitch or roll
if (!crash_state.checkedHardLanding && // only check once
been_auto_flying &&
(labs(ahrs.roll_sensor) > 6000 || labs(ahrs.pitch_sensor) > 6000)) {
crashed = true;
// did we "crash" within 75m of the landing location? Probably just a hard landing
crashed_near_land_waypoint =
current_loc.get_distance(mission.get_current_nav_cmd().content.location) < 75;
// trigger hard landing event right away, or never again. This inhibits a false hard landing
// event when, for example, a minute after a good landing you pick the plane up and
// this logic is still running and detects the plane is on its side as you carry it.
crash_state.debounce_timer_ms = now_ms;
crash_state.debounce_time_total_ms = 0; // no debounce
}
crash_state.checkedHardLanding = true;
} else if (landing.is_on_approach()) {
// when altitude gets low, we automatically flare so ground crashes
// most likely can not be triggered from here. However,
// a crash into a tree would be caught here.
if (been_auto_flying) {
crashed = true;
crash_state.debounce_time_total_ms = CRASH_DETECTION_DELAY_MS;
}
} else {
switch (flight_stage)
{
case AP_Vehicle::FixedWing::FLIGHT_TAKEOFF:
if (g.takeoff_throttle_min_accel > 0 &&
!throttle_suppressed) {
// if you have an acceleration holding back throttle, but you met the
// accel threshold but still not fying, then you either shook/hit the
// plane or it was a failed launch.
crashed = true;
crash_state.debounce_time_total_ms = CRASH_DETECTION_DELAY_MS;
}
// TODO: handle auto missions without NAV_TAKEOFF mission cmd
break;
case AP_Vehicle::FixedWing::FLIGHT_NORMAL:
if (!in_preLaunch_flight_stage() && been_auto_flying) {
crashed = true;
crash_state.debounce_time_total_ms = CRASH_DETECTION_DELAY_MS;
}
break;
case AP_Vehicle::FixedWing::FLIGHT_VTOL:
// we need a totally new method for this
crashed = false;
break;
default:
break;
} // switch
}
} else {
crash_state.checkedHardLanding = false;
}
// if we have no GPS lock and we don't have a functional airspeed
// sensor then don't do crash detection
if (gps.status() < AP_GPS::GPS_OK_FIX_3D) {
#if AP_AIRSPEED_ENABLED
if (!airspeed.use() || !airspeed.healthy()) {
crashed = false;
}
#else
crashed = false;
#endif
}
if (!crashed) {
// reset timer
crash_state.debounce_timer_ms = 0;
} else if (crash_state.debounce_timer_ms == 0) {
// start timer
crash_state.debounce_timer_ms = now_ms;
} else if ((now_ms - crash_state.debounce_timer_ms >= crash_state.debounce_time_total_ms) && !crash_state.is_crashed) {
crash_state.is_crashed = true;
if (aparm.crash_detection_enable & CRASH_DETECT_ACTION_BITMASK_DISARM) {
arming.disarm(AP_Arming::Method::CRASH);
}
if (crashed_near_land_waypoint) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Hard landing detected");
} else {
gcs().send_text(MAV_SEVERITY_EMERGENCY, "Crash detected");
}
}
}
/*
* return true if we are in a pre-launch phase of an auto-launch, typically used in bungee launches
*/
bool Plane::in_preLaunch_flight_stage(void)
{
if (control_mode == &mode_takeoff && throttle_suppressed) {
return true;
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.is_vtol_takeoff(mission.get_current_nav_cmd().id)) {
return false;
}
#endif
return (control_mode == &mode_auto &&
throttle_suppressed &&
flight_stage == AP_Vehicle::FixedWing::FLIGHT_NORMAL &&
mission.get_current_nav_cmd().id == MAV_CMD_NAV_TAKEOFF);
}