From 60e5e0821a0ee64c42cd77e85e0c0c39526dca88 Mon Sep 17 00:00:00 2001 From: Mark Sauder Date: Sun, 1 Sep 2019 22:30:46 -0600 Subject: [PATCH] land_detector: Add and max_altitude and timestamp validity checks to MulticopterLandDetector class. (#12681) * Rename local camelCase vars to snake_case and control_mode -> vehicle_control_mode to match typdef with established class convention. --- src/modules/land_detector/LandDetector.h | 6 ++-- .../land_detector/MulticopterLandDetector.cpp | 36 ++++++++++--------- .../land_detector/MulticopterLandDetector.h | 20 +++++------ 3 files changed, 31 insertions(+), 31 deletions(-) diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index f28e082cc5..41dc9df878 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -42,16 +42,16 @@ #pragma once -#include -#include #include #include #include +#include +#include +#include #include #include #include #include -#include using namespace time_literals; diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 5c002953e2..4348a47ebd 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -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); } diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index f079d88dea..7219ca7f95 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -42,22 +42,19 @@ #pragma once -#include "LandDetector.h" +#include -#include -#include -#include -#include -#include #include #include #include -#include #include #include #include #include #include +#include + +#include "LandDetector.h" using namespace time_literals; @@ -122,7 +119,6 @@ private: 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)}; @@ -132,17 +128,17 @@ private: actuator_controls_s _actuator_controls {}; battery_status_s _battery_status {}; - vehicle_control_mode_s _control_mode {}; vehicle_acceleration_s _vehicle_acceleration{}; - vehicle_attitude_s _vehicle_attitude {}; vehicle_angular_velocity_s _vehicle_angular_velocity{}; + vehicle_attitude_s _vehicle_attitude {}; + vehicle_control_mode_s _vehicle_control_mode {}; vehicle_local_position_s _vehicle_local_position {}; vehicle_local_position_setpoint_s _vehicle_local_position_setpoint {}; - hrt_abstime _min_trust_start{0}; ///< timestamp when minimum trust was applied first + hrt_abstime _min_trust_start{0}; ///< timestamp when minimum trust was applied first hrt_abstime _landed_time{0}; - bool _in_descend{false}; ///< vehicle is desending + bool _in_descend{false}; ///< vehicle is desending bool _horizontal_movement{false}; ///< vehicle is moving horizontally DEFINE_PARAMETERS_CUSTOM_PARENT(