From ba640acfcc00d5c71c97d5d4ec031f8cb3c80802 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 3 Aug 2020 10:30:52 -0400 Subject: [PATCH] mc_hover_thrust_estimator: validity flag and other small improvements/fixes - track and publish validity based on hover thrust variance, innovation test ratio, and hysteresis - only publish on actual updates or becoming inactive - fix dt (previous timestamp wasn't being saved) - use local position timestamp (corresponding) to accel data rather than current time to avoid unnecessary timing jitter - check local position validity before using - mc_hover_thrust_estimator: move from wq:lp_default -> wq:nav_and_controllers to ensure the hover thrust estimator runs after the position controller and uses the same vehicle_local_position data - land_detector: check hover thrust estimate validity and adjust low throttle thresholds if hover thrust is available - mc_pos_control: only use hover thrust estimate if valid --- msg/hover_thrust_estimate.msg | 5 +- .../land_detector/MulticopterLandDetector.cpp | 12 +- .../land_detector/MulticopterLandDetector.h | 1 + .../mc_hover_thrust_estimator/CMakeLists.txt | 3 + .../MulticopterHoverThrustEstimator.cpp | 105 +++++++++++++----- .../MulticopterHoverThrustEstimator.hpp | 9 +- .../mc_pos_control/mc_pos_control_main.cpp | 4 +- 7 files changed, 103 insertions(+), 36 deletions(-) diff --git a/msg/hover_thrust_estimate.msg b/msg/hover_thrust_estimate.msg index 5391d047ca..a38d90425f 100644 --- a/msg/hover_thrust_estimate.msg +++ b/msg/hover_thrust_estimate.msg @@ -1,4 +1,5 @@ -uint64 timestamp # time since system start (microseconds) +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # time of corresponding sensor data last used for this estimate float32 hover_thrust # estimated hover thrust [0.1, 0.9] float32 hover_thrust_var # estimated hover thrust variance @@ -8,3 +9,5 @@ float32 accel_innov_var # innovation variance of the last acceleration fusion float32 accel_innov_test_ratio # normalized innovation squared test ratio float32 accel_noise_var # vertical acceleration noise variance estimated form innovation residual + +bool valid diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 8c68da1648..b54ac8f5d4 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -106,7 +106,11 @@ void MulticopterLandDetector::_update_topics() hover_thrust_estimate_s hte; if (_hover_thrust_estimate_sub.update(&hte)) { - _params.hoverThrottle = hte.hover_thrust; + if (hte.valid) { + _params.hoverThrottle = hte.hover_thrust; + } + + _hover_thrust_estimate_valid = hte.valid; } } } @@ -182,9 +186,9 @@ bool MulticopterLandDetector::_get_ground_contact_state() } - // low thrust: 30% of throttle range between min and hover - const float sys_low_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * 0.3f; - + // low thrust: 30% of throttle range between min and hover, relaxed to 60% if hover thrust estimate available + const float thr_pct_hover = _hover_thrust_estimate_valid ? 0.6f : 0.3f; + const float sys_low_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * thr_pct_hover; bool ground_contact = (_actuator_controls_throttle <= sys_low_throttle); // if we have a valid velocity setpoint and the vehicle is demanded to go down but no vertical movement present, diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 11946f657c..a81c306ea5 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -114,6 +114,7 @@ private: bool _flag_control_climb_rate_enabled{false}; bool _hover_thrust_initialized{false}; + bool _hover_thrust_estimate_valid{false}; float _actuator_controls_throttle{0.f}; diff --git a/src/modules/mc_hover_thrust_estimator/CMakeLists.txt b/src/modules/mc_hover_thrust_estimator/CMakeLists.txt index d924da3bb1..9504a18add 100644 --- a/src/modules/mc_hover_thrust_estimator/CMakeLists.txt +++ b/src/modules/mc_hover_thrust_estimator/CMakeLists.txt @@ -39,10 +39,13 @@ px4_add_library(zero_order_hover_thrust_ekf px4_add_module( MODULE modules__mc_hover_thrust_estimator MAIN mc_hover_thrust_estimator + COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} SRCS MulticopterHoverThrustEstimator.cpp MulticopterHoverThrustEstimator.hpp DEPENDS + hysteresis mathlib px4_work_queue zero_order_hover_thrust_ekf diff --git a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp index 57ca54b9a3..6851e9f41a 100644 --- a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp +++ b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp @@ -41,10 +41,13 @@ #include +using namespace time_literals; + MulticopterHoverThrustEstimator::MulticopterHoverThrustEstimator() : ModuleParams(nullptr), - WorkItem(MODULE_NAME, px4::wq_configurations::lp_default) + WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) { + _valid_hysteresis.set_hysteresis_time_from(false, 2_s); updateParams(); reset(); } @@ -93,6 +96,11 @@ void MulticopterHoverThrustEstimator::Run() return; } + // new local position estimate and setpoint needed every iteration + if (!_vehicle_local_pos_sub.updated() || !_vehicle_local_position_setpoint_sub.updated()) { + return; + } + // check for parameter updates if (_parameter_update_sub.updated()) { // clear update @@ -105,73 +113,112 @@ void MulticopterHoverThrustEstimator::Run() perf_begin(_cycle_perf); - vehicle_land_detected_s vehicle_land_detected; - vehicle_status_s vehicle_status; - vehicle_local_position_s local_pos; + if (_vehicle_land_detected_sub.updated()) { + vehicle_land_detected_s vehicle_land_detected; - if (_vehicle_land_detected_sub.update(&vehicle_land_detected)) { - _landed = vehicle_land_detected.landed; + if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) { + _landed = vehicle_land_detected.landed; + } } - if (_vehicle_status_sub.update(&vehicle_status)) { - _armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.copy(&vehicle_status)) { + _armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + } } - if (_vehicle_local_pos_sub.update(&local_pos)) { + // always copy latest position estimate + vehicle_local_position_s local_pos{}; + + if (_vehicle_local_pos_sub.copy(&local_pos)) { // This is only necessary because the landed // flag of the land detector does not guarantee that // the vehicle does not touch the ground anymore // TODO: improve the landed flag - _in_air = local_pos.dist_bottom > 1.f; + if (local_pos.dist_bottom_valid) { + _in_air = local_pos.dist_bottom > 1.f; + } } - ZeroOrderHoverThrustEkf::status status{}; + const float dt = (local_pos.timestamp - _timestamp_last) * 1e-6f; + _timestamp_last = local_pos.timestamp; - if (_armed && !_landed && _in_air) { - vehicle_local_position_setpoint_s local_pos_sp; - - if (_vehicle_local_position_setpoint_sub.update(&local_pos_sp)) { + if (_armed && !_landed && (dt > 0.001f) && (dt < 1.f) && PX4_ISFINITE(local_pos.az)) { - const hrt_abstime now = hrt_absolute_time(); - const float dt = math::constrain((now - _timestamp_last) / 1e6f, 0.002f, 0.2f); + _hover_thrust_ekf.predict(dt); - _hover_thrust_ekf.predict(dt); + vehicle_local_position_setpoint_s local_pos_sp; - if (PX4_ISFINITE(local_pos.az) && PX4_ISFINITE(local_pos_sp.thrust[2])) { + if (_vehicle_local_position_setpoint_sub.update(&local_pos_sp)) { + if (PX4_ISFINITE(local_pos_sp.thrust[2])) { // Inform the hover thrust estimator about the measured vertical // acceleration (positive acceleration is up) and the current thrust (positive thrust is up) + ZeroOrderHoverThrustEkf::status status; _hover_thrust_ekf.fuseAccZ(-local_pos.az, -local_pos_sp.thrust[2], status); + + const bool valid = _in_air && (status.hover_thrust_var < 0.001f) && (status.innov_test_ratio < 1.f); + _valid_hysteresis.set_state_and_update(valid, local_pos.timestamp); + _valid = _valid_hysteresis.get_state(); + + publishStatus(local_pos.timestamp, status); } } } else { + _valid_hysteresis.set_state_and_update(false, hrt_absolute_time()); + if (!_armed) { reset(); } - status.hover_thrust = _hover_thrust_ekf.getHoverThrustEstimate(); - status.hover_thrust_var = _hover_thrust_ekf.getHoverThrustEstimateVar(); - status.accel_noise_var = _hover_thrust_ekf.getAccelNoiseVar(); - status.innov = NAN; - status.innov_var = NAN; - status.innov_test_ratio = NAN; - } + if (_valid) { + // only publish a single message to invalidate + publishInvalidStatus(); - publishStatus(status); + _valid = false; + } + } perf_end(_cycle_perf); } -void MulticopterHoverThrustEstimator::publishStatus(ZeroOrderHoverThrustEkf::status &status) +void MulticopterHoverThrustEstimator::publishStatus(const hrt_abstime ×tamp_sample, + const ZeroOrderHoverThrustEkf::status &status) { - hover_thrust_estimate_s status_msg{}; + hover_thrust_estimate_s status_msg; + + status_msg.timestamp_sample = timestamp_sample; + status_msg.hover_thrust = status.hover_thrust; status_msg.hover_thrust_var = status.hover_thrust_var; + status_msg.accel_innov = status.innov; status_msg.accel_innov_var = status.innov_var; status_msg.accel_innov_test_ratio = status.innov_test_ratio; status_msg.accel_noise_var = status.accel_noise_var; + + status_msg.valid = _valid; + + status_msg.timestamp = hrt_absolute_time(); + + _hover_thrust_ekf_pub.publish(status_msg); +} + +void MulticopterHoverThrustEstimator::publishInvalidStatus() +{ + hover_thrust_estimate_s status_msg{}; + + status_msg.hover_thrust = NAN; + status_msg.hover_thrust_var = NAN; + status_msg.accel_innov = NAN; + status_msg.accel_innov_var = NAN; + status_msg.accel_innov_test_ratio = NAN; + status_msg.accel_noise_var = NAN; + status_msg.timestamp = hrt_absolute_time(); + _hover_thrust_ekf_pub.publish(status_msg); } diff --git a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp index 5e16c997a2..0e6853d4c5 100644 --- a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp +++ b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp @@ -42,6 +42,7 @@ #pragma once #include +#include #include #include #include @@ -86,7 +87,9 @@ private: void updateParams() override; void reset(); - void publishStatus(ZeroOrderHoverThrustEkf::status &status); + + void publishStatus(const hrt_abstime ×tamp_sample, const ZeroOrderHoverThrustEkf::status &status); + void publishInvalidStatus(); ZeroOrderHoverThrustEkf _hover_thrust_ekf{}; @@ -105,6 +108,10 @@ private: bool _landed{false}; bool _in_air{false}; + bool _valid{false}; + + systemlib::Hysteresis _valid_hysteresis{false}; + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")}; DEFINE_PARAMETERS( diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 6f61f7ce6e..e7e72ea8cc 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -435,7 +435,9 @@ MulticopterPositionControl::poll_subscriptions() hover_thrust_estimate_s hte; if (_hover_thrust_estimate_sub.update(&hte)) { - _control.updateHoverThrust(hte.hover_thrust); + if (hte.valid) { + _control.updateHoverThrust(hte.hover_thrust); + } } } }