ardupilot/ArduCopter/land_detector.cpp

206 lines
8.4 KiB
C++
Raw Normal View History

#include "Copter.h"
// Code to detect a crash main ArduCopter code
#define LAND_CHECK_ANGLE_ERROR_DEG 30.0f // maximum angle error to be considered landing
#define LAND_CHECK_LARGE_ANGLE_CD 1500.0f // maximum angle target to be considered landing
#define LAND_CHECK_ACCEL_MOVING 3.0f // maximum acceleration after subtracting gravity
2015-04-16 01:27:06 -03:00
// counter to verify landings
2015-05-06 01:38:38 -03:00
static uint32_t land_detector_count = 0;
2015-04-16 01:27:06 -03:00
// run land and crash detectors
// called at MAIN_LOOP_RATE
void Copter::update_land_and_crash_detectors()
{
// update 1hz filtered acceleration
Vector3f accel_ef = ahrs.get_accel_ef_blended();
accel_ef.z += GRAVITY_MSS;
land_accel_ef_filter.apply(accel_ef, scheduler.get_loop_period_s());
update_land_detector();
#if PARACHUTE == ENABLED
// check parachute
parachute_check();
#endif
crash_check();
2018-08-12 11:19:47 -03:00
thrust_loss_check();
2020-11-07 14:33:10 -04:00
yaw_imbalance_check();
}
2015-04-16 01:27:06 -03:00
// update_land_detector - checks if we have landed and updates the ap.land_complete flag
2015-04-16 15:35:17 -03:00
// called at MAIN_LOOP_RATE
void Copter::update_land_detector()
2015-04-16 01:27:06 -03:00
{
// land detector can not use the following sensors because they are unreliable during landing
// barometer altitude : ground effect can cause errors larger than 4m
// EKF vertical velocity or altitude : poor barometer and large acceleration from ground impact
// earth frame angle or angle error : landing on an uneven surface will force the airframe to match the ground angle
// gyro output : on uneven surface the airframe may rock back an forth after landing
// range finder : tend to be problematic at very short distances
// input throttle : in slow land the input throttle may be only slightly less than hover
if (!motors->armed()) {
2015-05-06 01:38:38 -03:00
// if disarmed, always landed.
set_land_complete(true);
} else if (ap.land_complete) {
#if FRAME_CONFIG == HELI_FRAME
// if rotor speed and collective pitch are high then clear landing flag
if (motors->get_takeoff_collective() && motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
#else
2015-05-06 01:38:38 -03:00
// if throttle output is high then clear landing flag
if (motors->get_throttle() > get_non_takeoff_throttle()) {
#endif
2015-05-06 01:38:38 -03:00
set_land_complete(false);
}
2019-09-28 10:37:57 -03:00
} else if (standby_active) {
// land detector will not run in standby mode
land_detector_count = 0;
2015-05-06 01:38:38 -03:00
} else {
#if FRAME_CONFIG == HELI_FRAME
// check for both manual collective modes and modes that use altitude hold. For manual collective (called throttle
// because multi's use throttle), check that collective pitch is below mid collective (zero thrust) position. For modes
// that use altitude hold, check that the pilot is commanding a descent and collective is at min allowed for altitude hold modes.
bool motor_at_lower_limit = ((flightmode->has_manual_throttle() && motors->get_below_mid_collective() && fabsf(ahrs.get_roll()) < M_PI/2.0f)
|| (motors->limit.throttle_lower && pos_control->get_vel_desired_cms().z < 0.0f));
#else
// check that the average throttle output is near minimum (less than 12.5% hover throttle)
bool motor_at_lower_limit = motors->limit.throttle_lower && attitude_control->is_throttle_mix_min();
#endif
uint8_t land_detector_scalar = 1;
#if LANDING_GEAR_ENABLED == ENABLED
if (landinggear.get_wow_state() != AP_LandingGear::LG_WOW_UNKNOWN) {
// we have a WoW sensor so lets loosen the strictness of the landing detector
land_detector_scalar = 2;
}
#endif
// check that the airframe is not accelerating (not falling or braking after fast forward flight)
bool accel_stationary = (land_accel_ef_filter.get().length() <= LAND_DETECTOR_ACCEL_MAX * land_detector_scalar);
2015-04-16 01:27:06 -03:00
// check that vertical speed is within 1m/s of zero
bool descent_rate_low = fabsf(inertial_nav.get_velocity_z()) < 100 * land_detector_scalar;
// if we have a healthy rangefinder only allow landing detection below 2 meters
bool rangefinder_check = (!rangefinder_alt_ok() || rangefinder_state.alt_cm_filt.get() < LAND_RANGEFINDER_MIN_ALT_CM);
// if we have weight on wheels (WoW) or ambiguous unknown. never no WoW
#if LANDING_GEAR_ENABLED == ENABLED
const bool WoW_check = (landinggear.get_wow_state() == AP_LandingGear::LG_WOW || landinggear.get_wow_state() == AP_LandingGear::LG_WOW_UNKNOWN);
#else
const bool WoW_check = true;
#endif
if (motor_at_lower_limit && accel_stationary && descent_rate_low && rangefinder_check && WoW_check) {
2015-05-06 01:38:38 -03:00
// landed criteria met - increment the counter and check if we've triggered
if( land_detector_count < ((float)LAND_DETECTOR_TRIGGER_SEC)*scheduler.get_loop_rate_hz()) {
2015-05-06 01:38:38 -03:00
land_detector_count++;
} else {
2015-04-16 01:27:06 -03:00
set_land_complete(true);
}
} else {
// we've sensed movement up or down so reset land_detector
2015-05-06 01:38:38 -03:00
land_detector_count = 0;
2015-04-16 01:27:06 -03:00
}
}
set_land_complete_maybe(ap.land_complete || (land_detector_count >= LAND_DETECTOR_MAYBE_TRIGGER_SEC*scheduler.get_loop_rate_hz()));
2015-05-06 01:38:38 -03:00
}
// set land_complete flag and disarm motors if disarm-on-land is configured
2015-05-06 01:38:38 -03:00
void Copter::set_land_complete(bool b)
{
// if no change, exit immediately
if( ap.land_complete == b )
return;
land_detector_count = 0;
if(b){
AP::logger().Write_Event(LogEvent::LAND_COMPLETE);
2015-05-06 01:38:38 -03:00
} else {
AP::logger().Write_Event(LogEvent::NOT_LANDED);
2015-05-06 01:38:38 -03:00
}
ap.land_complete = b;
2018-02-28 08:20:57 -04:00
#if STATS_ENABLED == ENABLED
2016-10-13 07:40:13 -03:00
g2.stats.set_flying(!b);
2018-02-28 08:20:57 -04:00
#endif
2016-10-13 07:40:13 -03:00
2017-06-19 00:16:36 -03:00
// tell AHRS flying state
2020-01-06 19:06:03 -04:00
set_likely_flying(!b);
2017-06-19 00:16:36 -03:00
// trigger disarm-on-land if configured
bool disarm_on_land_configured = (g.throttle_behavior & THR_BEHAVE_DISARM_ON_LAND_DETECT) != 0;
const bool mode_disarms_on_land = flightmode->allows_arming(AP_Arming::Method::LANDING) && !flightmode->has_manual_throttle();
if (ap.land_complete && motors->armed() && disarm_on_land_configured && mode_disarms_on_land) {
2020-02-21 09:09:57 -04:00
arming.disarm(AP_Arming::Method::LANDED);
}
2015-04-16 01:27:06 -03:00
}
// set land complete maybe flag
void Copter::set_land_complete_maybe(bool b)
{
// if no change, exit immediately
if (ap.land_complete_maybe == b)
return;
if (b) {
AP::logger().Write_Event(LogEvent::LAND_COMPLETE_MAYBE);
}
ap.land_complete_maybe = b;
}
// sets motors throttle_low_comp value depending upon vehicle state
2015-04-16 01:27:06 -03:00
// low values favour pilot/autopilot throttle over attitude control, high values favour attitude control over throttle
// has no effect when throttle is above hover throttle
void Copter::update_throttle_mix()
2015-04-16 01:27:06 -03:00
{
#if FRAME_CONFIG != HELI_FRAME
// if disarmed or landed prioritise throttle
if (!motors->armed() || ap.land_complete) {
attitude_control->set_throttle_mix_min();
return;
}
if (flightmode->has_manual_throttle()) {
2015-04-16 01:27:06 -03:00
// manual throttle
2020-05-24 08:26:00 -03:00
if (channel_throttle->get_control_in() <= 0 || air_mode == AirMode::AIRMODE_DISABLED) {
attitude_control->set_throttle_mix_min();
2015-04-16 01:27:06 -03:00
} else {
attitude_control->set_throttle_mix_man();
2015-04-16 01:27:06 -03:00
}
} else {
// autopilot controlled throttle
// check for aggressive flight requests - requested roll or pitch angle below 15 degrees
const Vector3f angle_target = attitude_control->get_att_target_euler_cd();
bool large_angle_request = angle_target.xy().length() > LAND_CHECK_LARGE_ANGLE_CD;
2015-04-16 01:27:06 -03:00
// check for large external disturbance - angle error over 30 degrees
const float angle_error = attitude_control->get_att_error_angle_deg();
bool large_angle_error = (angle_error > LAND_CHECK_ANGLE_ERROR_DEG);
2015-04-16 01:27:06 -03:00
// check for large acceleration - falling or high turbulence
const bool accel_moving = (land_accel_ef_filter.get().length() > LAND_CHECK_ACCEL_MOVING);
2015-04-16 01:27:06 -03:00
2021-05-19 11:07:38 -03:00
// check for requested descent
2021-05-11 01:42:02 -03:00
bool descent_not_demanded = pos_control->get_vel_desired_cms().z >= 0.0f;
// check if landing
const bool landing = flightmode->is_landing();
if ((large_angle_request && !landing) || large_angle_error || accel_moving || descent_not_demanded) {
attitude_control->set_throttle_mix_max(pos_control->get_vel_z_control_ratio());
2015-04-16 01:27:06 -03:00
} else {
attitude_control->set_throttle_mix_min();
2015-04-16 01:27:06 -03:00
}
}
#endif
2015-04-16 01:27:06 -03:00
}