Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-19 22:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: reworked is_flying

Browse Source 
add crash detection, allow disengage via param CRASH_DETECT
improved is_flying behavior
take off, landing and hard-landing improvements
add stillness check to is_flying and log it
minimum airspeed is determined ARSPD_FBW_MIN*0.75
This commit is contained in:
Tom Pittenger 2015-06-03 12:22:24 -07:00 committed by Andrew Tridgell
parent 2620a57700
commit da8f4f9e95
8 changed files with 242 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

228
ArduPlane/ArduPlane.cpp
View File

@ -74,6 +74,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] PROGMEM = {
{ SCHED_TASK(frsky_telemetry_send), 10, 100 },
#endif
{ SCHED_TASK(terrain_update), 5, 500 },
{ SCHED_TASK(update_is_flying_5Hz), 10, 100 },
};
void Plane::setup()
@ -303,14 +304,13 @@ void Plane::one_second_loop()
update_aux();
// determine if we are flying or not
determine_is_flying();
// update notify flags
AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false);
AP_Notify::flags.pre_arm_gps_check = true;
AP_Notify::flags.armed = arming.is_armed() || arming.arming_required() == AP_Arming::NO;
crash_detection_update();
#if AP_TERRAIN_AVAILABLE
if (should_log(MASK_LOG_GPS)) {
terrain.log_terrain_data(DataFlash);
@ -856,43 +856,111 @@ void Plane::update_flight_stage(void)
Do we think we are flying?
Probabilistic method where a bool is low-passed and considered a probability.
*/
void Plane::determine_is_flying(void)
void Plane::update_is_flying_5Hz(void)
{
float aspeed;
bool isFlyingBool;
bool is_flying_bool;
uint32_t now_ms = hal.scheduler->millis();
bool airspeedMovement = ahrs.airspeed_estimate(&aspeed) && (aspeed >= 5);
uint32_t ground_speed_thresh_cm = (g.min_gndspeed_cm > 0) ? ((uint32_t)(g.min_gndspeed_cm*0.9f)) : 500;
bool gps_confirmed_movement = (gps.status() >= AP_GPS::GPS_OK_FIX_3D) &&
(gps.ground_speed_cm() >= ground_speed_thresh_cm);
// If we don't have a GPS lock then don't use GPS for this test
bool gpsMovement = (gps.status() < AP_GPS::GPS_OK_FIX_2D ||
gps.ground_speed() >= 5);
// airspeed at least 75% of stall speed?
bool airspeed_movement = ahrs.airspeed_estimate(&aspeed) && (aspeed >= (aparm.airspeed_min*0.75f));
if (ins.is_still()) {
// if motionless, we can't possibly be flying. This is a very strict test
is_flying_bool = false;
}
else if(arming.is_armed()) {
// when armed assuming flying and we need overwhelming evidence that we ARE NOT flying
if (hal.util->get_soft_armed()) {
// when armed, we need overwhelming evidence that we ARE NOT flying
isFlyingBool = airspeedMovement || gpsMovement;
// 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)
/*
make is_flying() more accurate for landing approach
*/
if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH &&
fabsf(auto_state.sink_rate) > 0.2f) {
isFlyingBool = true;
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 {
// we've never flown yet, require good GPS movement
is_flying_bool = airspeed_movement || // moving through the air
gps_confirmed_movement; // locked and we're moving
}
if (control_mode == AUTO) {
/*
make is_flying() more accurate during various auto modes
*/
switch (flight_stage)
{
case AP_SpdHgtControl::FLIGHT_TAKEOFF:
// while on the ground, an uncalibrated airspeed sensor can drift to 7m/s so
// ensure we aren't showing a false positive. If the throttle is suppressed
// we are definitely not flying, or at least for not much longer!
if (throttle_suppressed) {
is_flying_bool = false;
auto_state.is_crashed = false;
}
break;
case AP_SpdHgtControl::FLIGHT_NORMAL:
// TODO: detect ground impacts
break;
case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
// TODO: detect ground impacts
if (fabsf(auto_state.sink_rate) > 0.2f) {
is_flying_bool = true;
}
break;
case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
default:
break;
} // switch
}
} else {
// when disarmed, we need overwhelming evidence that we ARE flying
isFlyingBool = airspeedMovement && gpsMovement;
// when disarmed assume not flying and need overwhelming evidence that we ARE flying
is_flying_bool = airspeed_movement && gps_confirmed_movement;
if ((control_mode == AUTO) &&
((flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF) ||
(flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL)) ) {
is_flying_bool = false;
}
}
// low-pass the result.
isFlyingProbability = (0.6f * isFlyingProbability) + (0.4f * (float)isFlyingBool);
// 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
*/
if (is_flying()) {
auto_state.last_flying_ms = millis();
bool new_is_flying = is_flying();
static bool previous_is_flying = false;
// we are flying, note the time
if (new_is_flying) {
auto_state.last_flying_ms = now_ms;
if ((control_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 (should_log(MASK_LOG_MODE)) {
Log_Write_Status();
}
}
@ -912,6 +980,120 @@ bool Plane::is_flying(void)
return (isFlyingProbability >= 0.9f);
}
/*
* Determine if we have crashed
*/
void Plane::crash_detection_update(void)
{
static uint32_t crash_timer_ms = 0;
static bool checkHardLanding = false;
if (control_mode != AUTO)
{
// crash detection is only available in AUTO mode
crash_timer_ms = 0;
return;
}
uint32_t now_ms = hal.scheduler->millis();
bool auto_launch_detected;
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())
{
switch (flight_stage)
{
case AP_SpdHgtControl::FLIGHT_TAKEOFF:
auto_launch_detected = !throttle_suppressed && (g.takeoff_throttle_min_accel > 0);
if (been_auto_flying || // failed hand launch
auto_launch_detected) { // threshold of been_auto_flying may not be met on auto-launches
// has launched but is no longer flying. That's a crash on takeoff.
crashed = true;
}
break;
case AP_SpdHgtControl::FLIGHT_NORMAL:
if (been_auto_flying) {
crashed = true;
}
// TODO: handle auto missions without NAV_TAKEOFF mission cmd
break;
case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
if (been_auto_flying) {
crashed = true;
}
// when altitude gets low, we automatically progress to FLIGHT_LAND_FINAL
// so ground crashes most likely can not be triggered from here. However,
// a crash into a tree, for example, would be.
break;
case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
// 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 (been_auto_flying &&
!checkHardLanding && // only check once
(fabsf(ahrs.roll_sensor) > 6000 || fabsf(ahrs.pitch_sensor) > 6000)) {
crashed = true;
// did we "crash" within 75m of the landing location? Probably just a hard landing
crashed_near_land_waypoint =
get_distance(current_loc, 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_timer_ms = now_ms + 2500;
}
checkHardLanding = true;
break;
default:
break;
} // switch
} else {
checkHardLanding = false;
}
if (!crashed) {
// reset timer
crash_timer_ms = 0;
auto_state.is_crashed = false;
} else if (crash_timer_ms == 0) {
// start timer
crash_timer_ms = now_ms;
} else if ((now_ms - crash_timer_ms >= 2500) && !auto_state.is_crashed) {
auto_state.is_crashed = true;
if (g.crash_detection_enable == CRASH_DETECT_ACTION_BITMASK_DISABLED) {
if (crashed_near_land_waypoint) {
gcs_send_text_P(SEVERITY_HIGH, PSTR("Hard Landing Detected - no action taken"));
} else {
gcs_send_text_P(SEVERITY_HIGH, PSTR("Crash Detected - no action taken"));
}
}
else {
if (g.crash_detection_enable & CRASH_DETECT_ACTION_BITMASK_DISARM) {
disarm_motors();
}
auto_state.land_complete = true;
if (crashed_near_land_waypoint) {
gcs_send_text_P(SEVERITY_HIGH, PSTR("Hard Landing Detected"));
} else {
gcs_send_text_P(SEVERITY_HIGH, PSTR("Crash Detected"));
}
}
}
}
#if OPTFLOW == ENABLED
// called at 50hz

8
ArduPlane/Log.cpp
View File

@ -318,6 +318,8 @@ struct PACKED log_Status {
float is_flying_probability;
uint8_t armed;
uint8_t safety;
bool is_crashed;
bool is_still;
};
void Plane::Log_Write_Status()
@ -329,7 +331,9 @@ void Plane::Log_Write_Status()
,is_flying_probability : isFlyingProbability
,armed : hal.util->get_soft_armed()
,safety : hal.util->safety_switch_state()
};
,is_crashed : auto_state.is_crashed
,is_still : plane.ins.is_still()
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
@ -484,7 +488,7 @@ static const struct LogStructure log_structure[] PROGMEM = {
{ LOG_ATRP_MSG, sizeof(AP_AutoTune::log_ATRP),
"ATRP", "QBBcfff", "TimeUS,Type,State,Servo,Demanded,Achieved,P" },
{ LOG_STATUS_MSG, sizeof(log_Status),
"STAT", "QBfBB", "TimeUS,isFlying,isFlyProb,Armed,Safety" },
"STAT", "QBfBBBB", "TimeUS,isFlying,isFlyProb,Armed,Safety,Crash,Still" },
#if OPTFLOW == ENABLED
{ LOG_OPTFLOW_MSG, sizeof(log_Optflow),
"OF", "QBffff", "TimeUS,Qual,flowX,flowY,bodyX,bodyY" },

8
ArduPlane/Parameters.cpp
View File

@ -973,6 +973,14 @@ const AP_Param::Info Plane::var_info[] PROGMEM = {
// @User: Standard
GSCALAR(trim_rc_at_start, "TRIM_RC_AT_START", 0),
// @Param: CRASH_DETECT
// @DisplayName: Crash Detection
// @Description: Automatically detect a crash during AUTO flight and perform the bitmask selected action(s). Disarm will turn off motor for saftey and to help against burning out ESC and motor. Setting the mode to manual will help save the servos from burning out by overexerting if the aircraft crashed in an odd orientation such as upsidedown.
// @Values: 0:Disabled,1:Disarm
// @Bitmask: 0:Disarm
// @User: Advanced
GSCALAR(crash_detection_enable, "CRASH_DETECT", 0),
// barometer ground calibration. The GND_ prefix is chosen for
// compatibility with previous releases of ArduPlane
// @Group: GND_

2
ArduPlane/Parameters.h
View File

@ -139,6 +139,7 @@ public:
k_param_rudder_only,
k_param_gcs3, // 93
k_param_gcs_pid_mask,
k_param_crash_detection_enable,
// 100: Arming parameters
k_param_arming = 100,
@ -338,6 +339,7 @@ public:
AP_Int8 rtl_autoland;
AP_Int8 trim_rc_at_start;
AP_Int8 crash_detection_enable;
// Feed-forward gains
//

9
ArduPlane/Plane.h
View File

@ -433,6 +433,9 @@ private:
// movement until altitude is reached
bool idle_mode:1;
// crash detection
bool is_crashed:1;
// used to 'wiggle' servos in idle mode to prevent them freezing
// at high altitudes
uint8_t idle_wiggle_stage;
@ -471,6 +474,9 @@ private:
// once landed, post some landing statistics to the GCS
bool post_landing_stats;
// time stamp of when we start flying while in auto mode in milliseconds
uint32_t started_flying_in_auto_ms;
} auto_state;
// true if we are in an auto-throttle mode, which means
@ -865,7 +871,8 @@ private:
void set_servos_idle(void);
void set_servos();
void update_aux();
void determine_is_flying(void);
void update_is_flying_5Hz(void);
void crash_detection_update(void);
void gcs_send_text_fmt(const prog_char_t *fmt, ...);
void handle_auto_mode(void);
void calc_throttle();

4
ArduPlane/commands_logic.cpp
View File

@ -22,6 +22,9 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
// except in a takeoff
auto_state.takeoff_complete = true;
// if we are still executing mission commands then we must be traveling around still
auto_state.is_crashed = false;
// if a go around had been commanded, clear it now.
auto_state.commanded_go_around = false;
@ -39,6 +42,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
switch(cmd.id) {
case MAV_CMD_NAV_TAKEOFF:
auto_state.is_crashed = false;
do_takeoff(cmd);
break;

5
ArduPlane/defines.h
View File

@ -185,4 +185,9 @@ enum {
ATT_CONTROL_APMCONTROL = 1
};
enum {
CRASH_DETECT_ACTION_BITMASK_DISABLED = 0,
CRASH_DETECT_ACTION_BITMASK_DISARM = (1<<0),
// note: next enum will be (1<<1), then (1<<2), then (1<<3)
};
#endif // _DEFINES_H

4
ArduPlane/system.cpp
View File

@ -347,6 +347,9 @@ void Plane::set_mode(enum FlightMode mode)
// zero locked course
steer_state.locked_course_err = 0;
// reset crash detection
auto_state.is_crashed = false;
// set mode
previous_mode = control_mode;
control_mode = mode;
@ -484,6 +487,7 @@ void Plane::exit_mode(enum FlightMode mode)
if (mission.state() == AP_Mission::MISSION_RUNNING) {
mission.stop();
}
auto_state.started_flying_in_auto_ms = 0;
}
}