yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

MultiCopterLandDetector: Implement ModuleParams inheritance (#12356)

Browse Source
This commit is contained in:
Mark Sauder
2019-08-08 23:57:42 -06: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

54
src/modules/land_detector/MulticopterLandDetector.cpp
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;

44
src/modules/land_detector/MulticopterLandDetector.h
View File

@@ -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>
@@ -81,6 +82,14 @@ 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();
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 @@ private:
* @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 @@ private:
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 @@ private:
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