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

HTE: only run in air

Browse Source 
the _landed flag only is not enough to tell if the drone is still touching
the ground, so an additional check based on the altitude AGL is required.

to have this in_air detection correctly updated, the module needs to be
scheduled on the vehicle_local_position message as no setpoint is
produced in non-assisted modes.
This commit is contained in:
bresch 2020-12-15 16:15:24 +01:00 committed by Julian Kent
parent c05c213042
commit d532bc9555
2 changed files with 40 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -59,7 +59,7 @@ MulticopterHoverThrustEstimator::~MulticopterHoverThrustEstimator()
bool MulticopterHoverThrustEstimator::init()
{
if (!_vehicle_local_position_setpoint_sub.registerCallback()) {
if (!_vehicle_local_position_sub.registerCallback()) {
PX4_ERR("vehicle_local_position_setpoint callback registration failed!");
return false;
}
@ -91,13 +91,43 @@ void MulticopterHoverThrustEstimator::updateParams()
void MulticopterHoverThrustEstimator::Run()
{
if (should_exit()) {
_vehicle_local_position_setpoint_sub.unregisterCallback();
_vehicle_local_position_sub.unregisterCallback();
exit_and_cleanup();
return;
}
// new local position estimate and setpoint needed every iteration
if (!_vehicle_local_pos_sub.updated() || !_vehicle_local_position_setpoint_sub.updated()) {
if (_vehicle_land_detected_sub.updated()) {
vehicle_land_detected_s vehicle_land_detected;
if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
_landed = vehicle_land_detected.landed;
if (_landed) {
_in_air = false;
}
}
}
if (!_vehicle_local_position_sub.updated()) {
return;
}
vehicle_local_position_s local_pos{};
if (_vehicle_local_position_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
if (local_pos.dist_bottom_valid) {
if (!_landed && (local_pos.dist_bottom > 1.f)) {
_in_air = true;
}
}
}
// new local position setpoint needed every iteration
if (!_vehicle_local_position_setpoint_sub.updated()) {
return;
}
@ -113,14 +143,6 @@ void MulticopterHoverThrustEstimator::Run()
perf_begin(_cycle_perf);
if (_vehicle_land_detected_sub.updated()) {
vehicle_land_detected_s vehicle_land_detected;
if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
_landed = vehicle_land_detected.landed;
}
}
if (_vehicle_status_sub.updated()) {
vehicle_status_s vehicle_status;
@ -129,36 +151,23 @@ void MulticopterHoverThrustEstimator::Run()
}
}
// 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
if (local_pos.dist_bottom_valid) {
_in_air = local_pos.dist_bottom > 1.f;
}
}
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)) {
if (_armed && _in_air && (dt > 0.001f) && (dt < 1.f) && PX4_ISFINITE(local_pos.az)) {
_hover_thrust_ekf.predict(dt);
vehicle_local_position_setpoint_s local_pos_sp;
if (_vehicle_local_position_setpoint_sub.update(&local_pos_sp)) {
if (_vehicle_local_position_setpoint_sub.copy(&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;
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);
const bool valid = (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();

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

@ -95,12 +95,12 @@ private:
uORB::Publication<hover_thrust_estimate_s> _hover_thrust_ekf_pub{ORB_ID(hover_thrust_estimate)};
uORB::SubscriptionCallbackWorkItem _vehicle_local_position_setpoint_sub{this, ORB_ID(vehicle_local_position_setpoint)};
uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _vehicle_local_pos_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)};
hrt_abstime _timestamp_last{0};