ardupilot/ArduCopter/land_detector.cpp

180 lines
6.9 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, MAIN_LOOP_SECONDS);
update_land_detector();
#if PARACHUTE == ENABLED
// check parachute
parachute_check();
#endif
crash_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_throttle() > get_non_takeoff_throttle() && !motors->limit.throttle_lower && motors->rotor_runup_complete()) {
#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);
}
} else {
#if FRAME_CONFIG == HELI_FRAME
// check that collective pitch is on lower limit (should be constrained by LAND_COL_MIN)
bool motor_at_lower_limit = motors->limit.throttle_lower;
#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
// check that the airframe is not accelerating (not falling or breaking after fast forward flight)
bool accel_stationary = (land_accel_ef_filter.get().length() <= LAND_DETECTOR_ACCEL_MAX);
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;
// 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 (motor_at_lower_limit && accel_stationary && descent_rate_low && rangefinder_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){
Log_Write_Event(DATA_LAND_COMPLETE);
} else {
Log_Write_Event(DATA_NOT_LANDED);
}
ap.land_complete = b;
2016-10-13 07:40:13 -03:00
g2.stats.set_flying(!b);
2017-06-19 00:16:36 -03:00
// tell AHRS flying state
ahrs.set_likely_flying(!b);
// trigger disarm-on-land if configured
bool disarm_on_land_configured = (g.throttle_behavior & THR_BEHAVE_DISARM_ON_LAND_DETECT) != 0;
bool mode_disarms_on_land = mode_allows_arming(control_mode,false) && !mode_has_manual_throttle(control_mode);
if (ap.land_complete && motors->armed() && disarm_on_land_configured && mode_disarms_on_land) {
init_disarm_motors();
}
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) {
Log_Write_Event(DATA_LAND_COMPLETE_MAYBE);
}
ap.land_complete_maybe = b;
}
// update_throttle_thr_mix - 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_thr_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;
}
2015-04-16 01:27:06 -03:00
if (mode_has_manual_throttle(control_mode)) {
// manual throttle
ArduCopter: Fix up after refactoring RC_Channel class Further to refactor of RC_Channel class which included adding get_xx set_xx methods, change reads and writes to the public members to calls to get and set functionsss old public member(int16_t) get function -> int16_t set function (int16_t) (expression where c is an object of type RC_Channel) c.radio_in c.get_radio_in() c.set_radio_in(v) c.control_in c.get_control_in() c.set_control_in(v) c.servo_out c.get_servo_out() c.set_servo_out(v) c.pwm_out c.get_pwm_out() // use existing c.radio_out c.get_radio_out() c.set_radio_out(v) c.radio_max c.get_radio_max() c.set_radio_max(v) c.radio_min c.get_radio_min() c.set_radio_min(v) c.radio_trim c.get_radio_trim() c.set_radio_trim(v); c.min_max_configured() // return true if min and max are configured Because data members of RC_Channels are now private and so cannot be written directly some overloads are provided in the Plane classes to provide the old functionality new overload Plane::stick_mix_channel(RC_Channel *channel) which forwards to the previously existing void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const which forwards to (uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const; Rename functions RC_Channel_aux::set_radio_trim(Aux_servo_function_t function) to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function) RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value) to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value) Rationale: RC_Channel is a complicated class, which combines several functionalities dealing with stick inputs in pwm and logical units, logical and actual actuator outputs, unit conversion etc, etc The intent of this PR is to clarify existing use of the class. At the basic level it should now be possible to grep all places where private variable is set by searching for the set_xx function. (The wider purpose is to provide a more generic and logically simpler method of output mixing. This is a small step)
2016-05-08 05:46:59 -03:00
if(channel_throttle->get_control_in() <= 0) {
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 = (norm(angle_target.x, angle_target.y) > 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
2015-04-16 01:27:06 -03:00
Vector3f accel_ef = ahrs.get_accel_ef_blended();
accel_ef.z += GRAVITY_MSS;
bool accel_moving = (accel_ef.length() > LAND_CHECK_ACCEL_MOVING);
2015-04-16 01:27:06 -03:00
// check for requested decent
bool descent_not_demanded = pos_control->get_desired_velocity().z >= 0.0f;
if ( large_angle_request || large_angle_error || accel_moving || descent_not_demanded) {
attitude_control->set_throttle_mix_max();
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
}