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
This commit is contained in:
Daniel Agar
2020-08-03 10:30:52 -04:00
committed by GitHub
parent 958d5a36ec
commit ba640acfcc
7 changed files with 103 additions and 36 deletions
@@ -42,6 +42,7 @@
#pragma once
#include <drivers/drv_hrt.h>
#include <lib/hysteresis/hysteresis.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
@@ -86,7 +87,9 @@ private:
void updateParams() override;
void reset();
void publishStatus(ZeroOrderHoverThrustEkf::status &status);
void publishStatus(const hrt_abstime &timestamp_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(