fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-29 08:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_hover_thrust_estimator: validity flag and other small improvements/fixes

Browse Source 
- 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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp
+76 -29
View File
@@ -41,10 +41,13 @@
#include <mathlib/mathlib.h>
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 && (dt > 0.001f) && (dt < 1.f) && PX4_ISFINITE(local_pos.az)) {
_hover_thrust_ekf.predict(dt);
if (_armed && !_landed && _in_air) {
vehicle_local_position_setpoint_s local_pos_sp;
if (_vehicle_local_position_setpoint_sub.update(&local_pos_sp)) {
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);
if (PX4_ISFINITE(local_pos.az) && PX4_ISFINITE(local_pos_sp.thrust[2])) {
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 &timestamp_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);
}