diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index db1dcd3c1576..995eea4256ca 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-2025 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,9 +99,11 @@ 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)}; 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)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; @@ -118,8 +121,10 @@ class MulticopterAttitudeControl : public ModuleBase matrix::Vector3f _thrust_setpoint_body; /**< body frame 3D thrust vector */ - float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ - float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */ + float _hover_thrust{NAN}; + + float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ + float _man_tilt_max{0.f}; /**< maximum tilt allowed for manual flight [rad] */ SlewRate _manual_throttle_minimum{0.f}; ///< 0 when landed and ramped to MPC_MANTHR_MIN in air SlewRate _manual_throttle_maximum{0.f}; ///< 0 when disarmed ramped to 1 when spooled up 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..22b78d519c2b 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-2025 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,15 +103,31 @@ MulticopterAttitudeControl::throttle_curve(float throttle_stick_input) { float thrust = 0.f; + { + 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 [-1, 1] 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; - 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()}); + default: // 0 or other: rescale to hover throttle at 0 stick input + thrust = math::interpolateNXY(throttle_stick_input, + {-1.f, 0.f, 1.f}, + {_manual_throttle_minimum.getState(), _hover_thrust, _param_mpc_thr_max.get()}); break; }