Skip to content

Commit

Permalink
MultiCopterLandDetector: Implement ModuleParams inheritance (#12356)
Browse files Browse the repository at this point in the history
  • Loading branch information
@mcsauder @bkueng
mcsauder authored and bkueng committed Aug 9, 2019
1 parent 2f222d6 commit 056c800
Show file tree
Hide file tree
Showing 2 changed files with 41 additions and 57 deletions.
54 changes: 20 additions & 34 deletions src/modules/land_detector/MulticopterLandDetector.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand All @@ -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;
}

Expand All @@ -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()
Expand All @@ -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
Expand Down Expand Up @@ -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) ||
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down
44 changes: 21 additions & 23 deletions src/modules/land_detector/MulticopterLandDetector.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,7 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_bias.h>
#include <uORB/topics/vehicle_acceleration.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_attitude.h>
Expand Down Expand Up @@ -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;

Expand All @@ -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)};

This comment has been minimized.

Sign in to view

Copy link
@dagar

dagar Aug 9, 2019

Member

@mcsauder accidentally readded in rebase?

This comment has been minimized.

Sign in to view

Copy link
@mcsauder

mcsauder Aug 9, 2019 •
edited
Loading

Author Contributor

Well... this PR was working towards and being balanced with #11874, which grew out of #9756, so quite possibly... (Nice catch... at least it was alphabetized. :} )
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)};
Expand All @@ -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 {};
Expand All @@ -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<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
);
};


} // namespace land_detector

0 comments on commit 056c800

Please sign in to comment.