diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 04ff8a6449..c751c64185 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -198,6 +198,8 @@ void MulticopterPositionControl::parameters_update(bool force) Vector3f(_param_mpc_xy_vel_p_acc.get(), _param_mpc_xy_vel_p_acc.get(), _param_mpc_z_vel_p_acc.get()), 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.setAccelerationGains(_param_mpc_xy_acc_p.get()); + _control.setAccelerationCutoff(_param_mpc_acc_cutoff.get()); _control.setHorizontalThrustMargin(_param_mpc_thr_xy_marg.get()); _control.decoupleHorizontalAndVecticalAcceleration(_param_mpc_acc_decouple.get()); _goto_control.setParamMpcAccHor(_param_mpc_acc_hor.get()); @@ -545,7 +547,7 @@ void MulticopterPositionControl::Run() _control.setInputSetpoint(_setpoint); _control.setAttitude(Quatf(_vehicle_attitude.q)); - _control.setBodySpecificForce(Vector3f(_vehicle_acceleration.xyz)); + _control.setBodySpecificForce(Vector3f(_vehicle_acceleration.xyz), dt); // update states if (!PX4_ISFINITE(_setpoint.position[2]) diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index 00b3686624..d4460e7de1 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -147,6 +147,8 @@ private: (ParamFloat) _param_mpc_z_vel_p_acc, (ParamFloat) _param_mpc_z_vel_i_acc, (ParamFloat) _param_mpc_z_vel_d_acc, + (ParamFloat) _param_mpc_xy_acc_p, + (ParamFloat) _param_mpc_acc_cutoff, (ParamFloat) _param_mpc_xy_vel_max, (ParamFloat) _param_mpc_z_v_auto_up, (ParamFloat) _param_mpc_z_vel_max_up, diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index 23cf67498c..1368181391 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -149,7 +149,7 @@ void PositionControl::_velocityControl(const float dt) // No control input from setpoints or corresponding states which are NAN ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity); - _accelerationControl(); + _accelerationControl(dt); // Integrator anti-windup in vertical direction if ((_thr_sp(2) >= -_lim_thr_min && vel_error(2) >= 0.f) || @@ -201,7 +201,7 @@ void PositionControl::_velocityControl(const float dt) _vel_int += vel_error.emult(_gain_vel_i) * dt; } -void PositionControl::_accelerationControl() +void PositionControl::_accelerationControl(const float dt) { // Assume standard acceleration due to gravity in vertical direction for attitude generation float z_specific_force = -CONSTANTS_ONE_G; @@ -214,7 +214,7 @@ void PositionControl::_accelerationControl() const Vector3f sf_sp_ned = Vector3f(_acc_sp(0), _acc_sp(1), z_specific_force); const Vector3f sf_sp_body = _attitude.rotateVectorInverse(sf_sp_ned); - Quatf att_error(_body_specific_force, sf_sp_body); + Quatf att_error(AxisAnglef(AxisAnglef(Quatf(_body_specific_force_lpf.getState(), sf_sp_body)) * _gain_acc_xy)); Quatf desired_att = _attitude * att_error; Vector3f body_z = desired_att.dcm_z(); ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _lim_tilt); diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp index 292a79be09..fdaed27f9c 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp @@ -40,6 +40,7 @@ #pragma once #include +#include #include #include #include @@ -93,6 +94,9 @@ public: */ void setVelocityGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D); + void setAccelerationGains(const float gain_xy) { _gain_acc_xy = gain_xy; } + void setAccelerationCutoff(const float cutoff_frequency) { _body_specific_force_lpf.setCutoffFreq(cutoff_frequency); } + /** * Set the maximum velocity to execute with feed forward and position control * @param vel_horizontal horizontal velocity limit @@ -146,7 +150,7 @@ public: */ void setInputSetpoint(const trajectory_setpoint_s &setpoint); - void setBodySpecificForce(const matrix::Vector3f &specific_force) { _body_specific_force = specific_force; } + void setBodySpecificForce(const matrix::Vector3f &specific_force, const float dt) { _body_specific_force_lpf.update(specific_force, dt); } void setAttitude(const matrix::Quatf &attitude) { _attitude = attitude; } /** @@ -202,7 +206,7 @@ private: void _positionControl(); ///< Position proportional control void _velocityControl(const float dt); ///< Velocity PID control - void _accelerationControl(); ///< Acceleration setpoint processing + void _accelerationControl(float dt); ///< Acceleration setpoint processing // Gains matrix::Vector3f _gain_pos_p; ///< Position control proportional gain @@ -221,6 +225,8 @@ private: float _hover_thrust{}; ///< Thrust [HOVER_THRUST_MIN, HOVER_THRUST_MAX] with which the vehicle hovers not accelerating down or up with level orientation bool _decouple_horizontal_and_vertical_acceleration{true}; ///< Ignore vertical acceleration setpoint to remove its effect on the tilt setpoint + float _gain_acc_xy{1.f}; + AlphaFilter _body_specific_force_lpf{}; // States matrix::Vector3f _pos; /**< current position */ @@ -237,6 +243,5 @@ private: float _yaw_sp{}; /**< desired heading */ float _yawspeed_sp{}; /** desired yaw-speed */ - matrix::Vector3f _body_specific_force; matrix::Quatf _attitude; }; diff --git a/src/modules/mc_pos_control/multicopter_position_control_gain_params.c b/src/modules/mc_pos_control/multicopter_position_control_gain_params.c index df36d1f733..85aa2c1d20 100644 --- a/src/modules/mc_pos_control/multicopter_position_control_gain_params.c +++ b/src/modules/mc_pos_control/multicopter_position_control_gain_params.c @@ -135,3 +135,29 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D_ACC, 0.f); * @group Multicopter Position Control */ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D_ACC, 0.2f); + +/** + * Gain for xy acceleration control + * + * Reduce to lower the sensitivity to accelerometer noise + * + * @min 0.1 + * @max 1 + * @decimal 2 + * @increment 0.05 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_ACC_P, 1.f); + +/** + * Acceleration feedback cutoff frequency + * + * 0 to disable + * + * @min 0 + * @max 100 + * @decimal 1 + * @increment 1.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_ACC_CUTOFF, 0.1f);