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

land_detector: Add and max_altitude and timestamp validity checks to MulticopterLandDetector class. (#12681)

Browse Source 
* Rename local camelCase vars to snake_case and control_mode -> vehicle_control_mode to match typdef with established class convention.
This commit is contained in:
Mark Sauder
2019-09-02 00:30:46 -04:00
committed by Daniel Agar
parent c180d63441
commit 60e5e0821a
3 changed files with 31 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/land_detector/MulticopterLandDetector.cpp
+20 -16
View File
@@ -91,7 +91,7 @@ void MulticopterLandDetector::_update_topics()
_vehicle_acceleration_sub.update(&_vehicle_acceleration);
_vehicle_angular_velocity_sub.update(&_vehicle_angular_velocity);
_vehicle_attitude_sub.update(&_vehicle_attitude);
_vehicle_control_mode_sub.update(&_control_mode);
_vehicle_control_mode_sub.update(&_vehicle_control_mode);
_vehicle_local_position_sub.update(&_vehicle_local_position);
_vehicle_local_position_setpoint_sub.update(&_vehicle_local_position_setpoint);
}
@@ -137,20 +137,20 @@ bool MulticopterLandDetector::_get_ground_contact_state()
// 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 verticalMovement;
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.
verticalMovement = fabsf(_vehicle_local_position.vz) > _param_lndmc_z_vel_max.get() * 2.5f;
vertical_movement = 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) < _param_lndmc_z_vel_max.get()) ? (0.5f * land_speed_threshold) :
_param_lndmc_z_vel_max.get();
verticalMovement = fabsf(_vehicle_local_position.vz) > maxClimbRate;
// 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.
@@ -161,11 +161,11 @@ bool MulticopterLandDetector::_get_ground_contact_state()
// 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 && !verticalMovement;
bool hit_ground = _in_descend && !vertical_movement;
// TODO: we need an accelerometer based check for vertical movement for flying without GPS
if ((_has_low_thrust() || hit_ground) && (!_horizontal_movement || !_has_position_lock())
&& (!verticalMovement || !_has_altitude_lock())) {
&& (!vertical_movement || !_has_altitude_lock())) {
return true;
}
@@ -178,7 +178,7 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
const hrt_abstime now = hrt_absolute_time();
// When not armed, consider to be maybe-landed
if (!_actuator_armed.armed) {
if (!_actuator_armed.armed || (_vehicle_attitude.timestamp == 0)) {
return true;
}
@@ -199,11 +199,11 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
}
// Next look if all rotation angles are not moving.
float maxRotationScaled = math::radians(_param_lndmc_rot_max.get()) * landThresholdFactor;
float max_rotation_scaled = math::radians(_param_lndmc_rot_max.get()) * landThresholdFactor;
bool rotating = (fabsf(_vehicle_angular_velocity.xyz[0]) > maxRotationScaled) ||
(fabsf(_vehicle_angular_velocity.xyz[1]) > maxRotationScaled) ||
(fabsf(_vehicle_angular_velocity.xyz[2]) > maxRotationScaled);
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) {
@@ -251,6 +251,10 @@ float MulticopterLandDetector::_get_max_altitude()
/* TODO: add a meaningful altitude */
float valid_altitude_max = _param_lndmc_alt_max.get();
if (valid_altitude_max < 0.0f) {
return INFINITY;
}
if (_battery_status.warning == battery_status_s::BATTERY_WARNING_LOW) {
valid_altitude_max = _param_lndmc_alt_max.get() * 0.75f;
}
@@ -283,7 +287,7 @@ bool MulticopterLandDetector::_is_climb_rate_enabled()
bool has_updated = (_vehicle_local_position_setpoint.timestamp != 0)
&& (hrt_elapsed_time(&_vehicle_local_position_setpoint.timestamp) < 500_ms);
return (_control_mode.flag_control_climb_rate_enabled && has_updated
return (_vehicle_control_mode.flag_control_climb_rate_enabled && has_updated
&& PX4_ISFINITE(_vehicle_local_position_setpoint.vz));
}
@@ -303,7 +307,7 @@ bool MulticopterLandDetector::_has_minimal_thrust()
float sys_min_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * 0.1f;
// Determine the system min throttle based on flight mode
if (!_control_mode.flag_control_climb_rate_enabled) {
if (!_vehicle_control_mode.flag_control_climb_rate_enabled) {
sys_min_throttle = (_params.minManThrottle + 0.01f);
}