fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 19:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

land_detector: multicopter ground contact fixes, improvements, and cleanup

Browse Source 
- if "landed" and "maybe_landed" states are false then both the "hit_ground" and the "low_thrust" condition need to be true in order to detect landing
 - ground contact MC NAN setpoint workaround
 - ground contact additionally check acceleration setpoint
 - schedule with vehicle_local_position updates (most updates require valid local position)
 - don't allow LNDMC_Z_VEL_MAX to exceed MPC_LAND_SPEED
 - ground contact horizontal movement checks default to failed if estimates aren't available
This commit is contained in:
Daniel Agar
2020-08-02 13:37:23 -04:00
committed by GitHub
parent 63a23957b1
commit f5f2897486
13 changed files with 223 additions and 321 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/land_detector/MulticopterLandDetector.cpp
+130 -123
View File
@@ -67,32 +67,40 @@
#include "MulticopterLandDetector.h"
using matrix::Vector2f;
using matrix::Vector3f;
namespace land_detector
{
MulticopterLandDetector::MulticopterLandDetector()
{
_paramHandle.landSpeed = param_find("MPC_LAND_SPEED");
_paramHandle.landSpeed = param_find("MPC_LAND_SPEED");
_paramHandle.minManThrottle = param_find("MPC_MANTHR_MIN");
_paramHandle.minThrottle = param_find("MPC_THR_MIN");
_paramHandle.minThrottle = param_find("MPC_THR_MIN");
_paramHandle.useHoverThrustEstimate = param_find("MPC_USE_HTE");
_paramHandle.hoverThrottle = param_find("MPC_THR_HOVER");
_paramHandle.hoverThrottle = param_find("MPC_THR_HOVER");
// Use Trigger time when transitioning from in-air (false) to landed (true) / ground contact (true).
_freefall_hysteresis.set_hysteresis_time_from(false, FREEFALL_TRIGGER_TIME_US);
_ground_contact_hysteresis.set_hysteresis_time_from(false, GROUND_CONTACT_TRIGGER_TIME_US);
_landed_hysteresis.set_hysteresis_time_from(false, LAND_DETECTOR_TRIGGER_TIME_US);
_maybe_landed_hysteresis.set_hysteresis_time_from(false, MAYBE_LAND_DETECTOR_TRIGGER_TIME_US);
_landed_hysteresis.set_hysteresis_time_from(false, LAND_DETECTOR_TRIGGER_TIME_US);
}
void MulticopterLandDetector::_update_topics()
{
LandDetector::_update_topics();
actuator_controls_s actuator_controls;
_actuator_controls_sub.update(&_actuator_controls);
_vehicle_angular_velocity_sub.update(&_vehicle_angular_velocity);
_vehicle_control_mode_sub.update(&_vehicle_control_mode);
_vehicle_local_position_setpoint_sub.update(&_vehicle_local_position_setpoint);
if (_actuator_controls_sub.update(&actuator_controls)) {
_actuator_controls_throttle = actuator_controls.control[actuator_controls_s::INDEX_THROTTLE];
}
vehicle_control_mode_s vehicle_control_mode;
if (_vehicle_control_mode_sub.update(&vehicle_control_mode)) {
_flag_control_climb_rate_enabled = vehicle_control_mode.flag_control_climb_rate_enabled;
}
if (_params.useHoverThrustEstimate) {
hover_thrust_estimate_s hte;
@@ -105,14 +113,18 @@ void MulticopterLandDetector::_update_topics()
void MulticopterLandDetector::_update_params()
{
LandDetector::_update_params();
_freefall_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1e6f * _param_lndmc_ffall_ttri.get()));
param_get(_paramHandle.minThrottle, &_params.minThrottle);
param_get(_paramHandle.minManThrottle, &_params.minManThrottle);
param_get(_paramHandle.landSpeed, &_params.landSpeed);
if (_param_lndmc_z_vel_max.get() > _params.landSpeed) {
PX4_ERR("LNDMC_Z_VEL_MAX > MPC_LAND_SPEED, updating %.3f -> %.3f",
(double)_param_lndmc_z_vel_max.get(), (double)_params.landSpeed);
_param_lndmc_z_vel_max.set(_params.landSpeed);
_param_lndmc_z_vel_max.commit_no_notification();
}
int32_t use_hover_thrust_estimate = 0;
param_get(_paramHandle.useHoverThrustEstimate, &use_hover_thrust_estimate);
_params.useHoverThrustEstimate = (use_hover_thrust_estimate == 1);
@@ -131,82 +143,120 @@ void MulticopterLandDetector::_update_params()
bool MulticopterLandDetector::_get_freefall_state()
{
if (_param_lndmc_ffall_thr.get() < 0.1f ||
_param_lndmc_ffall_thr.get() > 10.0f) { //if parameter is set to zero or invalid, disable free-fall detection.
return false;
}
if (_vehicle_acceleration.timestamp == 0) {
// _sensors is not valid yet, we have to assume we're not falling.
return false;
}
// norm of specific force. Should be close to 9.8 m/s^2 when landed.
const matrix::Vector3f accel{_vehicle_acceleration.xyz};
return (accel.norm() < _param_lndmc_ffall_thr.get()); // true if we are currently falling
return _acceleration.norm() < 2.f;
}
bool MulticopterLandDetector::_get_ground_contact_state()
{
// When not armed, consider to have ground-contact
if (!_actuator_armed.armed) {
return true;
const bool lpos_available = (hrt_elapsed_time(&_vehicle_local_position.timestamp) < 1_s);
// land speed threshold, 90% of MPC_LAND_SPEED
const float land_speed_threshold = 0.9f * math::max(_params.landSpeed, 0.1f);
bool vertical_movement = true;
if (lpos_available && _vehicle_local_position.v_z_valid) {
// Check if we are moving vertically - this might see a spike after arming due to
// throttle-up vibration. If accelerating fast the throttle thresholds will still give
// an accurate in-air indication.
float max_climb_rate = math::min(land_speed_threshold * 0.5f, _param_lndmc_z_vel_max.get());
if (hrt_elapsed_time(&_landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) {
// Widen acceptance thresholds for landed state right after arming
// so that motor spool-up and other effects do not trigger false negatives.
max_climb_rate = _param_lndmc_z_vel_max.get() * 2.5f;
}
vertical_movement = (fabsf(_vehicle_local_position.vz) > max_climb_rate);
}
// land speed threshold
float land_speed_threshold = 0.9f * math::max(_params.landSpeed, 0.1f);
// Check if we are moving vertically - this might see a spike after arming due to
// throttle-up vibration. If accelerating fast the throttle thresholds will still give
// an accurate in-air indication.
bool vertical_movement = false;
if (hrt_elapsed_time(&_landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) {
// Widen acceptance thresholds for landed state right after arming
// so that motor spool-up and other effects do not trigger false negatives.
vertical_movement = fabsf(_vehicle_local_position.vz) > _param_lndmc_z_vel_max.get() * 2.5f;
} else {
// Adjust max_climb_rate if land_speed is lower than 2x max_climb_rate
float max_climb_rate = ((land_speed_threshold * 0.5f) < _param_lndmc_z_vel_max.get()) ? (0.5f * land_speed_threshold) :
_param_lndmc_z_vel_max.get();
vertical_movement = fabsf(_vehicle_local_position.vz) > max_climb_rate;
}
// Check if we are moving horizontally.
_horizontal_movement = sqrtf(_vehicle_local_position.vx * _vehicle_local_position.vx
+ _vehicle_local_position.vy * _vehicle_local_position.vy) > _param_lndmc_xy_vel_max.get();
if (lpos_available && _vehicle_local_position.v_xy_valid) {
const Vector2f v_xy{_vehicle_local_position.vx, _vehicle_local_position.vy};
_horizontal_movement = v_xy.longerThan(_param_lndmc_xy_vel_max.get());
} else {
_horizontal_movement = false; // not known
}
// low thrust: 30% of throttle range between min and hover
const float sys_low_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * 0.3f;
bool ground_contact = (_actuator_controls_throttle <= sys_low_throttle);
// if we have a valid velocity setpoint and the vehicle is demanded to go down but no vertical movement present,
// we then can assume that the vehicle hit ground
_in_descend = _is_climb_rate_enabled()
&& (_vehicle_local_position_setpoint.vz >= land_speed_threshold);
bool hit_ground = _in_descend && !vertical_movement;
if (_flag_control_climb_rate_enabled) {
vehicle_local_position_setpoint_s vehicle_local_position_setpoint;
if (_vehicle_local_position_setpoint_sub.update(&vehicle_local_position_setpoint)) {
// setpoints can briefly be NAN to signal resets, TODO: fix in multicopter position controller
const bool descend_vel_sp = PX4_ISFINITE(vehicle_local_position_setpoint.vz)
&& (vehicle_local_position_setpoint.vz >= land_speed_threshold);
const bool descend_acc_sp = PX4_ISFINITE(vehicle_local_position_setpoint.acceleration[2])
&& (vehicle_local_position_setpoint.acceleration[2] >= 100.f);
_in_descend = descend_vel_sp || descend_acc_sp;
}
// ground contact requires commanded descent until landed
if (!_maybe_landed_hysteresis.get_state() && !_landed_hysteresis.get_state()) {
ground_contact &= _in_descend;
}
} else {
_in_descend = false;
}
// When not armed, consider to have ground-contact
if (!_armed) {
return true;
}
// TODO: we need an accelerometer based check for vertical movement for flying without GPS
return (_has_low_thrust() || hit_ground) && (!_horizontal_movement || !_has_position_lock())
&& (!vertical_movement || !_has_altitude_lock());
return ground_contact && !_horizontal_movement && !vertical_movement;
}
bool MulticopterLandDetector::_get_maybe_landed_state()
{
// When not armed, consider to be maybe-landed
if (!_actuator_armed.armed) {
if (!_armed) {
return true;
}
if (_has_minimal_thrust()) {
if (_min_trust_start == 0) {
_min_trust_start = hrt_absolute_time();
// minimal throttle: initially 10% of throttle range between min and hover
float sys_min_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * 0.1f;
// Determine the system min throttle based on flight mode
if (!_flag_control_climb_rate_enabled) {
sys_min_throttle = (_params.minManThrottle + 0.01f);
}
// Check if thrust output is less than the minimum throttle.
if (_actuator_controls_throttle <= sys_min_throttle) {
if (_min_thrust_start == 0) {
_min_thrust_start = hrt_absolute_time();
}
} else {
_min_trust_start = 0;
_min_thrust_start = 0;
return false;
}
float landThresholdFactor = 1.0f;
if (_freefall_hysteresis.get_state()) {
return false;
}
float landThresholdFactor = 1.f;
// Widen acceptance thresholds for landed state right after landed
if (hrt_elapsed_time(&_landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) {
@@ -214,32 +264,26 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
}
// Next look if all rotation angles are not moving.
float max_rotation_scaled = math::radians(_param_lndmc_rot_max.get()) * landThresholdFactor;
vehicle_angular_velocity_s vehicle_angular_velocity{};
_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity);
const Vector2f angular_velocity{vehicle_angular_velocity.xyz[0], vehicle_angular_velocity.xyz[1]};
const float max_rotation_scaled = math::radians(_param_lndmc_rot_max.get()) * landThresholdFactor;
bool rotating = (fabsf(_vehicle_angular_velocity.xyz[0]) > max_rotation_scaled) ||
(fabsf(_vehicle_angular_velocity.xyz[1]) > max_rotation_scaled) ||
(fabsf(_vehicle_angular_velocity.xyz[2]) > max_rotation_scaled);
// Return status based on armed state and throttle if no position lock is available.
if (!_has_altitude_lock() && !rotating) {
// The system has minimum trust set (manual or in failsafe)
// if this persists for 8 seconds AND the drone is not
// falling consider it to be landed. This should even sustain
// quite acrobatic flight.
return (_min_trust_start > 0) && (hrt_elapsed_time(&_min_trust_start) > 8_s);
if (angular_velocity.norm() > max_rotation_scaled) {
return false;
}
// Ground contact, no thrust and no movement -> landed
return _ground_contact_hysteresis.get_state() && _has_minimal_thrust() && !rotating;
// If vertical velocity is available: ground contact, no thrust, no movement -> landed
if ((hrt_elapsed_time(&_vehicle_local_position.timestamp) < 1_s) && _vehicle_local_position.v_z_valid) {
return _ground_contact_hysteresis.get_state();
}
// Otherwise, landed if the system has minimum thrust (manual or in failsafe) and no rotation for at least 8 seconds
return (_min_thrust_start > 0) && (hrt_elapsed_time(&_min_thrust_start) > 8_s);
}
bool MulticopterLandDetector::_get_landed_state()
{
// When not armed, consider to be landed
if (!_actuator_armed.armed) {
return true;
}
// reset the landed_time
if (!_maybe_landed_hysteresis.get_state()) {
_landed_time = 0;
@@ -248,6 +292,11 @@ bool MulticopterLandDetector::_get_landed_state()
_landed_time = hrt_absolute_time();
}
// When not armed, consider to be landed
if (!_armed) {
return true;
}
// if we have maybe_landed, the mc_pos_control goes into idle (thrust_sp = 0.0)
// therefore check if all other condition of the landed state remain true
return _maybe_landed_hysteresis.get_state();
@@ -263,48 +312,6 @@ float MulticopterLandDetector::_get_max_altitude()
}
}
bool MulticopterLandDetector::_has_altitude_lock()
{
return (hrt_elapsed_time(&_vehicle_local_position.timestamp) < 1_s) && _vehicle_local_position.z_valid;
}
bool MulticopterLandDetector::_has_position_lock()
{
return _has_altitude_lock() && _vehicle_local_position.xy_valid;
}
bool MulticopterLandDetector::_is_climb_rate_enabled()
{
bool has_updated = (hrt_elapsed_time(&_vehicle_local_position_setpoint.timestamp) < 1_s);
return (_vehicle_control_mode.flag_control_climb_rate_enabled && has_updated
&& PX4_ISFINITE(_vehicle_local_position_setpoint.vz));
}
bool MulticopterLandDetector::_has_low_thrust()
{
// 30% of throttle range between min and hover
float sys_min_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) *
_param_lndmc_low_t_thr.get();
// Check if thrust output is less than the minimum auto throttle param.
return _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] <= sys_min_throttle;
}
bool MulticopterLandDetector::_has_minimal_thrust()
{
// 10% of throttle range between min and hover once we entered ground contact
float sys_min_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * 0.1f;
// Determine the system min throttle based on flight mode
if (!_vehicle_control_mode.flag_control_climb_rate_enabled) {
sys_min_throttle = (_params.minManThrottle + 0.01f);
}
// Check if thrust output is less than the minimum auto throttle param.
return _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] <= sys_min_throttle;
}
bool MulticopterLandDetector::_get_ground_effect_state()
{
return _in_descend && !_horizontal_movement;