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

ArduPlane: convert dos linefeeds to unix

Browse Source
This commit is contained in:
Lucas De Marchi 2015-10-24 19:52:39 -02:00 committed by Randy Mackay
parent 4d458833dc
commit bd0f0a7536
1 changed files with 245 additions and 245 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

490
ArduPlane/is_flying.cpp
View File

@ -1,245 +1,245 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Plane.h" #include "Plane.h"
/* /*
is_flying and crash detection logic is_flying and crash detection logic
*/ */
/* /*
Do we think we are flying? Do we think we are flying?
Probabilistic method where a bool is low-passed and considered a probability. Probabilistic method where a bool is low-passed and considered a probability.
*/ */
void Plane::update_is_flying_5Hz(void) void Plane::update_is_flying_5Hz(void)
{ {
float aspeed; float aspeed;
bool is_flying_bool; bool is_flying_bool;
uint32_t now_ms = hal.scheduler->millis(); uint32_t now_ms = hal.scheduler->millis();
uint32_t ground_speed_thresh_cm = (g.min_gndspeed_cm > 0) ? ((uint32_t)(g.min_gndspeed_cm*0.9f)) : 500; 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) && bool gps_confirmed_movement = (gps.status() >= AP_GPS::GPS_OK_FIX_3D) &&
(gps.ground_speed_cm() >= ground_speed_thresh_cm); (gps.ground_speed_cm() >= ground_speed_thresh_cm);
// airspeed at least 75% of stall speed? // airspeed at least 75% of stall speed?
bool airspeed_movement = ahrs.airspeed_estimate(&aspeed) && (aspeed >= (aparm.airspeed_min*0.75f)); bool airspeed_movement = ahrs.airspeed_estimate(&aspeed) && (aspeed >= (aparm.airspeed_min*0.75f));
if(arming.is_armed()) { if(arming.is_armed()) {
// when armed assuming flying and we need overwhelming evidence that we ARE NOT flying // 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 // 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 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 (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) (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) { if ((auto_state.last_flying_ms > 0) && gps_lost_recently) {
// we've flown before, remove GPS constraints temporarily and only use airspeed // we've flown before, remove GPS constraints temporarily and only use airspeed
is_flying_bool = airspeed_movement; // moving through the air is_flying_bool = airspeed_movement; // moving through the air
} else { } else {
// we've never flown yet, require good GPS movement // we've never flown yet, require good GPS movement
is_flying_bool = airspeed_movement || // moving through the air is_flying_bool = airspeed_movement || // moving through the air
gps_confirmed_movement; // locked and we're moving gps_confirmed_movement; // locked and we're moving
} }
if (control_mode == AUTO) { if (control_mode == AUTO) {
/* /*
make is_flying() more accurate during various auto modes make is_flying() more accurate during various auto modes
*/ */
switch (flight_stage) switch (flight_stage)
{ {
case AP_SpdHgtControl::FLIGHT_TAKEOFF: case AP_SpdHgtControl::FLIGHT_TAKEOFF:
// while on the ground, an uncalibrated airspeed sensor can drift to 7m/s so // 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 // 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! // we are definitely not flying, or at least for not much longer!
if (throttle_suppressed) { if (throttle_suppressed) {
is_flying_bool = false; is_flying_bool = false;
crash_state.is_crashed = false; crash_state.is_crashed = false;
} }
break; break;
case AP_SpdHgtControl::FLIGHT_LAND_ABORT: case AP_SpdHgtControl::FLIGHT_LAND_ABORT:
case AP_SpdHgtControl::FLIGHT_NORMAL: case AP_SpdHgtControl::FLIGHT_NORMAL:
// TODO: detect ground impacts // TODO: detect ground impacts
break; break;
case AP_SpdHgtControl::FLIGHT_LAND_APPROACH: case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
// TODO: detect ground impacts // TODO: detect ground impacts
if (fabsf(auto_state.sink_rate) > 0.2f) { if (fabsf(auto_state.sink_rate) > 0.2f) {
is_flying_bool = true; is_flying_bool = true;
} }
break; break;
case AP_SpdHgtControl::FLIGHT_LAND_FINAL: case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
break; break;
} // switch } // switch
} }
} else { } else {
// when disarmed assume not flying and need overwhelming evidence that we ARE flying // when disarmed assume not flying and need overwhelming evidence that we ARE flying
is_flying_bool = airspeed_movement && gps_confirmed_movement; is_flying_bool = airspeed_movement && gps_confirmed_movement;
if ((control_mode == AUTO) && if ((control_mode == AUTO) &&
((flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF) || ((flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF) ||
(flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL)) ) { (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL)) ) {
is_flying_bool = false; is_flying_bool = false;
} }
} }
// low-pass the result. // low-pass the result.
// coef=0.15f @ 5Hz takes 3.0s to go from 100% down to 10% (or 0% up to 90%) // 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); isFlyingProbability = (0.85f * isFlyingProbability) + (0.15f * (float)is_flying_bool);
/* /*
update last_flying_ms so we always know how long we have not update last_flying_ms so we always know how long we have not
been flying for. This helps for crash detection and auto-disarm been flying for. This helps for crash detection and auto-disarm
*/ */
bool new_is_flying = is_flying(); bool new_is_flying = is_flying();
// we are flying, note the time // we are flying, note the time
if (new_is_flying) { if (new_is_flying) {
auto_state.last_flying_ms = now_ms; auto_state.last_flying_ms = now_ms;
if (!previous_is_flying) { if (!previous_is_flying) {
// just started flying in any mode // just started flying in any mode
started_flying_ms = now_ms; started_flying_ms = now_ms;
} }
if ((control_mode == AUTO) && if ((control_mode == AUTO) &&
((auto_state.started_flying_in_auto_ms == 0) || !previous_is_flying) ) { ((auto_state.started_flying_in_auto_ms == 0) || !previous_is_flying) ) {
// We just started flying, note that time also // We just started flying, note that time also
auto_state.started_flying_in_auto_ms = now_ms; auto_state.started_flying_in_auto_ms = now_ms;
} }
} }
previous_is_flying = new_is_flying; previous_is_flying = new_is_flying;
if (should_log(MASK_LOG_MODE)) { if (should_log(MASK_LOG_MODE)) {
Log_Write_Status(); Log_Write_Status();
} }
} }
/* /*
return true if we think we are flying. This is a probabilistic return true if we think we are flying. This is a probabilistic
estimate, and needs to be used very carefully. Each use case needs estimate, and needs to be used very carefully. Each use case needs
to be thought about individually. to be thought about individually.
*/ */
bool Plane::is_flying(void) bool Plane::is_flying(void)
{ {
if (hal.util->get_soft_armed()) { if (hal.util->get_soft_armed()) {
// when armed, assume we're flying unless we probably aren't // when armed, assume we're flying unless we probably aren't
return (isFlyingProbability >= 0.1f); return (isFlyingProbability >= 0.1f);
} }
// when disarmed, assume we're not flying unless we probably are // when disarmed, assume we're not flying unless we probably are
return (isFlyingProbability >= 0.9f); return (isFlyingProbability >= 0.9f);
} }
/* /*
* Determine if we have crashed * Determine if we have crashed
*/ */
void Plane::crash_detection_update(void) void Plane::crash_detection_update(void)
{ {
if (control_mode != AUTO) if (control_mode != AUTO)
{ {
// crash detection is only available in AUTO mode // crash detection is only available in AUTO mode
crash_state.debounce_timer_ms = 0; crash_state.debounce_timer_ms = 0;
return; return;
} }
uint32_t now_ms = hal.scheduler->millis(); uint32_t now_ms = hal.scheduler->millis();
bool auto_launch_detected; bool auto_launch_detected;
bool crashed_near_land_waypoint = false; bool crashed_near_land_waypoint = false;
bool crashed = false; bool crashed = false;
bool been_auto_flying = (auto_state.started_flying_in_auto_ms > 0) && bool been_auto_flying = (auto_state.started_flying_in_auto_ms > 0) &&
(now_ms - auto_state.started_flying_in_auto_ms >= 2500); (now_ms - auto_state.started_flying_in_auto_ms >= 2500);
if (!is_flying()) if (!is_flying())
{ {
switch (flight_stage) switch (flight_stage)
{ {
case AP_SpdHgtControl::FLIGHT_TAKEOFF: case AP_SpdHgtControl::FLIGHT_TAKEOFF:
auto_launch_detected = !throttle_suppressed && (g.takeoff_throttle_min_accel > 0); auto_launch_detected = !throttle_suppressed && (g.takeoff_throttle_min_accel > 0);
if (been_auto_flying || // failed hand launch if (been_auto_flying || // failed hand launch
auto_launch_detected) { // threshold of been_auto_flying may not be met on auto-launches 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. // has launched but is no longer flying. That's a crash on takeoff.
crashed = true; crashed = true;
} }
break; break;
case AP_SpdHgtControl::FLIGHT_LAND_ABORT: case AP_SpdHgtControl::FLIGHT_LAND_ABORT:
case AP_SpdHgtControl::FLIGHT_NORMAL: case AP_SpdHgtControl::FLIGHT_NORMAL:
if (been_auto_flying) { if (been_auto_flying) {
crashed = true; crashed = true;
} }
// TODO: handle auto missions without NAV_TAKEOFF mission cmd // TODO: handle auto missions without NAV_TAKEOFF mission cmd
break; break;
case AP_SpdHgtControl::FLIGHT_LAND_APPROACH: case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
if (been_auto_flying) { if (been_auto_flying) {
crashed = true; crashed = true;
} }
// when altitude gets low, we automatically progress to FLIGHT_LAND_FINAL // when altitude gets low, we automatically progress to FLIGHT_LAND_FINAL
// so ground crashes most likely can not be triggered from here. However, // so ground crashes most likely can not be triggered from here. However,
// a crash into a tree, for example, would be. // a crash into a tree, for example, would be.
break; break;
case AP_SpdHgtControl::FLIGHT_LAND_FINAL: case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
// We should be nice and level-ish in this flight stage. If not, we most // 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 // 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. // 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 // Declare a crash if we are oriented more that 60deg in pitch or roll
if (been_auto_flying && if (been_auto_flying &&
!crash_state.checkHardLanding && // only check once !crash_state.checkHardLanding && // only check once
(fabsf(ahrs.roll_sensor) > 6000 || fabsf(ahrs.pitch_sensor) > 6000)) { (fabsf(ahrs.roll_sensor) > 6000 || fabsf(ahrs.pitch_sensor) > 6000)) {
crashed = true; crashed = true;
// did we "crash" within 75m of the landing location? Probably just a hard landing // did we "crash" within 75m of the landing location? Probably just a hard landing
crashed_near_land_waypoint = crashed_near_land_waypoint =
get_distance(current_loc, mission.get_current_nav_cmd().content.location) < 75; 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 // 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 // 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. // this logic is still running and detects the plane is on its side as you carry it.
crash_state.debounce_timer_ms = now_ms + 2500; crash_state.debounce_timer_ms = now_ms + 2500;
} }
crash_state.checkHardLanding = true; crash_state.checkHardLanding = true;
break; break;
} // switch } // switch
} else { } else {
crash_state.checkHardLanding = false; crash_state.checkHardLanding = false;
} }
if (!crashed) { if (!crashed) {
// reset timer // reset timer
crash_state.debounce_timer_ms = 0; crash_state.debounce_timer_ms = 0;
} else if (crash_state.debounce_timer_ms == 0) { } else if (crash_state.debounce_timer_ms == 0) {
// start timer // start timer
crash_state.debounce_timer_ms = now_ms; crash_state.debounce_timer_ms = now_ms;
} else if ((now_ms - crash_state.debounce_timer_ms >= 2500) && !crash_state.is_crashed) { } else if ((now_ms - crash_state.debounce_timer_ms >= 2500) && !crash_state.is_crashed) {
crash_state.is_crashed = true; crash_state.is_crashed = true;
if (g.crash_detection_enable == CRASH_DETECT_ACTION_BITMASK_DISABLED) { if (g.crash_detection_enable == CRASH_DETECT_ACTION_BITMASK_DISABLED) {
if (crashed_near_land_waypoint) { if (crashed_near_land_waypoint) {
gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Hard Landing Detected - no action taken")); gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Hard Landing Detected - no action taken"));
} else { } else {
gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Crash Detected - no action taken")); gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Crash Detected - no action taken"));
} }
} }
else { else {
if (g.crash_detection_enable & CRASH_DETECT_ACTION_BITMASK_DISARM) { if (g.crash_detection_enable & CRASH_DETECT_ACTION_BITMASK_DISARM) {
disarm_motors(); disarm_motors();
} }
auto_state.land_complete = true; auto_state.land_complete = true;
if (crashed_near_land_waypoint) { if (crashed_near_land_waypoint) {
gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Hard Landing Detected")); gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Hard Landing Detected"));
} else { } else {
gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Crash Detected")); gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Crash Detected"));
} }
} }
} }
} }