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 <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/parameter_update.h> #include <uORB/topics/autotune_attitude_control_status.h> +#include <uORB/topics/hover_thrust_estimate.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_control_mode.h> @@ -103,6 +104,7 @@ class MulticopterAttitudeControl : public ModuleBase<MulticopterAttitudeControl> 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<MulticopterAttitudeControl> 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<float> _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 <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 "zero_order_hover_thrust_ekf.hpp" @@ -101,9 +102,11 @@ class MulticopterHoverThrustEstimator : public ModuleBase<MulticopterHoverThrust 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)}; hrt_abstime _timestamp_last{0};