diff --git a/src/lib/motion_planning/PositionSmoothing.hpp b/src/lib/motion_planning/PositionSmoothing.hpp index 61070d9dab..ba13b461c4 100644 --- a/src/lib/motion_planning/PositionSmoothing.hpp +++ b/src/lib/motion_planning/PositionSmoothing.hpp @@ -262,11 +262,11 @@ public: /** * @param jerk maximum jerk for generated trajectory */ - inline void setMaxJerk(const Vector3f &jerk) + inline void setMaxJerk(const float jerk) { - _trajectory[0].setMaxJerk(jerk(0)); - _trajectory[1].setMaxJerk(jerk(1)); - _trajectory[2].setMaxJerk(jerk(2)); + _trajectory[0].setMaxJerk(jerk); + _trajectory[1].setMaxJerk(jerk); + _trajectory[2].setMaxJerk(jerk); } /** diff --git a/src/lib/motion_planning/PositionSmoothingTest.cpp b/src/lib/motion_planning/PositionSmoothingTest.cpp index 12faa7a102..ee06f8c2b1 100644 --- a/src/lib/motion_planning/PositionSmoothingTest.cpp +++ b/src/lib/motion_planning/PositionSmoothingTest.cpp @@ -43,7 +43,7 @@ public: PositionSmoothingTest() { - _position_smoothing.setMaxJerk({MAX_JERK, MAX_JERK, MAX_JERK}); + _position_smoothing.setMaxJerk(MAX_JERK); _position_smoothing.setMaxAcceleration({MAX_ACCELERATION, MAX_ACCELERATION, MAX_ACCELERATION}); _position_smoothing.setMaxVelocity({MAX_VELOCITY, MAX_VELOCITY, MAX_VELOCITY}); _position_smoothing.setMaxAllowedHorizontalError(MAX_ALLOWED_HOR_ERR); diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 0ae583dd2f..61af3f124d 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -804,14 +804,13 @@ void FlightTaskAuto::_updateTrajConstraints() // Update the constraints of the trajectories _position_smoothing.setMaxAccelerationXY(_param_mpc_acc_hor.get()); // TODO : Should be computed using heading _position_smoothing.setMaxVelocityXY(_param_mpc_xy_vel_max.get()); - float max_jerk = _param_mpc_jerk_auto.get(); - _position_smoothing.setMaxJerk({max_jerk, max_jerk, max_jerk}); // TODO : Should be computed using heading + _position_smoothing.setMaxJerk(_param_mpc_jerk_auto.get()); // TODO : Should be computed using heading if (_is_emergency_braking_active) { // When initializing with large velocity, allow 1g of // acceleration in 1s on all axes for fast braking _position_smoothing.setMaxAcceleration({9.81f, 9.81f, 9.81f}); - _position_smoothing.setMaxJerk({9.81f, 9.81f, 9.81f}); + _position_smoothing.setMaxJerk(9.81f); // If the current velocity is beyond the usual constraints, tell // the controller to exceptionally increase its saturations to avoid diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp index ea68b452fc..f639e49249 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp @@ -245,8 +245,7 @@ void FlightTaskOrbit::_updateTrajectoryBoundaries() // Update the constraints of the trajectories _position_smoothing.setMaxAccelerationXY(_param_mpc_acc_hor.get()); // TODO : Should be computed using heading _position_smoothing.setMaxVelocityXY(_param_mpc_xy_vel_max.get()); - float max_jerk = _param_mpc_jerk_auto.get(); - _position_smoothing.setMaxJerk({max_jerk, max_jerk, max_jerk}); // TODO : Should be computed using heading + _position_smoothing.setMaxJerk(_param_mpc_jerk_auto.get()); // TODO : Should be computed using heading if (_velocity_setpoint(2) < 0.f) { // up _position_smoothing.setMaxVelocityZ(_param_mpc_z_v_auto_up.get()); diff --git a/src/modules/mc_pos_control/GotoControl/GotoControl.cpp b/src/modules/mc_pos_control/GotoControl/GotoControl.cpp index 0e74ecde5f..2453997dcb 100644 --- a/src/modules/mc_pos_control/GotoControl/GotoControl.cpp +++ b/src/modules/mc_pos_control/GotoControl/GotoControl.cpp @@ -93,12 +93,10 @@ 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); - const trajectory_setpoint_s empty_trajectory_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN}; - trajectory_setpoint_s trajectory_setpoint = empty_trajectory_setpoint; - - _position_smoothing.getCurrentPosition().copyTo(trajectory_setpoint.position); - _position_smoothing.getCurrentVelocity().copyTo(trajectory_setpoint.velocity); - _position_smoothing.getCurrentAcceleration().copyTo(trajectory_setpoint.acceleration); + trajectory_setpoint_s trajectory_setpoint{}; + out_setpoints.position.copyTo(trajectory_setpoint.position); + out_setpoints.velocity.copyTo(trajectory_setpoint.velocity); + out_setpoints.acceleration.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)) { @@ -162,43 +160,35 @@ void GotoControl::resetHeadingSmoother(const float heading) _heading_smoothing.reset(heading, initial_heading_rate); } -void GotoControl::updateParams() -{ - ModuleParams::updateParams(); - setMaxAllowedHorizontalPositionError(_param_mpc_xy_err_max.get()); -} - void GotoControl::setPositionSmootherLimits(const goto_setpoint_s &goto_setpoint) { // Horizontal constraints - float max_horizontal_speed = _param_mpc_xy_cruise.get(); - float max_horizontal_accel = _param_mpc_acc_hor.get(); + float max_horizontal_speed = _param_mpc_xy_cruise; + float max_horizontal_accel = _param_mpc_acc_hor; 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, - _param_mpc_xy_cruise.get()); + _param_mpc_xy_cruise); // linearly scale horizontal acceleration limit with horizontal speed limit to maintain smoothing dynamic // only limit acceleration once within velocity constraints if (!_position_smoothing.getCurrentVelocityXY().longerThan(max_horizontal_speed)) { - const float speed_scale = max_horizontal_speed / _param_mpc_xy_cruise.get(); - max_horizontal_accel = math::constrain(_param_mpc_acc_hor.get() * speed_scale, 0.f, _param_mpc_acc_hor.get()); + const float speed_scale = max_horizontal_speed / _param_mpc_xy_cruise; + max_horizontal_accel = math::constrain(_param_mpc_acc_hor * speed_scale, 0.f, _param_mpc_acc_hor); } } - _position_smoothing.setMaxVelocityXY(_param_mpc_xy_vel_max.get()); _position_smoothing.setCruiseSpeed(max_horizontal_speed); _position_smoothing.setMaxAccelerationXY(max_horizontal_accel); - _position_smoothing.setMaxJerkXY(_param_mpc_jerk_auto.get()); // Vertical constraints - float vehicle_max_vertical_speed = _param_mpc_z_v_auto_dn.get(); - float vehicle_max_vertical_accel = _param_mpc_acc_down_max.get(); + float vehicle_max_vertical_speed = _param_mpc_z_v_auto_dn; + float vehicle_max_vertical_accel = _param_mpc_acc_down_max; if (goto_setpoint.position[2] < _position_smoothing.getCurrentPositionZ()) { // goto higher -> more negative - vehicle_max_vertical_speed = _param_mpc_z_v_auto_up.get(); - vehicle_max_vertical_accel = _param_mpc_acc_up_max.get(); + vehicle_max_vertical_speed = _param_mpc_z_v_auto_up; + vehicle_max_vertical_accel = _param_mpc_acc_up_max; } float max_vertical_speed = vehicle_max_vertical_speed; @@ -217,22 +207,21 @@ void GotoControl::setPositionSmootherLimits(const goto_setpoint_s &goto_setpoint _position_smoothing.setMaxVelocityZ(max_vertical_speed); _position_smoothing.setMaxAccelerationZ(max_vertical_accel); - _position_smoothing.setMaxJerkZ(_param_mpc_jerk_auto.get()); } void GotoControl::setHeadingSmootherLimits(const goto_setpoint_s &goto_setpoint) { - float max_heading_rate = _param_mpc_yawrauto_max.get(); - float max_heading_accel = _param_mpc_yawrauto_acc.get(); + float max_heading_rate = _param_mpc_yawrauto_max; + float max_heading_accel = _param_mpc_yawrauto_acc; 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, 0.f, _param_mpc_yawrauto_max.get()); + max_heading_rate = math::constrain(goto_setpoint.max_heading_rate, 0.f, _param_mpc_yawrauto_max); // linearly scale heading acceleration limit with heading rate limit to maintain smoothing dynamic // only limit acceleration once within velocity constraints if (fabsf(_heading_smoothing.getSmoothedHeadingRate()) <= max_heading_rate) { - const float rate_scale = max_heading_rate / _param_mpc_yawrauto_max.get(); - max_heading_accel = math::constrain(_param_mpc_yawrauto_acc.get() * rate_scale, 0.f, _param_mpc_yawrauto_acc.get()); + const float rate_scale = max_heading_rate / _param_mpc_yawrauto_max; + max_heading_accel = math::constrain(_param_mpc_yawrauto_acc * rate_scale, 0.f, _param_mpc_yawrauto_acc); } } diff --git a/src/modules/mc_pos_control/GotoControl/GotoControl.hpp b/src/modules/mc_pos_control/GotoControl/GotoControl.hpp index 6682ae84c6..63158ee979 100644 --- a/src/modules/mc_pos_control/GotoControl/GotoControl.hpp +++ b/src/modules/mc_pos_control/GotoControl/GotoControl.hpp @@ -46,7 +46,6 @@ #include #include #include -#include #include #include @@ -54,15 +53,12 @@ #include #include -class GotoControl : public ModuleParams +class GotoControl { public: - GotoControl(ModuleParams *parent) : ModuleParams(parent) {}; + GotoControl() = default; ~GotoControl() = default; - /** @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); /** @@ -91,9 +87,20 @@ public: */ void update(const float dt, const matrix::Vector3f &position, const float heading); -private: - void updateParams() override; + // Setting all parameters from the outside saves 300bytes flash + void setParamMpcAccHor(const float param_mpc_acc_hor) { _param_mpc_acc_hor = param_mpc_acc_hor; } + void setParamMpcAccDownMax(const float param_mpc_acc_down_max) { _param_mpc_acc_down_max = param_mpc_acc_down_max; } + void setParamMpcAccUpMax(const float param_mpc_acc_up_max) { _param_mpc_acc_up_max = param_mpc_acc_up_max; } + void setParamMpcJerkAuto(const float param_mpc_jerk_auto) { _position_smoothing.setMaxJerk(param_mpc_jerk_auto); } + void setParamMpcXyCruise(const float param_mpc_xy_cruise) { _param_mpc_xy_cruise = param_mpc_xy_cruise; } + void setParamMpcXyErrMax(const float param_mpc_xy_err_max) { _position_smoothing.setMaxAllowedHorizontalError(param_mpc_xy_err_max); } + void setParamMpcXyVelMax(const float param_mpc_xy_vel_max) { _position_smoothing.setMaxVelocityXY(param_mpc_xy_vel_max); } + void setParamMpcYawrautoMax(const float param_mpc_yawrauto_max) { _param_mpc_yawrauto_max = param_mpc_yawrauto_max; } + void setParamMpcYawrautoAcc(const float param_mpc_yawrauto_acc) { _param_mpc_yawrauto_acc = param_mpc_yawrauto_acc; } + void setParamMpcZVAutoDn(const float param_mpc_z_v_auto_dn) { _param_mpc_z_v_auto_dn = param_mpc_z_v_auto_dn; } + void setParamMpcZVAutoUp(const float param_mpc_z_v_auto_up) { _param_mpc_z_v_auto_up = param_mpc_z_v_auto_up; } +private: /** * @brief optionally sets dynamic translational speed limits with corresponding scale on acceleration * @@ -123,17 +130,12 @@ private: // flags if the last update() was controlling heading bool _controlling_heading{false}; - DEFINE_PARAMETERS( - (ParamFloat) _param_mpc_acc_hor, - (ParamFloat) _param_mpc_acc_down_max, - (ParamFloat) _param_mpc_acc_up_max, - (ParamFloat) _param_mpc_jerk_auto, - (ParamFloat) _param_mpc_xy_cruise, - (ParamFloat) _param_mpc_xy_err_max, - (ParamFloat) _param_mpc_xy_vel_max, - (ParamFloat) _param_mpc_yawrauto_max, - (ParamFloat) _param_mpc_yawrauto_acc, - (ParamFloat) _param_mpc_z_v_auto_dn, - (ParamFloat) _param_mpc_z_v_auto_up - ); + float _param_mpc_acc_hor{0.f}; + float _param_mpc_acc_down_max{0.f}; + float _param_mpc_acc_up_max{0.f}; + float _param_mpc_xy_cruise{0.f}; + float _param_mpc_yawrauto_max{0.f}; + float _param_mpc_yawrauto_acc{0.f}; + float _param_mpc_z_v_auto_dn{0.f}; + float _param_mpc_z_v_auto_up{0.f}; }; diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 4ed6caff90..ca8360c804 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -167,6 +167,17 @@ void MulticopterPositionControl::parameters_update(bool force) Vector3f(_param_mpc_xy_vel_i_acc.get(), _param_mpc_xy_vel_i_acc.get(), _param_mpc_z_vel_i_acc.get()), Vector3f(_param_mpc_xy_vel_d_acc.get(), _param_mpc_xy_vel_d_acc.get(), _param_mpc_z_vel_d_acc.get())); _control.setHorizontalThrustMargin(_param_mpc_thr_xy_marg.get()); + _goto_control.setParamMpcAccHor(_param_mpc_acc_hor.get()); + _goto_control.setParamMpcAccDownMax(_param_mpc_acc_down_max.get()); + _goto_control.setParamMpcAccUpMax(_param_mpc_acc_up_max.get()); + _goto_control.setParamMpcJerkAuto(_param_mpc_jerk_auto.get()); + _goto_control.setParamMpcXyCruise(_param_mpc_xy_cruise.get()); + _goto_control.setParamMpcXyErrMax(_param_mpc_xy_err_max.get()); + _goto_control.setParamMpcXyVelMax(_param_mpc_xy_vel_max.get()); + _goto_control.setParamMpcYawrautoMax(_param_mpc_yawrauto_max.get()); + _goto_control.setParamMpcYawrautoAcc(_param_mpc_yawrauto_acc.get()); + _goto_control.setParamMpcZVAutoDn(_param_mpc_z_v_auto_dn.get()); + _goto_control.setParamMpcZVAutoUp(_param_mpc_z_v_auto_up.get()); // Check that the design parameters are inside the absolute maximum constraints if (_param_mpc_xy_cruise.get() > _param_mpc_xy_vel_max.get()) { diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index 64f9b33a0e..b1f88fde57 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -177,14 +177,18 @@ private: (ParamFloat) _param_mpc_man_y_tau, (ParamFloat) _param_mpc_xy_vel_all, - (ParamFloat) _param_mpc_z_vel_all + (ParamFloat) _param_mpc_z_vel_all, + + (ParamFloat) _param_mpc_xy_err_max, + (ParamFloat) _param_mpc_yawrauto_max, + (ParamFloat) _param_mpc_yawrauto_acc ); control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */ control::BlockDerivative _vel_y_deriv; /**< velocity derivative in y */ control::BlockDerivative _vel_z_deriv; /**< velocity derivative in z */ - GotoControl _goto_control{this}; ///< class for handling smooth goto position setpoints + GotoControl _goto_control; ///< 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 */