From 4d009817d0859c8dde8c0a7392511f55ab7710b3 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 10 May 2022 20:26:29 -0400 Subject: [PATCH 1/6] multicopter hover thrust estimator use vehicle_thrust_setpoint (work in stabilized mode) --- Tools/simulation/gz | 2 +- src/drivers/gps/devices | 2 +- src/modules/mc_att_control/mc_att_control.hpp | 18 +++++++++++- .../mc_att_control/mc_att_control_main.cpp | 27 ++++++++++++++++- .../MulticopterHoverThrustEstimator.cpp | 29 ++++++++++++------- .../MulticopterHoverThrustEstimator.hpp | 7 +++-- 6 files changed, 69 insertions(+), 16 deletions(-) diff --git a/Tools/simulation/gz b/Tools/simulation/gz index 019f63e2d507..9e47793f2bc1 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 019f63e2d50763862b8f8d40cce77371b3260c52 +Subproject commit 9e47793f2bc18aa7cde39b1fc1c4b7bbc67e04ba diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices index e048340d0f6a..fb151bcaa269 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit e048340d0f6a395a3189292c33d08174bb309143 +Subproject commit fb151bcaa2690b56d4b16aa8de9fe2448f637c3b diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index db1dcd3c1576..cba72092b5bf 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 @@ -47,6 +47,7 @@ #include #include #include +#include #include #include #include @@ -98,6 +99,12 @@ class MulticopterAttitudeControl : public ModuleBase uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; +<<<<<<< HEAD +======= + 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 */ +>>>>>>> 0a90621635 (multicopter hover thrust estimator use vehicle_thrust_setpoint (work in stabilized mode)) uORB::Subscription _autotune_attitude_control_status_sub{ORB_ID(autotune_attitude_control_status)}; uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)}; @@ -118,6 +125,7 @@ class MulticopterAttitudeControl : public ModuleBase matrix::Vector3f _thrust_setpoint_body; /**< body frame 3D thrust vector */ +<<<<<<< HEAD float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */ @@ -125,6 +133,14 @@ class MulticopterAttitudeControl : public ModuleBase SlewRate _manual_throttle_maximum{0.f}; ///< 0 when disarmed ramped to 1 when spooled up AlphaFilter _man_roll_input_filter; AlphaFilter _man_pitch_input_filter; +======= + 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; + AlphaFilter _man_y_input_filter; +>>>>>>> 0a90621635 (multicopter hover thrust estimator use vehicle_thrust_setpoint (work in stabilized mode)) hrt_abstime _last_run{0}; hrt_abstime _last_attitude_setpoint{0}; 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 f6cc5db43d88..4f2b994759e9 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 @@ -103,16 +103,41 @@ MulticopterAttitudeControl::throttle_curve(float throttle_stick_input) { float thrust = 0.f; +<<<<<<< HEAD +======= + { + 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] +>>>>>>> 0a90621635 (multicopter hover thrust estimator use vehicle_thrust_setpoint (work in stabilized mode)) switch (_param_mpc_thr_curve.get()) { case 1: // no rescaling to hover throttle thrust = math::interpolate(throttle_stick_input, -1.f, 1.f, _manual_throttle_minimum.getState(), _param_mpc_thr_max.get()); break; +<<<<<<< HEAD default: // 0 or other: rescale such that a centered throttle stick corresponds to hover throttle thrust = math::interpolateNXY(throttle_stick_input, {-1.f, 0.f, 1.f}, {_manual_throttle_minimum.getState(), _param_mpc_thr_hover.get(), _param_mpc_thr_max.get()}); break; +======= + 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, _hover_thrust, _param_mpc_thr_max.get()); +>>>>>>> 0a90621635 (multicopter hover thrust estimator use vehicle_thrust_setpoint (work in stabilized mode)) } return math::min(thrust, _manual_throttle_maximum.getState()); diff --git a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp index 07c23bbb5d4e..3cc91e81293e 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 b1d03242981a..68413f284d74 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 Date: Fri, 13 Dec 2024 15:55:22 +0100 Subject: [PATCH 2/6] Resolve conflicts --- .../mc_att_control/mc_att_control_main.cpp | 25 ------------------- 1 file changed, 25 deletions(-) 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 4f2b994759e9..def38007d61c 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -103,41 +103,16 @@ MulticopterAttitudeControl::throttle_curve(float throttle_stick_input) { float thrust = 0.f; -<<<<<<< HEAD -======= - { - 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] ->>>>>>> 0a90621635 (multicopter hover thrust estimator use vehicle_thrust_setpoint (work in stabilized mode)) switch (_param_mpc_thr_curve.get()) { case 1: // no rescaling to hover throttle thrust = math::interpolate(throttle_stick_input, -1.f, 1.f, _manual_throttle_minimum.getState(), _param_mpc_thr_max.get()); break; -<<<<<<< HEAD default: // 0 or other: rescale such that a centered throttle stick corresponds to hover throttle thrust = math::interpolateNXY(throttle_stick_input, {-1.f, 0.f, 1.f}, {_manual_throttle_minimum.getState(), _param_mpc_thr_hover.get(), _param_mpc_thr_max.get()}); break; -======= - 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, _hover_thrust, _param_mpc_thr_max.get()); ->>>>>>> 0a90621635 (multicopter hover thrust estimator use vehicle_thrust_setpoint (work in stabilized mode)) } return math::min(thrust, _manual_throttle_maximum.getState()); From cfb41c04d53529e27b88c35a982707aceb48fae7 Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Mon, 16 Dec 2024 14:13:33 +0100 Subject: [PATCH 3/6] Resolve conflicts --- src/modules/mc_att_control/mc_att_control.hpp | 15 --------------- 1 file changed, 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 cba72092b5bf..26e2c13f8933 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -99,12 +99,6 @@ class MulticopterAttitudeControl : public ModuleBase uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; -<<<<<<< HEAD -======= - 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 */ ->>>>>>> 0a90621635 (multicopter hover thrust estimator use vehicle_thrust_setpoint (work in stabilized mode)) uORB::Subscription _autotune_attitude_control_status_sub{ORB_ID(autotune_attitude_control_status)}; uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)}; @@ -125,7 +119,6 @@ class MulticopterAttitudeControl : public ModuleBase matrix::Vector3f _thrust_setpoint_body; /**< body frame 3D thrust vector */ -<<<<<<< HEAD float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */ @@ -133,14 +126,6 @@ class MulticopterAttitudeControl : public ModuleBase SlewRate _manual_throttle_maximum{0.f}; ///< 0 when disarmed ramped to 1 when spooled up AlphaFilter _man_roll_input_filter; AlphaFilter _man_pitch_input_filter; -======= - 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; - AlphaFilter _man_y_input_filter; ->>>>>>> 0a90621635 (multicopter hover thrust estimator use vehicle_thrust_setpoint (work in stabilized mode)) hrt_abstime _last_run{0}; hrt_abstime _last_attitude_setpoint{0}; From 48533b88edf558500148a4f37ca3a192b37bf046 Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Mon, 16 Dec 2024 14:29:58 +0100 Subject: [PATCH 4/6] Use allocated thrust as input for hover thrust estimator --- .../MulticopterHoverThrustEstimator.cpp | 14 ++++++++++++-- .../MulticopterHoverThrustEstimator.hpp | 2 ++ 2 files changed, 14 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp index 3cc91e81293e..dbfd7630dbb0 100644 --- a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp +++ b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp @@ -165,18 +165,28 @@ void MulticopterHoverThrustEstimator::Run() _vehicle_attitude_sub.copy(&vehicle_attitude); 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) ) { + const matrix::Quatf q_att{vehicle_attitude.q}; matrix::Vector3f thrust_body_sp(vehicle_thrust_setpoint.xyz); + matrix::Vector3f thrust_body_unallocated(control_allocator_status.unallocated_thrust); + 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); + thrust_body_unallocated(0) = 0.f; // ignore for now + thrust_body_unallocated(1) = 0.f; // ignore for now + + + matrix::Vector3f thrust_sp = q_att.rotateVector(thrust_body_sp); + matrix::Vector3f thrust_unallocated = q_att.rotateVector(thrust_body_unallocated); if (PX4_ISFINITE(thrust_sp(2))) { // Inform the hover thrust estimator about the measured vertical @@ -188,7 +198,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, -thrust_sp(2)); + _hover_thrust_ekf.fuseAccZ(-local_pos.az, -(thrust_sp(2)-thrust_unallocated(2))); bool valid = (_hover_thrust_ekf.getHoverThrustEstimateVar() < 0.001f); diff --git a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp index 68413f284d74..1738cfe785a9 100644 --- a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp +++ b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp @@ -59,6 +59,7 @@ #include #include #include +#include #include "zero_order_hover_thrust_ekf.hpp" @@ -107,6 +108,7 @@ class MulticopterHoverThrustEstimator : public ModuleBase Date: Mon, 16 Dec 2024 14:50:46 +0100 Subject: [PATCH 5/6] Formatting --- .../MulticopterHoverThrustEstimator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp index dbfd7630dbb0..ab60a1ccbb2c 100644 --- a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp +++ b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp @@ -198,7 +198,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, -(thrust_sp(2)-thrust_unallocated(2))); + _hover_thrust_ekf.fuseAccZ(-local_pos.az, -(thrust_sp(2) - thrust_unallocated(2))); bool valid = (_hover_thrust_ekf.getHoverThrustEstimateVar() < 0.001f); From 38c0a6ff4923393198cb5e77799e2b626c36139b Mon Sep 17 00:00:00 2001 From: mahimayoga Date: Tue, 17 Dec 2024 10:59:36 +0100 Subject: [PATCH 6/6] Compute allocated thrust before applying rotation --- .../MulticopterHoverThrustEstimator.cpp | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp index ab60a1ccbb2c..0680a2a1a140 100644 --- a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp +++ b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp @@ -177,18 +177,15 @@ void MulticopterHoverThrustEstimator::Run() 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; - thrust_body_sp(0) = 0.f; // ignore for now - thrust_body_sp(1) = 0.f; // ignore for now + thrust_body_allocated(0) = 0.f; // ignore for now + thrust_body_allocated(1) = 0.f; // ignore for now - thrust_body_unallocated(0) = 0.f; // ignore for now - thrust_body_unallocated(1) = 0.f; // ignore for now + matrix::Vector3f thrust_allocated = q_att.rotateVector(thrust_body_allocated); - matrix::Vector3f thrust_sp = q_att.rotateVector(thrust_body_sp); - matrix::Vector3f thrust_unallocated = q_att.rotateVector(thrust_body_unallocated); - - if (PX4_ISFINITE(thrust_sp(2))) { + 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 @@ -198,7 +195,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, -(thrust_sp(2) - thrust_unallocated(2))); + _hover_thrust_ekf.fuseAccZ(-local_pos.az, -thrust_allocated(2)); bool valid = (_hover_thrust_ekf.getHoverThrustEstimateVar() < 0.001f);