From 0a906216352ff73f2d466305174c969a551baeec Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 10 May 2022 20:26:29 -0400 Subject: [PATCH] multicopter hover thrust estimator use vehicle_thrust_setpoint (work in stabilized mode) --- src/modules/mc_att_control/mc_att_control.hpp | 6 +++- .../mc_att_control/mc_att_control_main.cpp | 18 ++++++++++-- .../MulticopterHoverThrustEstimator.cpp | 29 ++++++++++++------- .../MulticopterHoverThrustEstimator.hpp | 7 +++-- 4 files changed, 45 insertions(+), 15 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index d5860be271e7..be2cabbe6418 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -48,6 +48,7 @@ #include #include #include +#include #include #include #include @@ -103,6 +104,7 @@ class MulticopterAttitudeControl : public ModuleBase 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_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */ uORB::Subscription _autotune_attitude_control_status_sub{ORB_ID(autotune_attitude_control_status)}; @@ -122,6 +124,8 @@ class MulticopterAttitudeControl : public ModuleBase matrix::Vector3f _thrust_setpoint_body; ///< body frame 3D thrust vector + float _hover_thrust{NAN}; + float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */ AlphaFilter _man_x_input_filter; diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index c211539011ef..728ad310d904 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -105,6 +105,20 @@ MulticopterAttitudeControl::throttle_curve(float throttle_stick_input) { const float throttle_min = _landed ? 0.0f : _param_mpc_manthr_min.get(); + { + hover_thrust_estimate_s hte; + + if (_hover_thrust_estimate_sub.update(&hte)) { + if (hte.valid) { + _hover_thrust = hte.hover_thrust; + } + } + } + + if (!PX4_ISFINITE(_hover_thrust)) { + _hover_thrust = _param_mpc_thr_hover.get(); + } + // throttle_stick_input is in range [0, 1] switch (_param_mpc_thr_curve.get()) { case 1: // no rescaling to hover throttle @@ -113,7 +127,7 @@ MulticopterAttitudeControl::throttle_curve(float throttle_stick_input) default: // 0 or other: rescale to hover throttle at 0.5 stick return math::gradual3(throttle_stick_input, 0.f, .5f, 1.f, - throttle_min, _param_mpc_thr_hover.get(), _param_mpc_thr_max.get()); + throttle_min, _hover_thrust, _param_mpc_thr_max.get()); } } diff --git a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp index e767eb7d32cb..30855e643444 100644 --- a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp +++ b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp @@ -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,24 @@ 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); + + vehicle_thrust_setpoint_s vehicle_thrust_setpoint; + + if (_vehicle_thrust_setpoint_sub.update(&vehicle_thrust_setpoint) + && (hrt_elapsed_time(&vehicle_thrust_setpoint.timestamp) < 20_ms) + && (hrt_elapsed_time(&vehicle_attitude.timestamp) < 20_ms) + ) { + const matrix::Quatf q_att{vehicle_attitude.q}; + + matrix::Vector3f thrust_body_sp(vehicle_thrust_setpoint.xyz); + thrust_body_sp(0) = 0.f; // ignore for now + thrust_body_sp(1) = 0.f; // ignore for now + + matrix::Vector3f thrust_sp = q_att.rotateVector(thrust_body_sp); - if (_vehicle_local_position_setpoint_sub.copy(&local_pos_sp)) { - if (PX4_ISFINITE(local_pos_sp.thrust[2])) { + if (PX4_ISFINITE(thrust_sp(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 +188,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_sp(2)); bool valid = (_hover_thrust_ekf.getHoverThrustEstimateVar() < 0.001f); @@ -191,7 +200,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); } } diff --git a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp index 596bb95483c9..60a64a139bf2 100644 --- a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp +++ b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp @@ -54,10 +54,11 @@ #include #include #include +#include #include #include -#include #include +#include #include "zero_order_hover_thrust_ekf.hpp" @@ -101,9 +102,11 @@ class MulticopterHoverThrustEstimator : public ModuleBase