diff --git a/src/modules/mc_pos_control/GotoControl/GotoControl.cpp b/src/modules/mc_pos_control/GotoControl/GotoControl.cpp index dd05e635a3..4d08e359c4 100644 --- a/src/modules/mc_pos_control/GotoControl/GotoControl.cpp +++ b/src/modules/mc_pos_control/GotoControl/GotoControl.cpp @@ -69,12 +69,12 @@ void GotoControl::update(const float dt, const matrix::Vector3f &position, const const Vector3f feedforward_velocity{}; const bool force_zero_velocity_setpoint = false; PositionSmoothing::PositionSmoothingSetpoints out_setpoints; - _position_smoother.generateSetpoints(position, position_setpoint, feedforward_velocity, dt, - force_zero_velocity_setpoint, out_setpoints); + _position_smoothing.generateSetpoints(position, position_setpoint, feedforward_velocity, dt, + force_zero_velocity_setpoint, out_setpoints); - _position_smoother.getCurrentPosition().copyTo(trajectory_setpoint.position); - _position_smoother.getCurrentVelocity().copyTo(trajectory_setpoint.velocity); - _position_smoother.getCurrentAcceleration().copyTo(trajectory_setpoint.acceleration); + _position_smoothing.getCurrentPosition().copyTo(trajectory_setpoint.position); + _position_smoothing.getCurrentVelocity().copyTo(trajectory_setpoint.velocity); + _position_smoothing.getCurrentAcceleration().copyTo(trajectory_setpoint.acceleration); out_setpoints.jerk.copyTo(trajectory_setpoint.jerk); if (goto_setpoint.flag_control_heading && PX4_ISFINITE(goto_setpoint.heading) && PX4_ISFINITE(heading)) { @@ -83,10 +83,10 @@ void GotoControl::update(const float dt, const matrix::Vector3f &position, const } setHeadingSmootherLimits(goto_setpoint); - _heading_smoother.update(goto_setpoint.heading, dt); + _heading_smoothing.update(goto_setpoint.heading, dt); - trajectory_setpoint.yaw = _heading_smoother.getSmoothedHeading(); - trajectory_setpoint.yawspeed = _heading_smoother.getSmoothedHeadingRate(); + trajectory_setpoint.yaw = _heading_smoothing.getSmoothedHeading(); + trajectory_setpoint.yawspeed = _heading_smoothing.getSmoothedHeadingRate(); _controlling_heading = true; @@ -114,7 +114,7 @@ void GotoControl::resetPositionSmoother(const matrix::Vector3f &position) const Vector3f initial_acceleration{}; const Vector3f initial_velocity{}; - _position_smoother.reset(initial_acceleration, initial_velocity, position); + _position_smoothing.reset(initial_acceleration, initial_velocity, position); _need_smoother_reset = false; } @@ -128,41 +128,41 @@ void GotoControl::resetHeadingSmoother(const float heading) } const float initial_heading_rate{0.f}; - _heading_smoother.reset(heading, initial_heading_rate); + _heading_smoothing.reset(heading, initial_heading_rate); } void GotoControl::setPositionSmootherLimits(const goto_setpoint_s &goto_setpoint) { // constrain horizontal velocity - float max_horizontal_speed = _vehicle_constraints.max_horizontal_speed; - float max_horizontal_accel = _vehicle_constraints.max_horizontal_accel; + float max_horizontal_speed = _goto_constraints.max_horizontal_speed; + float max_horizontal_accel = _goto_constraints.max_horizontal_accel; if (goto_setpoint.flag_set_max_horizontal_speed && PX4_ISFINITE(goto_setpoint.max_horizontal_speed)) { max_horizontal_speed = math::constrain(goto_setpoint.max_horizontal_speed, 0.f, - _vehicle_constraints.max_horizontal_speed); + _goto_constraints.max_horizontal_speed); // linearly scale horizontal acceleration limit with horizontal speed limit to maintain smoothing dynamic // only limit acceleration once within velocity constraints - if (_position_smoother.getCurrentVelocityXY().norm() <= max_horizontal_speed) { - const float speed_scale = max_horizontal_speed / _vehicle_constraints.max_horizontal_speed; - max_horizontal_accel = math::constrain(_vehicle_constraints.max_horizontal_accel * speed_scale, - VelocitySmoothing::kMinAccel, - _vehicle_constraints.max_horizontal_accel); + if (_position_smoothing.getCurrentVelocityXY().norm() <= max_horizontal_speed) { + const float speed_scale = max_horizontal_speed / _goto_constraints.max_horizontal_speed; + max_horizontal_accel = math::constrain(_goto_constraints.max_horizontal_accel * speed_scale, + 0.f, + _goto_constraints.max_horizontal_accel); } } - _position_smoother.setCruiseSpeed(max_horizontal_speed); - _position_smoother.setMaxVelocityXY(max_horizontal_speed); - _position_smoother.setMaxAccelerationXY(max_horizontal_accel); + _position_smoothing.setCruiseSpeed(max_horizontal_speed); + _position_smoothing.setMaxVelocityXY(max_horizontal_speed); + _position_smoothing.setMaxAccelerationXY(max_horizontal_accel); // constrain vertical velocity - const bool pos_setpoint_is_below_smooth_pos = goto_setpoint.position[2] - _position_smoother.getCurrentPositionZ() > + const bool pos_setpoint_is_below_smooth_pos = goto_setpoint.position[2] - _position_smoothing.getCurrentPositionZ() > 0.f; - const float vehicle_max_vertical_speed = (pos_setpoint_is_below_smooth_pos) ? _vehicle_constraints.max_down_speed : - _vehicle_constraints.max_up_speed; - const float vehicle_max_vertical_accel = (pos_setpoint_is_below_smooth_pos) ? _vehicle_constraints.max_down_accel : - _vehicle_constraints.max_up_accel; + const float vehicle_max_vertical_speed = (pos_setpoint_is_below_smooth_pos) ? _goto_constraints.max_down_speed : + _goto_constraints.max_up_speed; + const float vehicle_max_vertical_accel = (pos_setpoint_is_below_smooth_pos) ? _goto_constraints.max_down_accel : + _goto_constraints.max_up_accel; float max_vertical_speed = vehicle_max_vertical_speed; float max_vertical_accel = vehicle_max_vertical_accel; @@ -173,38 +173,38 @@ void GotoControl::setPositionSmootherLimits(const goto_setpoint_s &goto_setpoint // linearly scale vertical acceleration limit with vertical speed limit to maintain smoothing dynamic // only limit acceleration once within velocity constraints - if (fabsf(_position_smoother.getCurrentVelocityZ()) <= max_vertical_speed) { + if (fabsf(_position_smoothing.getCurrentVelocityZ()) <= max_vertical_speed) { const float speed_scale = max_vertical_speed / vehicle_max_vertical_speed; - max_vertical_accel = math::constrain(vehicle_max_vertical_accel * speed_scale, VelocitySmoothing::kMinAccel, + max_vertical_accel = math::constrain(vehicle_max_vertical_accel * speed_scale, 0.f, vehicle_max_vertical_accel); } } - _position_smoother.setMaxVelocityZ(max_vertical_speed); - _position_smoother.setMaxAccelerationZ(max_vertical_accel); + _position_smoothing.setMaxVelocityZ(max_vertical_speed); + _position_smoothing.setMaxAccelerationZ(max_vertical_accel); - _position_smoother.setMaxJerkXY(_vehicle_constraints.max_jerk); - _position_smoother.setMaxJerkZ(_vehicle_constraints.max_jerk); + _position_smoothing.setMaxJerkXY(_goto_constraints.max_jerk); + _position_smoothing.setMaxJerkZ(_goto_constraints.max_jerk); } void GotoControl::setHeadingSmootherLimits(const goto_setpoint_s &goto_setpoint) { - float max_heading_rate = _vehicle_constraints.max_heading_rate; - float max_heading_accel = _vehicle_constraints.max_heading_accel; + float max_heading_rate = _goto_constraints.max_heading_rate; + float max_heading_accel = _goto_constraints.max_heading_accel; if (goto_setpoint.flag_set_max_heading_rate && PX4_ISFINITE(goto_setpoint.max_heading_rate)) { - max_heading_rate = math::constrain(goto_setpoint.max_heading_rate, HeadingSmoother::kMinHeadingRate, - _vehicle_constraints.max_heading_rate); + max_heading_rate = math::constrain(goto_setpoint.max_heading_rate, 0.f, + _goto_constraints.max_heading_rate); // linearly scale heading acceleration limit with heading rate limit to maintain smoothing dynamic // only limit acceleration once within velocity constraints - if (fabsf(_heading_smoother.getSmoothedHeadingRate()) <= max_heading_rate) { - const float rate_scale = max_heading_rate / _vehicle_constraints.max_heading_rate; - max_heading_accel = math::constrain(_vehicle_constraints.max_heading_accel * rate_scale, - HeadingSmoother::kMinHeadingAccel, _vehicle_constraints.max_heading_accel); + if (fabsf(_heading_smoothing.getSmoothedHeadingRate()) <= max_heading_rate) { + const float rate_scale = max_heading_rate / _goto_constraints.max_heading_rate; + max_heading_accel = math::constrain(_goto_constraints.max_heading_accel * rate_scale, + 0.f, _goto_constraints.max_heading_accel); } } - _heading_smoother.setMaxHeadingRate(max_heading_rate); - _heading_smoother.setMaxHeadingAccel(max_heading_accel); + _heading_smoothing.setMaxHeadingRate(max_heading_rate); + _heading_smoothing.setMaxHeadingAccel(max_heading_accel); } diff --git a/src/modules/mc_pos_control/GotoControl/GotoControl.hpp b/src/modules/mc_pos_control/GotoControl/GotoControl.hpp index ee9d4d2a9c..202aee55b5 100644 --- a/src/modules/mc_pos_control/GotoControl/GotoControl.hpp +++ b/src/modules/mc_pos_control/GotoControl/GotoControl.hpp @@ -37,15 +37,15 @@ * A class which smooths position and heading references from "go-to" setpoints * for planar multicopters. * - * Be sure to set constraints with setVehicleConstraints() before calling the update() method for the first time, otherwise - * the default motion will be VERY slow. + * Be sure to set constraints with setGotoConstraints() before calling the update() method for the first time */ #pragma once -#include +#include #include -#include +#include +#include #include #include @@ -55,34 +55,43 @@ public: GotoControl() = default; ~GotoControl() = default; - /** - * @brief struct containing maximum vehicle translational and rotational constraints - * - */ - struct VehicleConstraints { - float max_horizontal_speed = kMinSpeed; // [m/s] - float max_down_speed = kMinSpeed; // [m/s] - float max_up_speed = kMinSpeed; // [m/s] - float max_horizontal_accel = VelocitySmoothing::kMinAccel; // [m/s^2] - float max_down_accel = VelocitySmoothing::kMinAccel; // [m/s^2] - float max_up_accel = VelocitySmoothing::kMinAccel; // [m/s^2] - float max_jerk = VelocitySmoothing::kMinJerk; // [m/s^3] - float max_heading_rate = HeadingSmoother::kMinHeadingRate; // [rad/s] - float max_heading_accel = HeadingSmoother::kMinHeadingAccel; // [rad/s^2] + /** @brief struct containing maximum vehicle translational and rotational constraints */ + struct GotoConstraints { + float max_horizontal_speed; // [m/s] + float max_down_speed; // [m/s] + float max_up_speed; // [m/s] + float max_horizontal_accel; // [m/s^2] + float max_down_accel; // [m/s^2] + float max_up_accel; // [m/s^2] + float max_jerk; // [m/s^3] + float max_heading_rate; // [rad/s] + float max_heading_accel; // [rad/s^2] }; /** - * @brief updates the smoothers with the current setpoints and outputs the "trajectory setpoint" for lower level - * loops to track. + * @brief sets the maximum vehicle translational and rotational constraints. note these can be more conservatively + * overriden (e.g. slowed down) via the speed scalers in the go-to setpoint. * - * @param[in] dt [s] time since last control update - * @param[in] position [m] (NED) local vehicle position - * @param[in] heading [rad] (from North) vehicle heading - * @param[in] goto_setpoint struct containing current go-to setpoints - * @param[out] trajectory_setpoint struct containing trajectory (tracking) setpoints + * @param vehicle_constraints Struct containing desired vehicle constraints */ - void update(const float dt, const matrix::Vector3f &position, const float heading, - const goto_setpoint_s &goto_setpoint, trajectory_setpoint_s &trajectory_setpoint); + void setGotoConstraints(const GotoConstraints &vehicle_constraints) + { + _goto_constraints.max_horizontal_speed = math::max(0.f, vehicle_constraints.max_horizontal_speed); + _goto_constraints.max_down_speed = math::max(0.f, vehicle_constraints.max_down_speed); + _goto_constraints.max_up_speed = math::max(0.f, vehicle_constraints.max_up_speed); + _goto_constraints.max_horizontal_accel = math::max(0.f, + vehicle_constraints.max_horizontal_accel); + _goto_constraints.max_down_accel = math::max(0.f, vehicle_constraints.max_down_accel); + _goto_constraints.max_up_accel = math::max(0.f, vehicle_constraints.max_up_accel); + _goto_constraints.max_jerk = math::max(0.f, vehicle_constraints.max_jerk); + _goto_constraints.max_heading_rate = math::max(0.f, + vehicle_constraints.max_heading_rate); + _goto_constraints.max_heading_accel = math::max(0.f, + vehicle_constraints.max_heading_accel); + } + + /** @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); } /** * @brief resets the position smoother at the current position with zero velocity and acceleration. @@ -99,42 +108,19 @@ public: void resetHeadingSmoother(const float heading); /** - * @brief sets the maximum vehicle translational and rotational constraints. note these can be more conservatively - * overriden (e.g. slowed down) via the speed scalers in the go-to setpoint. + * @brief updates the smoothers with the current setpoints and outputs the "trajectory setpoint" for lower level + * loops to track. * - * @param vehicle_constraints Struct containing desired vehicle constraints + * @param[in] dt [s] time since last control update + * @param[in] position [m] (NED) local vehicle position + * @param[in] heading [rad] (from North) vehicle heading + * @param[in] goto_setpoint struct containing current go-to setpoints + * @param[out] trajectory_setpoint struct containing trajectory (tracking) setpoints */ - void setVehicleConstraints(const VehicleConstraints &vehicle_constraints) - { - _vehicle_constraints.max_horizontal_speed = math::max(kMinSpeed, vehicle_constraints.max_horizontal_speed); - _vehicle_constraints.max_down_speed = math::max(kMinSpeed, vehicle_constraints.max_down_speed); - _vehicle_constraints.max_up_speed = math::max(kMinSpeed, vehicle_constraints.max_up_speed); - _vehicle_constraints.max_horizontal_accel = math::max(VelocitySmoothing::kMinAccel, - vehicle_constraints.max_horizontal_accel); - _vehicle_constraints.max_down_accel = math::max(VelocitySmoothing::kMinAccel, vehicle_constraints.max_down_accel); - _vehicle_constraints.max_up_accel = math::max(VelocitySmoothing::kMinAccel, vehicle_constraints.max_up_accel); - _vehicle_constraints.max_jerk = math::max(VelocitySmoothing::kMinJerk, vehicle_constraints.max_jerk); - _vehicle_constraints.max_heading_rate = math::max(HeadingSmoother::kMinHeadingRate, - vehicle_constraints.max_heading_rate); - _vehicle_constraints.max_heading_accel = math::max(HeadingSmoother::kMinHeadingAccel, - vehicle_constraints.max_heading_accel); - } - - /** - * @brief Set the position smoother's maximum allowed horizontal position error at which trajectory integration halts - * - * @param error [m] horizontal position error - */ - void setMaxAllowedHorizontalPositionError(const float error) - { - _position_smoother.setMaxAllowedHorizontalError(error); - } + void update(const float dt, const matrix::Vector3f &position, const float heading, + const goto_setpoint_s &goto_setpoint, trajectory_setpoint_s &trajectory_setpoint); private: - - // [m/s] minimum value of the maximum translational speeds - static constexpr float kMinSpeed{0.1f}; - /** * @brief optionally sets dynamic translational speed limits with corresponding scale on acceleration * @@ -149,9 +135,10 @@ private: */ void setHeadingSmootherLimits(const goto_setpoint_s &goto_setpoint); - VehicleConstraints _vehicle_constraints{}; - PositionSmoothing _position_smoother; - HeadingSmoother _heading_smoother; + GotoConstraints _goto_constraints{}; + + PositionSmoothing _position_smoothing; + HeadingSmoothing _heading_smoothing; // 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 25b4338b4e..c400340457 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -386,7 +386,7 @@ void MulticopterPositionControl::Run() _goto_control.resetHeadingSmoother(states.yaw); } - const GotoControl::VehicleConstraints vehicle_constraints = { + const GotoControl::GotoConstraints goto_constraints = { _param_mpc_xy_cruise.get(), _param_mpc_z_v_auto_dn.get(), _param_mpc_z_v_auto_up.get(), @@ -398,7 +398,7 @@ void MulticopterPositionControl::Run() _param_mpc_yawaauto_max.get() }; - _goto_control.setVehicleConstraints(vehicle_constraints); + _goto_control.setGotoConstraints(goto_constraints); _goto_control.setMaxAllowedHorizontalPositionError(_param_mpc_xy_err_max.get()); _goto_control.update(dt, states.position, states.yaw, _goto_setpoint, _setpoint);