mirror of https://github.com/ArduPilot/ardupilot
96 lines
3.0 KiB
C++
96 lines
3.0 KiB
C++
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
#include "Sub.h"
|
|
#define LAND_END_DEPTH -20 //Landed at 20cm depth
|
|
|
|
// counter to verify landings
|
|
static uint32_t land_detector_count = 0;
|
|
|
|
// run land and crash detectors
|
|
// called at MAIN_LOOP_RATE
|
|
void Sub::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();
|
|
|
|
crash_check();
|
|
}
|
|
|
|
// update_land_detector - checks if we have landed and updates the ap.land_complete flag
|
|
// called at MAIN_LOOP_RATE
|
|
void Sub::update_land_detector()
|
|
{
|
|
if(barometer.num_instances() > 1 && barometer.get_altitude() > LAND_END_DEPTH && ap.throttle_zero) {
|
|
set_land_complete(true);
|
|
} else {
|
|
set_land_complete(false);
|
|
}
|
|
}
|
|
|
|
void Sub::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;
|
|
}
|
|
|
|
// update_throttle_thr_mix - sets motors throttle_low_comp value depending upon vehicle state
|
|
// 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 Sub::update_throttle_thr_mix()
|
|
{
|
|
#if FRAME_CONFIG != HELI_FRAME
|
|
// if disarmed or landed prioritise throttle
|
|
if(!motors.armed() || ap.land_complete) {
|
|
motors.set_throttle_mix_min();
|
|
return;
|
|
}
|
|
|
|
if (mode_has_manual_throttle(control_mode)) {
|
|
// manual throttle
|
|
if(channel_throttle->control_in <= 0) {
|
|
motors.set_throttle_mix_min();
|
|
} else {
|
|
motors.set_throttle_mix_mid();
|
|
}
|
|
} 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 = (pythagorous2(angle_target.x, angle_target.y) > 1500.0f);
|
|
|
|
// check for large external disturbance - angle error over 30 degrees
|
|
const Vector3f angle_error = attitude_control.get_att_error_rot_vec_cd();
|
|
bool large_angle_error = (pythagorous2(angle_error.x, angle_error.y) > 3000.0f);
|
|
|
|
// check for large acceleration - falling or high turbulence
|
|
Vector3f accel_ef = ahrs.get_accel_ef_blended();
|
|
accel_ef.z += GRAVITY_MSS;
|
|
bool accel_moving = (accel_ef.length() > 3.0f);
|
|
|
|
// 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) {
|
|
motors.set_throttle_mix_max();
|
|
} else {
|
|
motors.set_throttle_mix_min();
|
|
}
|
|
}
|
|
#endif
|
|
}
|