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

MultiCopterLandDetector: Implement ModuleParams inheritance (#12356)

Browse Source
This commit is contained in:
Mark Sauder
2019-08-09 07:57:42 +02:00
committed by Beat Küng
parent 2f222d6cbf
commit 056c8000a1
2 changed files with 41 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/land_detector/MulticopterLandDetector.cpp
+20 -34
View File
@@ -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;