From 3de6fee07fbc620c7c8dfef8d114a0f0f3519cfa Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 23 Nov 2023 14:53:02 +0100 Subject: [PATCH] GotoControl: make interface over uORB --- .../flight_mode_manager/FlightModeManager.hpp | 1 + .../GotoControl/GotoControl.cpp | 47 ++++++++++++++++--- .../GotoControl/GotoControl.hpp | 17 +++++-- .../MulticopterPositionControl.cpp | 38 ++++----------- .../MulticopterPositionControl.hpp | 9 +--- 5 files changed, 65 insertions(+), 47 deletions(-) diff --git a/src/modules/flight_mode_manager/FlightModeManager.hpp b/src/modules/flight_mode_manager/FlightModeManager.hpp index d06884a47e..d066b58294 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.hpp +++ b/src/modules/flight_mode_manager/FlightModeManager.hpp @@ -50,6 +50,7 @@ #include #include #include +#include #include #include #include diff --git a/src/modules/mc_pos_control/GotoControl/GotoControl.cpp b/src/modules/mc_pos_control/GotoControl/GotoControl.cpp index 2a811316c0..5bbfb18d8f 100644 --- a/src/modules/mc_pos_control/GotoControl/GotoControl.cpp +++ b/src/modules/mc_pos_control/GotoControl/GotoControl.cpp @@ -38,15 +38,37 @@ #include "GotoControl.hpp" #include "PositionControl.hpp" +#include #include #include -void GotoControl::update(const float dt, const matrix::Vector3f &position, const float heading, - const goto_setpoint_s &goto_setpoint, trajectory_setpoint_s &trajectory_setpoint) -{ - trajectory_setpoint = PositionControl::empty_trajectory_setpoint; +using namespace time_literals; - const Vector3f position_setpoint(goto_setpoint.position); +bool GotoControl::checkForSetpoint(const hrt_abstime &now, const bool enabled) +{ + _goto_setpoint_sub.update(); + const bool timestamp_initialized = _goto_setpoint_sub.get().timestamp != 0; + const bool no_timeout = now < (_goto_setpoint_sub.get().timestamp + 500_ms); + const bool need_to_run = timestamp_initialized && no_timeout && enabled; + + if (!need_to_run) { + _is_initialized = false; + } + + return need_to_run; +} + +void GotoControl::update(const float dt, const matrix::Vector3f &position, const float heading) +{ + if (!_is_initialized) { + resetPositionSmoother(position); + resetHeadingSmoother(heading); + _is_initialized = true; + } + + const goto_setpoint_s &goto_setpoint = _goto_setpoint_sub.get(); + + const Vector3f position_setpoint(_goto_setpoint_sub.get().position); if (!position_setpoint.isAllFinite()) { // TODO: error messaging @@ -72,6 +94,8 @@ void GotoControl::update(const float dt, const matrix::Vector3f &position, const _position_smoothing.generateSetpoints(position, position_setpoint, feedforward_velocity, dt, force_zero_velocity_setpoint, out_setpoints); + trajectory_setpoint_s trajectory_setpoint = PositionControl::empty_trajectory_setpoint; + _position_smoothing.getCurrentPosition().copyTo(trajectory_setpoint.position); _position_smoothing.getCurrentVelocity().copyTo(trajectory_setpoint.velocity); _position_smoothing.getCurrentAcceleration().copyTo(trajectory_setpoint.acceleration); @@ -99,9 +123,18 @@ void GotoControl::update(const float dt, const matrix::Vector3f &position, const _controlling_heading = false; } - trajectory_setpoint.timestamp = goto_setpoint.timestamp; - _need_smoother_reset = false; + + trajectory_setpoint.timestamp = goto_setpoint.timestamp; + _trajectory_setpoint_pub.publish(trajectory_setpoint); + + vehicle_constraints_s vehicle_constraints{ + .timestamp = goto_setpoint.timestamp, + .speed_up = NAN, + .speed_down = NAN, + .want_takeoff = false + }; + _vehicle_constraints_pub.publish(vehicle_constraints); } void GotoControl::resetPositionSmoother(const matrix::Vector3f &position) diff --git a/src/modules/mc_pos_control/GotoControl/GotoControl.hpp b/src/modules/mc_pos_control/GotoControl/GotoControl.hpp index 968c83669e..0ce8a96329 100644 --- a/src/modules/mc_pos_control/GotoControl/GotoControl.hpp +++ b/src/modules/mc_pos_control/GotoControl/GotoControl.hpp @@ -47,8 +47,12 @@ #include #include #include + +#include +#include #include #include +#include class GotoControl : public ModuleParams { @@ -59,6 +63,8 @@ public: /** @param error [m] position smoother's maximum allowed horizontal position error at which trajectory integration halts */ void setMaxAllowedHorizontalPositionError(const float error) { _position_smoothing.setMaxAllowedHorizontalError(error); } + bool checkForSetpoint(const hrt_abstime &now, const bool enabled); + /** * @brief resets the position smoother at the current position with zero velocity and acceleration. * @@ -83,10 +89,7 @@ public: * @param[in] goto_setpoint struct containing current go-to setpoints * @param[out] trajectory_setpoint struct containing trajectory (tracking) setpoints */ - void update(const float dt, const matrix::Vector3f &position, const float heading, - const goto_setpoint_s &goto_setpoint, trajectory_setpoint_s &trajectory_setpoint); - - bool is_initialized{false}; + void update(const float dt, const matrix::Vector3f &position, const float heading); private: void updateParams() override; @@ -105,9 +108,15 @@ private: */ void setHeadingSmootherLimits(const goto_setpoint_s &goto_setpoint); + uORB::SubscriptionData _goto_setpoint_sub{ORB_ID(goto_setpoint)}; + uORB::Publication _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)}; + uORB::Publication _vehicle_constraints_pub{ORB_ID(vehicle_constraints)}; + PositionSmoothing _position_smoothing; HeadingSmoothing _heading_smoothing; + bool _is_initialized{false}; ///< true if smoothers were reset to current state + // flags that the next update() requires a valid current vehicle position to reset the smoothers bool _need_smoother_reset{true}; diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 6f3007c497..4ed6caff90 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -370,37 +370,14 @@ void MulticopterPositionControl::Run() PositionControlStates states{set_vehicle_states(vehicle_local_position)}; - _trajectory_setpoint_sub.update(&_setpoint); - - const hrt_abstime last_goto_timestamp = _goto_setpoint.timestamp; - _goto_setpoint_sub.update(&_goto_setpoint); - - if ((_goto_setpoint.timestamp != 0) - && (_goto_setpoint.timestamp >= _time_position_control_enabled) - && (hrt_elapsed_time(&last_goto_timestamp) < 200_ms) - && _vehicle_control_mode.flag_multicopter_position_control_enabled) { - // take goto setpoint as priority over trajectory setpoint - - if (!_goto_control.is_initialized) { - _goto_control.resetPositionSmoother(states.position); - _goto_control.resetHeadingSmoother(states.yaw); - } - - _goto_control.update(dt, states.position, states.yaw, _goto_setpoint, _setpoint); - - // for logging - _trajectory_setpoint_pub.publish(_setpoint); - - _vehicle_constraints.timestamp = hrt_absolute_time(); - _vehicle_constraints.want_takeoff = false; - _vehicle_constraints.speed_up = _param_mpc_z_vel_max_up.get(); - _vehicle_constraints.speed_down = _param_mpc_z_vel_max_dn.get(); - - } else { - _goto_control.is_initialized = false; - _vehicle_constraints_sub.update(&_vehicle_constraints); + // if a goto setpoint available this publishes a trajectory setpoint to go there + if (_goto_control.checkForSetpoint(vehicle_local_position.timestamp_sample, + _vehicle_control_mode.flag_multicopter_position_control_enabled)) { + _goto_control.update(dt, states.position, states.yaw); } + _trajectory_setpoint_sub.update(&_setpoint); + adjustSetpointForEKFResets(vehicle_local_position, _setpoint); if (_vehicle_control_mode.flag_multicopter_position_control_enabled) { @@ -416,6 +393,9 @@ void MulticopterPositionControl::Run() if (_vehicle_control_mode.flag_multicopter_position_control_enabled && (_setpoint.timestamp >= _time_position_control_enabled)) { + // update vehicle constraints and handle smooth takeoff + _vehicle_constraints_sub.update(&_vehicle_constraints); + // fix to prevent the takeoff ramp to ramp to a too high value or get stuck because of NAN // TODO: this should get obsolete once the takeoff limiting moves into the flight tasks if (!PX4_ISFINITE(_vehicle_constraints.speed_up) || (_vehicle_constraints.speed_up > _param_mpc_z_vel_max_up.get())) { diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index a606eee290..64f9b33a0e 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -58,7 +58,6 @@ #include #include #include -#include #include #include #include @@ -97,14 +96,12 @@ private: uORB::PublicationData _takeoff_status_pub{ORB_ID(takeoff_status)}; uORB::Publication _vehicle_attitude_setpoint_pub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Publication _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; /**< vehicle local position setpoint publication */ - uORB::Publication _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)}; uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)}; /**< vehicle local position */ uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)}; - uORB::Subscription _goto_setpoint_sub{ORB_ID(goto_setpoint)}; uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; uORB::Subscription _vehicle_constraints_sub{ORB_ID(vehicle_constraints)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; @@ -113,7 +110,6 @@ private: hrt_abstime _time_stamp_last_loop{0}; /**< time stamp of last loop iteration */ hrt_abstime _time_position_control_enabled{0}; - goto_setpoint_s _goto_setpoint{}; trajectory_setpoint_s _setpoint{PositionControl::empty_trajectory_setpoint}; vehicle_control_mode_s _vehicle_control_mode{}; @@ -132,8 +128,6 @@ private: .landed = true, }; - GotoControl _goto_control{this}; - DEFINE_PARAMETERS( // Position Control (ParamFloat) _param_mpc_xy_p, @@ -190,7 +184,8 @@ private: control::BlockDerivative _vel_y_deriv; /**< velocity derivative in y */ control::BlockDerivative _vel_z_deriv; /**< velocity derivative in z */ - PositionControl _control; /**< class for core PID position control */ + GotoControl _goto_control{this}; ///< class for handling smooth goto position setpoints + PositionControl _control; ///< class for core PID position control hrt_abstime _last_warn{0}; /**< timer when the last warn message was sent out */