fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

MC-hte: use allocated thrust as input for hover thrust estimator.

Browse Source 
Improves estimates on vehicles where thrust is often saturating.
This commit is contained in:
mahimayoga 2025-01-13 10:33:14 +01:00 committed by Mahima Yoga
parent 28fa044386
commit 879e0ea9b1
2 changed files with 27 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp
View File

@ -134,11 +134,6 @@ void MulticopterHoverThrustEstimator::Run()
}
}
// new local position setpoint needed every iteration
if (!_vehicle_local_position_setpoint_sub.updated()) {
return;
}
// check for parameter updates
if (_parameter_update_sub.updated()) {
// clear update
@ -166,10 +161,25 @@ void MulticopterHoverThrustEstimator::Run()
_hover_thrust_ekf.predict(dt);
vehicle_local_position_setpoint_s local_pos_sp;
vehicle_attitude_s vehicle_attitude{};
_vehicle_attitude_sub.copy(&vehicle_attitude);
if (_vehicle_local_position_setpoint_sub.copy(&local_pos_sp)) {
if (PX4_ISFINITE(local_pos_sp.thrust[2])) {
vehicle_thrust_setpoint_s vehicle_thrust_setpoint;
control_allocator_status_s control_allocator_status;
if (_vehicle_thrust_setpoint_sub.update(&vehicle_thrust_setpoint)
&& _control_allocator_status_sub.update(&control_allocator_status)
&& (hrt_elapsed_time(&vehicle_thrust_setpoint.timestamp) < 20_ms)
&& (hrt_elapsed_time(&vehicle_attitude.timestamp) < 20_ms)
) {
matrix::Vector3f thrust_body_sp(vehicle_thrust_setpoint.xyz);
matrix::Vector3f thrust_body_unallocated(control_allocator_status.unallocated_thrust);
matrix::Vector3f thrust_body_allocated = thrust_body_sp - thrust_body_unallocated;
const matrix::Quatf q_att{vehicle_attitude.q};
matrix::Vector3f thrust_allocated = q_att.rotateVector(thrust_body_allocated);
if (PX4_ISFINITE(thrust_allocated(2))) {
// Inform the hover thrust estimator about the measured vertical
// acceleration (positive acceleration is up) and the current thrust (positive thrust is up)
// Guard against fast up and down motions biasing the estimator due to large drag and prop wash effects
@ -179,7 +189,7 @@ void MulticopterHoverThrustEstimator::Run()
1.f);
_hover_thrust_ekf.setMeasurementNoiseScale(fmaxf(meas_noise_coeff_xy, meas_noise_coeff_z));
_hover_thrust_ekf.fuseAccZ(-local_pos.az, -local_pos_sp.thrust[2]);
_hover_thrust_ekf.fuseAccZ(-local_pos.az, -thrust_allocated(2));
bool valid = (_hover_thrust_ekf.getHoverThrustEstimateVar() < 0.001f);
@ -191,7 +201,7 @@ void MulticopterHoverThrustEstimator::Run()
_valid_hysteresis.set_state_and_update(valid, local_pos.timestamp);
_valid = _valid_hysteresis.get_state();
publishStatus(local_pos.timestamp);
publishStatus(vehicle_thrust_setpoint.timestamp);
}
}

9
src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp
View File

@ -54,10 +54,12 @@
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/hover_thrust_estimate.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_thrust_setpoint.h>
#include <uORB/topics/control_allocator_status.h>
#include "zero_order_hover_thrust_ekf.hpp"
@ -101,9 +103,12 @@ private:
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_thrust_setpoint_sub{ORB_ID(vehicle_thrust_setpoint)};
uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)};
hrt_abstime _timestamp_last{0};