diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index bc06ef057660..5c002953e24d 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -73,22 +73,15 @@ namespace land_detector MulticopterLandDetector::MulticopterLandDetector() { - _paramHandle.maxRotation = param_find("LNDMC_ROT_MAX"); - _paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX"); - _paramHandle.maxClimbRate = param_find("LNDMC_Z_VEL_MAX"); - _paramHandle.minThrottle = param_find("MPC_THR_MIN"); - _paramHandle.hoverThrottle = param_find("MPC_THR_HOVER"); + _paramHandle.landSpeed = param_find("MPC_LAND_SPEED"); _paramHandle.minManThrottle = param_find("MPC_MANTHR_MIN"); - _paramHandle.freefall_acc_threshold = param_find("LNDMC_FFALL_THR"); - _paramHandle.freefall_trigger_time = param_find("LNDMC_FFALL_TTRI"); - _paramHandle.altitude_max = param_find("LNDMC_ALT_MAX"); - _paramHandle.landSpeed = param_find("MPC_LAND_SPEED"); - _paramHandle.low_thrust_threshold = param_find("LNDMC_LOW_T_THR"); + _paramHandle.minThrottle = param_find("MPC_THR_MIN"); + _paramHandle.hoverThrottle = param_find("MPC_THR_HOVER"); // Use Trigger time when transitioning from in-air (false) to landed (true) / ground contact (true). + _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); - _ground_contact_hysteresis.set_hysteresis_time_from(false, GROUND_CONTACT_TRIGGER_TIME_US); } void MulticopterLandDetector::_update_topics() @@ -105,25 +98,18 @@ void MulticopterLandDetector::_update_topics() void MulticopterLandDetector::_update_params() { - param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate); - param_get(_paramHandle.maxVelocity, &_params.maxVelocity); - param_get(_paramHandle.maxRotation, &_params.maxRotation_rad_s); - _params.maxRotation_rad_s = math::radians(_params.maxRotation_rad_s); + _freefall_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1e6f * _param_lndmc_ffall_ttri.get())); + param_get(_paramHandle.minThrottle, &_params.minThrottle); param_get(_paramHandle.hoverThrottle, &_params.hoverThrottle); param_get(_paramHandle.minManThrottle, &_params.minManThrottle); - param_get(_paramHandle.freefall_acc_threshold, &_params.freefall_acc_threshold); - param_get(_paramHandle.freefall_trigger_time, &_params.freefall_trigger_time); - _freefall_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1e6f * _params.freefall_trigger_time)); - param_get(_paramHandle.altitude_max, &_params.altitude_max); param_get(_paramHandle.landSpeed, &_params.landSpeed); - param_get(_paramHandle.low_thrust_threshold, &_params.low_thrust_threshold); } bool MulticopterLandDetector::_get_freefall_state() { - if (_params.freefall_acc_threshold < 0.1f - || _params.freefall_acc_threshold > 10.0f) { //if parameter is set to zero or invalid, disable free-fall detection. + 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; } @@ -135,7 +121,7 @@ bool MulticopterLandDetector::_get_freefall_state() // 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() < _params.freefall_acc_threshold); // true if we are currently falling + return (accel.norm() < _param_lndmc_ffall_thr.get()); // true if we are currently falling } bool MulticopterLandDetector::_get_ground_contact_state() @@ -157,19 +143,19 @@ bool MulticopterLandDetector::_get_ground_contact_state() // Widen acceptance thresholds for landed state right after arming // so that motor spool-up and other effects do not trigger false negatives. - verticalMovement = fabsf(_vehicle_local_position.vz) > _params.maxClimbRate * 2.5f; + verticalMovement = fabsf(_vehicle_local_position.vz) > _param_lndmc_z_vel_max.get() * 2.5f; } else { // Adjust maxClimbRate if land_speed is lower than 2x maxClimbrate - float maxClimbRate = ((land_speed_threshold * 0.5f) < _params.maxClimbRate) ? (0.5f * land_speed_threshold) : - _params.maxClimbRate; + float maxClimbRate = ((land_speed_threshold * 0.5f) < _param_lndmc_z_vel_max.get()) ? (0.5f * land_speed_threshold) : + _param_lndmc_z_vel_max.get(); verticalMovement = fabsf(_vehicle_local_position.vz) > maxClimbRate; } // 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) > _params.maxVelocity; + + _vehicle_local_position.vy * _vehicle_local_position.vy) > _param_lndmc_xy_vel_max.get(); // 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 @@ -213,7 +199,7 @@ bool MulticopterLandDetector::_get_maybe_landed_state() } // Next look if all rotation angles are not moving. - float maxRotationScaled = _params.maxRotation_rad_s * landThresholdFactor; + float maxRotationScaled = math::radians(_param_lndmc_rot_max.get()) * landThresholdFactor; bool rotating = (fabsf(_vehicle_angular_velocity.xyz[0]) > maxRotationScaled) || (fabsf(_vehicle_angular_velocity.xyz[1]) > maxRotationScaled) || @@ -262,19 +248,19 @@ bool MulticopterLandDetector::_get_landed_state() float MulticopterLandDetector::_get_max_altitude() { - /* ToDo: add a meaningful altitude */ - float valid_altitude_max = _params.altitude_max; + /* TODO: add a meaningful altitude */ + float valid_altitude_max = _param_lndmc_alt_max.get(); if (_battery_status.warning == battery_status_s::BATTERY_WARNING_LOW) { - valid_altitude_max = _params.altitude_max * 0.75f; + valid_altitude_max = _param_lndmc_alt_max.get() * 0.75f; } if (_battery_status.warning == battery_status_s::BATTERY_WARNING_CRITICAL) { - valid_altitude_max = _params.altitude_max * 0.5f; + valid_altitude_max = _param_lndmc_alt_max.get() * 0.5f; } if (_battery_status.warning == battery_status_s::BATTERY_WARNING_EMERGENCY) { - valid_altitude_max = _params.altitude_max * 0.25f; + valid_altitude_max = _param_lndmc_alt_max.get() * 0.25f; } return valid_altitude_max; @@ -305,7 +291,7 @@ bool MulticopterLandDetector::_has_low_thrust() { // 30% of throttle range between min and hover float sys_min_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * - _params.low_thrust_threshold; + _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; diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index c434c136257d..8289c43b5429 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -52,6 +52,7 @@ #include #include #include +#include #include #include #include @@ -81,6 +82,14 @@ class MulticopterLandDetector : public LandDetector 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(); + 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; @@ -97,35 +106,22 @@ class MulticopterLandDetector : public LandDetector * @brief Handles for interesting parameters **/ struct { - param_t maxClimbRate; - param_t maxVelocity; - param_t maxRotation; param_t minThrottle; param_t hoverThrottle; param_t minManThrottle; - param_t freefall_acc_threshold; - param_t freefall_trigger_time; - param_t altitude_max; param_t landSpeed; - param_t low_thrust_threshold; } _paramHandle{}; struct { - float maxClimbRate; - float maxVelocity; - float maxRotation_rad_s; float minThrottle; float hoverThrottle; float minManThrottle; - float freefall_acc_threshold; - float freefall_trigger_time; - float altitude_max; float landSpeed; - float low_thrust_threshold; } _params{}; uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)}; uORB::Subscription _battery_sub{ORB_ID(battery_status)}; + uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)}; uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; @@ -136,7 +132,7 @@ class MulticopterLandDetector : public LandDetector actuator_controls_s _actuator_controls {}; battery_status_s _battery_status {}; vehicle_control_mode_s _control_mode {}; - vehicle_acceleration_s _vehicle_acceleration{}; + vehicle_acceleration_s _vehicle_acceleration{}; vehicle_attitude_s _vehicle_attitude {}; vehicle_angular_velocity_s _vehicle_angular_velocity{}; vehicle_local_position_s _vehicle_local_position {}; @@ -148,14 +144,16 @@ class MulticopterLandDetector : public LandDetector bool _in_descend{false}; ///< vehicle is desending bool _horizontal_movement{false}; ///< vehicle is moving horizontally - /* get control mode dependent pilot throttle threshold with which we should quit landed state and take off */ - float _get_takeoff_throttle(); - bool _has_altitude_lock(); - bool _has_position_lock(); - bool _has_minimal_thrust(); - bool _has_low_thrust(); - bool _is_climb_rate_enabled(); + DEFINE_PARAMETERS_CUSTOM_PARENT( + LandDetector, + (ParamFloat) _param_lndmc_alt_max, + (ParamFloat) _param_lndmc_ffall_thr, + (ParamFloat) _param_lndmc_ffall_ttri, + (ParamFloat) _param_lndmc_low_t_thr, + (ParamFloat) _param_lndmc_rot_max, + (ParamFloat) _param_lndmc_xy_vel_max, + (ParamFloat) _param_lndmc_z_vel_max + ); }; - } // namespace land_detector