mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
land_detector: multicopter ground contact fixes, improvements, and cleanup
- 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
GitHub
parent
63a23957b1
commit
f5f2897486
@ -1575,7 +1575,6 @@ Commander::run()
|
||||
/* Update land detector */
|
||||
if (_land_detector_sub.updated()) {
|
||||
_was_landed = _land_detector.landed;
|
||||
bool was_falling = _land_detector.freefall;
|
||||
_land_detector_sub.copy(&_land_detector);
|
||||
|
||||
// Only take actions if armed
|
||||
@ -1596,12 +1595,6 @@ Commander::run()
|
||||
_lvel_probation_time_us = _param_com_pos_fs_prob.get() * 1_s;
|
||||
}
|
||||
}
|
||||
|
||||
if (was_falling != _land_detector.freefall) {
|
||||
if (_land_detector.freefall) {
|
||||
mavlink_log_info(&mavlink_log_pub, "Freefall detected");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -51,16 +51,10 @@ FixedwingLandDetector::FixedwingLandDetector()
|
||||
_landed_hysteresis.set_hysteresis_time_from(true, FLYING_TRIGGER_TIME_US);
|
||||
}
|
||||
|
||||
void FixedwingLandDetector::_update_topics()
|
||||
{
|
||||
LandDetector::_update_topics();
|
||||
_airspeed_validated_sub.update(&_airspeed_validated);
|
||||
}
|
||||
|
||||
bool FixedwingLandDetector::_get_landed_state()
|
||||
{
|
||||
// Only trigger flight conditions if we are armed.
|
||||
if (!_actuator_armed.armed) {
|
||||
if (!_armed) {
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -83,19 +77,20 @@ bool FixedwingLandDetector::_get_landed_state()
|
||||
_velocity_z_filtered = val;
|
||||
}
|
||||
|
||||
airspeed_validated_s airspeed_validated{};
|
||||
_airspeed_validated_sub.copy(&airspeed_validated);
|
||||
|
||||
// set _airspeed_filtered to 0 if airspeed data is invalid
|
||||
if (!PX4_ISFINITE(_airspeed_validated.true_airspeed_m_s) || hrt_elapsed_time(&_airspeed_validated.timestamp) > 1_s) {
|
||||
if (!PX4_ISFINITE(airspeed_validated.true_airspeed_m_s) || hrt_elapsed_time(&airspeed_validated.timestamp) > 1_s) {
|
||||
_airspeed_filtered = 0.0f;
|
||||
|
||||
} else {
|
||||
_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed_validated.true_airspeed_m_s;
|
||||
_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * airspeed_validated.true_airspeed_m_s;
|
||||
}
|
||||
|
||||
// A leaking lowpass prevents biases from building up, but
|
||||
// gives a mostly correct response for short impulses.
|
||||
const matrix::Vector3f accel{_vehicle_acceleration.xyz};
|
||||
const float acc_hor = sqrtf(accel(0) * accel(0) + accel(1) * accel(1));
|
||||
|
||||
const float acc_hor = matrix::Vector2f(_acceleration).norm();
|
||||
_xy_accel_filtered = _xy_accel_filtered * 0.8f + acc_hor * 0.18f;
|
||||
|
||||
// Crude land detector for fixedwing.
|
||||
|
||||
@ -61,7 +61,6 @@ public:
|
||||
protected:
|
||||
|
||||
bool _get_landed_state() override;
|
||||
void _update_topics() override;
|
||||
|
||||
private:
|
||||
|
||||
@ -71,8 +70,6 @@ private:
|
||||
|
||||
uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
|
||||
|
||||
airspeed_validated_s _airspeed_validated{};
|
||||
|
||||
float _airspeed_filtered{0.0f};
|
||||
float _velocity_xy_filtered{0.0f};
|
||||
float _velocity_z_filtered{0.0f};
|
||||
|
||||
@ -57,21 +57,52 @@ LandDetector::~LandDetector()
|
||||
|
||||
void LandDetector::start()
|
||||
{
|
||||
_update_params();
|
||||
ScheduleOnInterval(LAND_DETECTOR_UPDATE_INTERVAL);
|
||||
ScheduleDelayed(50_ms);
|
||||
_vehicle_local_position_sub.registerCallback();
|
||||
}
|
||||
|
||||
void LandDetector::Run()
|
||||
{
|
||||
// push backup schedule
|
||||
ScheduleDelayed(50_ms);
|
||||
|
||||
perf_begin(_cycle_perf);
|
||||
|
||||
if (_parameter_update_sub.updated()) {
|
||||
if (_parameter_update_sub.updated() || (_land_detected.timestamp == 0)) {
|
||||
parameter_update_s param_update;
|
||||
_parameter_update_sub.copy(¶m_update);
|
||||
|
||||
updateParams();
|
||||
_update_params();
|
||||
|
||||
_total_flight_time = static_cast<uint64_t>(_param_total_flight_time_high.get()) << 32;
|
||||
_total_flight_time |= static_cast<uint32_t>(_param_total_flight_time_low.get());
|
||||
}
|
||||
|
||||
_actuator_armed_sub.update(&_actuator_armed);
|
||||
actuator_armed_s actuator_armed;
|
||||
|
||||
if (_actuator_armed_sub.update(&actuator_armed)) {
|
||||
_armed = actuator_armed.armed;
|
||||
}
|
||||
|
||||
vehicle_acceleration_s vehicle_acceleration;
|
||||
|
||||
if (_vehicle_acceleration_sub.update(&vehicle_acceleration)) {
|
||||
_acceleration = matrix::Vector3f{vehicle_acceleration.xyz};
|
||||
}
|
||||
|
||||
_vehicle_local_position_sub.update(&_vehicle_local_position);
|
||||
_vehicle_status_sub.update(&_vehicle_status);
|
||||
|
||||
_update_topics();
|
||||
_update_state();
|
||||
|
||||
const hrt_abstime now_us = hrt_absolute_time();
|
||||
|
||||
_freefall_hysteresis.set_state_and_update(_get_freefall_state(), now_us);
|
||||
_ground_contact_hysteresis.set_state_and_update(_get_ground_contact_state(), now_us);
|
||||
_maybe_landed_hysteresis.set_state_and_update(_get_maybe_landed_state(), now_us);
|
||||
_landed_hysteresis.set_state_and_update(_get_landed_state(), now_us);
|
||||
_ground_effect_hysteresis.set_state_and_update(_get_ground_effect_state(), now_us);
|
||||
|
||||
const bool freefallDetected = _freefall_hysteresis.get_state();
|
||||
const bool ground_contactDetected = _ground_contact_hysteresis.get_state();
|
||||
@ -80,8 +111,6 @@ void LandDetector::Run()
|
||||
const float alt_max = _get_max_altitude() > 0.0f ? _get_max_altitude() : INFINITY;
|
||||
const bool in_ground_effect = _ground_effect_hysteresis.get_state();
|
||||
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
// publish at 1 Hz, very first time, or when the result has changed
|
||||
if ((hrt_elapsed_time(&_land_detected.timestamp) >= 1_s) ||
|
||||
(_land_detected.landed != landDetected) ||
|
||||
@ -93,24 +122,23 @@ void LandDetector::Run()
|
||||
|
||||
if (!landDetected && _land_detected.landed && _takeoff_time == 0) { /* only set take off time once, until disarming */
|
||||
// We did take off
|
||||
_takeoff_time = now;
|
||||
_takeoff_time = now_us;
|
||||
}
|
||||
|
||||
_land_detected.timestamp = hrt_absolute_time();
|
||||
_land_detected.landed = landDetected;
|
||||
_land_detected.freefall = freefallDetected;
|
||||
_land_detected.maybe_landed = maybe_landedDetected;
|
||||
_land_detected.ground_contact = ground_contactDetected;
|
||||
_land_detected.alt_max = alt_max;
|
||||
_land_detected.in_ground_effect = in_ground_effect;
|
||||
|
||||
_land_detected.timestamp = hrt_absolute_time();
|
||||
_vehicle_land_detected_pub.publish(_land_detected);
|
||||
}
|
||||
|
||||
// set the flight time when disarming (not necessarily when landed, because all param changes should
|
||||
// happen on the same event and it's better to set/save params while not in armed state)
|
||||
if (_takeoff_time != 0 && !_actuator_armed.armed && _previous_armed_state) {
|
||||
_total_flight_time += now - _takeoff_time;
|
||||
if (_takeoff_time != 0 && !_armed && _previous_armed_state) {
|
||||
_total_flight_time += now_us - _takeoff_time;
|
||||
_takeoff_time = 0;
|
||||
|
||||
uint32_t flight_time = (_total_flight_time >> 32) & 0xffffffff;
|
||||
@ -124,7 +152,7 @@ void LandDetector::Run()
|
||||
_param_total_flight_time_low.commit_no_notification();
|
||||
}
|
||||
|
||||
_previous_armed_state = _actuator_armed.armed;
|
||||
_previous_armed_state = _armed;
|
||||
|
||||
perf_end(_cycle_perf);
|
||||
|
||||
@ -134,36 +162,4 @@ void LandDetector::Run()
|
||||
}
|
||||
}
|
||||
|
||||
void LandDetector::_update_params()
|
||||
{
|
||||
parameter_update_s param_update;
|
||||
_parameter_update_sub.copy(¶m_update);
|
||||
|
||||
updateParams();
|
||||
_update_total_flight_time();
|
||||
}
|
||||
|
||||
void LandDetector::_update_state()
|
||||
{
|
||||
const hrt_abstime now_us = hrt_absolute_time();
|
||||
_freefall_hysteresis.set_state_and_update(_get_freefall_state(), now_us);
|
||||
_ground_contact_hysteresis.set_state_and_update(_get_ground_contact_state(), now_us);
|
||||
_maybe_landed_hysteresis.set_state_and_update(_get_maybe_landed_state(), now_us);
|
||||
_landed_hysteresis.set_state_and_update(_get_landed_state(), now_us);
|
||||
_ground_effect_hysteresis.set_state_and_update(_get_ground_effect_state(), now_us);
|
||||
}
|
||||
|
||||
void LandDetector::_update_topics()
|
||||
{
|
||||
_actuator_armed_sub.update(&_actuator_armed);
|
||||
_vehicle_acceleration_sub.update(&_vehicle_acceleration);
|
||||
_vehicle_local_position_sub.update(&_vehicle_local_position);
|
||||
}
|
||||
|
||||
void LandDetector::_update_total_flight_time()
|
||||
{
|
||||
_total_flight_time = static_cast<uint64_t>(_param_total_flight_time_high.get()) << 32;
|
||||
_total_flight_time |= static_cast<uint32_t>(_param_total_flight_time_low.get());
|
||||
}
|
||||
|
||||
} // namespace land_detector
|
||||
|
||||
@ -56,11 +56,13 @@
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/vehicle_acceleration.h>
|
||||
#include <uORB/topics/vehicle_land_detected.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
@ -70,11 +72,9 @@ namespace land_detector
|
||||
class LandDetector : public ModuleBase<LandDetector>, ModuleParams, px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
|
||||
LandDetector();
|
||||
virtual ~LandDetector();
|
||||
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int custom_command(int argc, char *argv[])
|
||||
{
|
||||
@ -99,12 +99,12 @@ protected:
|
||||
/**
|
||||
* Updates parameters.
|
||||
*/
|
||||
virtual void _update_params();
|
||||
virtual void _update_params() {};
|
||||
|
||||
/**
|
||||
* Updates subscribed uORB topics.
|
||||
*/
|
||||
virtual void _update_topics();
|
||||
virtual void _update_topics() {};
|
||||
|
||||
/**
|
||||
* @return true if UAV is in a landed state.
|
||||
@ -136,17 +136,22 @@ protected:
|
||||
*/
|
||||
virtual bool _get_ground_effect_state() { return false; }
|
||||
|
||||
/** Run main land detector loop at this interval. */
|
||||
static constexpr uint32_t LAND_DETECTOR_UPDATE_INTERVAL = 20_ms;
|
||||
|
||||
systemlib::Hysteresis _freefall_hysteresis{false};
|
||||
systemlib::Hysteresis _landed_hysteresis{true};
|
||||
systemlib::Hysteresis _maybe_landed_hysteresis{true};
|
||||
systemlib::Hysteresis _ground_contact_hysteresis{true};
|
||||
systemlib::Hysteresis _ground_effect_hysteresis{false};
|
||||
|
||||
actuator_armed_s _actuator_armed{};
|
||||
vehicle_acceleration_s _vehicle_acceleration{};
|
||||
vehicle_local_position_s _vehicle_local_position{};
|
||||
vehicle_status_s _vehicle_status{};
|
||||
|
||||
matrix::Vector3f _acceleration{};
|
||||
|
||||
bool _armed{false};
|
||||
bool _previous_armed_state{false}; ///< stores the previous actuator_armed.armed state
|
||||
|
||||
private:
|
||||
void Run() override;
|
||||
|
||||
vehicle_land_detected_s _land_detected = {
|
||||
.timestamp = 0,
|
||||
@ -157,29 +162,19 @@ protected:
|
||||
.landed = true,
|
||||
};
|
||||
|
||||
vehicle_local_position_s _vehicle_local_position{};
|
||||
|
||||
uORB::Publication<vehicle_land_detected_s> _vehicle_land_detected_pub{ORB_ID(vehicle_land_detected)};
|
||||
|
||||
private:
|
||||
|
||||
void Run() override;
|
||||
|
||||
void _update_state();
|
||||
|
||||
void _update_total_flight_time();
|
||||
|
||||
bool _previous_armed_state{false}; ///< stores the previous actuator_armed.armed state
|
||||
|
||||
hrt_abstime _takeoff_time{0};
|
||||
hrt_abstime _total_flight_time{0}; ///< total vehicle flight time in microseconds
|
||||
|
||||
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, "land_detector_cycle")};
|
||||
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
|
||||
|
||||
uORB::Publication<vehicle_land_detected_s> _vehicle_land_detected_pub{ORB_ID(vehicle_land_detected)};
|
||||
|
||||
uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)};
|
||||
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
|
||||
uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
|
||||
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
|
||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||
|
||||
uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)};
|
||||
|
||||
DEFINE_PARAMETERS_CUSTOM_PARENT(
|
||||
ModuleParams,
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -43,12 +43,10 @@
|
||||
#pragma once
|
||||
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/hover_thrust_estimate.h>
|
||||
#include <uORB/topics/vehicle_angular_velocity.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/vehicle_local_position_setpoint.h>
|
||||
#include <uORB/topics/hover_thrust_estimate.h>
|
||||
|
||||
#include "LandDetector.h"
|
||||
|
||||
@ -76,23 +74,17 @@ protected:
|
||||
float _get_max_altitude() override;
|
||||
private:
|
||||
|
||||
/** Get control mode dependent pilot throttle threshold with which we should quit landed state and take off. */
|
||||
float _get_takeoff_throttle();
|
||||
/** Time in us that freefall has to hold before triggering freefall */
|
||||
static constexpr hrt_abstime FREEFALL_TRIGGER_TIME_US = 300_ms;
|
||||
|
||||
bool _has_low_thrust();
|
||||
bool _has_minimal_thrust();
|
||||
bool _has_altitude_lock();
|
||||
bool _has_position_lock();
|
||||
bool _is_climb_rate_enabled();
|
||||
|
||||
/** Time in us that landing conditions have to hold before triggering a land. */
|
||||
static constexpr hrt_abstime LAND_DETECTOR_TRIGGER_TIME_US = 300_ms;
|
||||
/** Time in us that ground contact condition have to hold before triggering contact ground */
|
||||
static constexpr hrt_abstime GROUND_CONTACT_TRIGGER_TIME_US = 350_ms;
|
||||
|
||||
/** Time in us that almost landing conditions have to hold before triggering almost landed . */
|
||||
static constexpr hrt_abstime MAYBE_LAND_DETECTOR_TRIGGER_TIME_US = 250_ms;
|
||||
|
||||
/** Time in us that ground contact condition have to hold before triggering contact ground */
|
||||
static constexpr hrt_abstime GROUND_CONTACT_TRIGGER_TIME_US = 350_ms;
|
||||
/** Time in us that landing conditions have to hold before triggering a land. */
|
||||
static constexpr hrt_abstime LAND_DETECTOR_TRIGGER_TIME_US = 300_ms;
|
||||
|
||||
/** Time interval in us in which wider acceptance thresholds are used after landed. */
|
||||
static constexpr hrt_abstime LAND_DETECTOR_LAND_PHASE_TIME_US = 2_s;
|
||||
@ -115,21 +107,17 @@ private:
|
||||
} _params{};
|
||||
|
||||
uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)};
|
||||
uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
|
||||
uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
|
||||
uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
|
||||
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
|
||||
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
|
||||
uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)};
|
||||
uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
|
||||
|
||||
actuator_controls_s _actuator_controls {};
|
||||
vehicle_angular_velocity_s _vehicle_angular_velocity{};
|
||||
vehicle_control_mode_s _vehicle_control_mode {};
|
||||
vehicle_local_position_setpoint_s _vehicle_local_position_setpoint {};
|
||||
|
||||
bool _flag_control_climb_rate_enabled{false};
|
||||
bool _hover_thrust_initialized{false};
|
||||
|
||||
hrt_abstime _min_trust_start{0}; ///< timestamp when minimum trust was applied first
|
||||
float _actuator_controls_throttle{0.f};
|
||||
|
||||
hrt_abstime _min_thrust_start{0}; ///< timestamp when minimum trust was applied first
|
||||
hrt_abstime _landed_time{0};
|
||||
|
||||
bool _in_descend{false}; ///< vehicle is desending
|
||||
@ -138,9 +126,6 @@ private:
|
||||
DEFINE_PARAMETERS_CUSTOM_PARENT(
|
||||
LandDetector,
|
||||
(ParamFloat<px4::params::LNDMC_ALT_MAX>) _param_lndmc_alt_max,
|
||||
(ParamFloat<px4::params::LNDMC_FFALL_THR>) _param_lndmc_ffall_thr,
|
||||
(ParamFloat<px4::params::LNDMC_FFALL_TTRI>) _param_lndmc_ffall_ttri,
|
||||
(ParamFloat<px4::params::LNDMC_LOW_T_THR>) _param_lndmc_low_t_thr,
|
||||
(ParamFloat<px4::params::LNDMC_ROT_MAX>) _param_lndmc_rot_max,
|
||||
(ParamFloat<px4::params::LNDMC_XY_VEL_MAX>) _param_lndmc_xy_vel_max,
|
||||
(ParamFloat<px4::params::LNDMC_Z_VEL_MAX>) _param_lndmc_z_vel_max
|
||||
|
||||
@ -44,7 +44,6 @@
|
||||
namespace land_detector
|
||||
{
|
||||
|
||||
|
||||
bool RoverLandDetector::_get_ground_contact_state()
|
||||
{
|
||||
return true;
|
||||
@ -52,14 +51,11 @@ bool RoverLandDetector::_get_ground_contact_state()
|
||||
|
||||
bool RoverLandDetector::_get_landed_state()
|
||||
{
|
||||
|
||||
_vehicle_status_sub.update(&_vehicle_status);
|
||||
|
||||
if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) {
|
||||
return true; // If Landing has been requested then say we have landed.
|
||||
|
||||
} else {
|
||||
return !_actuator_armed.armed; // If we are armed we are not landed.
|
||||
return !_armed; // If we are armed we are not landed.
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -42,7 +42,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "LandDetector.h"
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
|
||||
namespace land_detector
|
||||
{
|
||||
@ -57,12 +56,6 @@ protected:
|
||||
bool _get_ground_contact_state() override;
|
||||
bool _get_landed_state() override;
|
||||
|
||||
|
||||
private:
|
||||
// Program crashes when Subscriptor declared here
|
||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||
vehicle_status_s _vehicle_status{}; /**< vehicle status */
|
||||
|
||||
};
|
||||
|
||||
} // namespace land_detector
|
||||
|
||||
@ -50,15 +50,13 @@ namespace land_detector
|
||||
void VtolLandDetector::_update_topics()
|
||||
{
|
||||
MulticopterLandDetector::_update_topics();
|
||||
_airspeed_validated_sub.update(&_airspeed_validated);
|
||||
_vehicle_status_sub.update(&_vehicle_status);
|
||||
}
|
||||
|
||||
bool VtolLandDetector::_get_maybe_landed_state()
|
||||
{
|
||||
// If in Fixed-wing mode, only trigger if disarmed
|
||||
if ((_vehicle_status.timestamp != 0) && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
|
||||
return !_actuator_armed.armed;
|
||||
if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
|
||||
return !_armed;
|
||||
}
|
||||
|
||||
return MulticopterLandDetector::_get_maybe_landed_state();
|
||||
@ -67,17 +65,20 @@ bool VtolLandDetector::_get_maybe_landed_state()
|
||||
bool VtolLandDetector::_get_landed_state()
|
||||
{
|
||||
// If in Fixed-wing mode, only trigger if disarmed
|
||||
if ((_vehicle_status.timestamp != 0) && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
|
||||
return !_actuator_armed.armed;
|
||||
if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
|
||||
return !_armed;
|
||||
}
|
||||
|
||||
// this is returned from the mutlicopter land detector
|
||||
bool landed = MulticopterLandDetector::_get_landed_state();
|
||||
|
||||
// for vtol we additionally consider airspeed
|
||||
if (hrt_elapsed_time(&_airspeed_validated.timestamp) < 1_s && PX4_ISFINITE(_airspeed_validated.true_airspeed_m_s)) {
|
||||
airspeed_validated_s airspeed_validated{};
|
||||
_airspeed_validated_sub.copy(&airspeed_validated);
|
||||
|
||||
_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed_validated.true_airspeed_m_s;
|
||||
if (hrt_elapsed_time(&airspeed_validated.timestamp) < 1_s && PX4_ISFINITE(airspeed_validated.true_airspeed_m_s)) {
|
||||
|
||||
_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * airspeed_validated.true_airspeed_m_s;
|
||||
|
||||
} else {
|
||||
// if airspeed does not update, set it to zero and rely on multicopter land detector
|
||||
@ -97,8 +98,8 @@ bool VtolLandDetector::_get_landed_state()
|
||||
|
||||
bool VtolLandDetector::_get_freefall_state()
|
||||
{
|
||||
bool free_fall_detected =
|
||||
MulticopterLandDetector::_get_freefall_state(); // true if falling or in a parabolic flight (low gravity)
|
||||
// true if falling or in a parabolic flight (low gravity)
|
||||
bool free_fall_detected = MulticopterLandDetector::_get_freefall_state();
|
||||
|
||||
// only return a positive free fall detected if not in fixed-wing mode
|
||||
return _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_FIXED_WING && free_fall_detected;
|
||||
|
||||
@ -42,7 +42,6 @@
|
||||
#pragma once
|
||||
|
||||
#include <uORB/topics/airspeed_validated.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
|
||||
#include "MulticopterLandDetector.h"
|
||||
|
||||
@ -62,12 +61,7 @@ protected:
|
||||
bool _get_freefall_state() override;
|
||||
|
||||
private:
|
||||
|
||||
uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
|
||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||
|
||||
airspeed_validated_s _airspeed_validated{};
|
||||
vehicle_status_s _vehicle_status{};
|
||||
|
||||
bool _was_in_air{false}; /**< indicates whether the vehicle was in the air in the previous iteration */
|
||||
float _airspeed_filtered{0.0f}; /**< low pass filtered airspeed */
|
||||
|
||||
@ -54,8 +54,6 @@
|
||||
namespace land_detector
|
||||
{
|
||||
|
||||
extern "C" __EXPORT int land_detector_main(int argc, char *argv[]);
|
||||
|
||||
static char _currentMode[12];
|
||||
|
||||
int LandDetector::task_spawn(int argc, char *argv[])
|
||||
@ -145,8 +143,7 @@ The module runs periodically on the HP work queue.
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int land_detector_main(int argc, char *argv[])
|
||||
extern "C" __EXPORT int land_detector_main(int argc, char *argv[])
|
||||
{
|
||||
return LandDetector::main(argc, argv);
|
||||
}
|
||||
|
||||
@ -67,35 +67,6 @@ PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.5f);
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 20.0f);
|
||||
|
||||
/**
|
||||
* Multicopter specific force threshold
|
||||
*
|
||||
* Multicopter threshold on the specific force measured by accelerometers in m/s^2 for free-fall detection
|
||||
*
|
||||
* @unit m/s^2
|
||||
* @min 0.1
|
||||
* @max 10
|
||||
* @decimal 2
|
||||
*
|
||||
* @group Land Detector
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LNDMC_FFALL_THR, 2.0f);
|
||||
|
||||
/**
|
||||
* Multicopter free-fall trigger time
|
||||
*
|
||||
* Seconds (decimal) that freefall conditions have to met before triggering a freefall.
|
||||
* Minimal value is limited by LAND_DETECTOR_UPDATE_RATE=50Hz in landDetector.h
|
||||
*
|
||||
* @unit s
|
||||
* @min 0.02
|
||||
* @max 5
|
||||
* @decimal 2
|
||||
*
|
||||
* @group Land Detector
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LNDMC_FFALL_TTRI, 0.3);
|
||||
|
||||
/**
|
||||
* Maximum altitude for multicopters
|
||||
*
|
||||
@ -113,21 +84,3 @@ PARAM_DEFINE_FLOAT(LNDMC_FFALL_TTRI, 0.3);
|
||||
*
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LNDMC_ALT_MAX, -1.0f);
|
||||
|
||||
/**
|
||||
* Low throttle detection threshold.
|
||||
*
|
||||
* Defines the commanded throttle value below which the land detector
|
||||
* considers the vehicle to have "low thrust". This is one condition that
|
||||
* is used to detect the ground contact state. The value is calculated as
|
||||
* val = (MPC_THR_HOVER - MPC_THR_MIN) * LNDMC_LOW_T_THR + MPC_THR_MIN
|
||||
* Increase this value if the system takes long time to detect landing.
|
||||
*
|
||||
* @unit norm
|
||||
* @min 0.1
|
||||
* @max 0.9
|
||||
* @decimal 2
|
||||
* @group Land Detector
|
||||
*
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LNDMC_LOW_T_THR, 0.3);
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user