dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

GotoControl: Save flash

This commit is contained in:
Matthias Grob 2023-11-28 16:19:41 +01:00
parent 7b4712cb29
commit 8bb20db7da
8 changed files with 66 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/lib/motion_planning/PositionSmoothing.hpp
View File

@ -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);
}
/**

2
src/lib/motion_planning/PositionSmoothingTest.cpp
View File

@ -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);

5
src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
View File

@ -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

3
src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
View File

@ -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());

47
src/modules/mc_pos_control/GotoControl/GotoControl.cpp
View File

@ -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);
}
}

44
src/modules/mc_pos_control/GotoControl/GotoControl.hpp
View File

@ -46,7 +46,6 @@
#include <lib/motion_planning/PositionSmoothing.hpp>
#include <mathlib/math/Limits.hpp>
#include <matrix/matrix/math.hpp>
#include <px4_platform_common/module_params.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
@ -54,15 +53,12 @@
#include <uORB/topics/trajectory_setpoint.h>
#include <uORB/topics/vehicle_constraints.h>
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<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor,
(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max,
(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
(ParamFloat<px4::params::MPC_JERK_AUTO>) _param_mpc_jerk_auto,
(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise,
(ParamFloat<px4::params::MPC_XY_ERR_MAX>) _param_mpc_xy_err_max,
(ParamFloat<px4::params::MPC_XY_VEL_MAX>) _param_mpc_xy_vel_max,
(ParamFloat<px4::params::MPC_YAWRAUTO_MAX>) _param_mpc_yawrauto_max,
(ParamFloat<px4::params::MPC_YAWRAUTO_ACC>) _param_mpc_yawrauto_acc,
(ParamFloat<px4::params::MPC_Z_V_AUTO_DN>) _param_mpc_z_v_auto_dn,
(ParamFloat<px4::params::MPC_Z_V_AUTO_UP>) _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};
};

11
src/modules/mc_pos_control/MulticopterPositionControl.cpp
View File

@ -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()) {

8
src/modules/mc_pos_control/MulticopterPositionControl.hpp
View File

@ -177,14 +177,18 @@ private:
(ParamFloat<px4::params::MPC_MAN_Y_TAU>) _param_mpc_man_y_tau,
(ParamFloat<px4::params::MPC_XY_VEL_ALL>) _param_mpc_xy_vel_all,
(ParamFloat<px4::params::MPC_Z_VEL_ALL>) _param_mpc_z_vel_all
(ParamFloat<px4::params::MPC_Z_VEL_ALL>) _param_mpc_z_vel_all,
(ParamFloat<px4::params::MPC_XY_ERR_MAX>) _param_mpc_xy_err_max,
(ParamFloat<px4::params::MPC_YAWRAUTO_MAX>) _param_mpc_yawrauto_max,
(ParamFloat<px4::params::MPC_YAWRAUTO_ACC>) _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 */