forked from Archive/PX4-Autopilot
mpc: add possibility to generate tilt using full 3D accel
Using full 3D acceleration provides better horizontal acceleration tracking but also creates a sometimes unwanted behavior because the tilt is directly coupled with the vertical acceleration setpoint.
This commit is contained in:
parent
32aa3263a6
commit
2e6dd243af
|
@ -167,6 +167,7 @@ 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_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()));
|
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());
|
_control.setHorizontalThrustMargin(_param_mpc_thr_xy_marg.get());
|
||||||
|
_control.decoupleHorizontalAndVecticalAcceleration(_param_mpc_acc_decouple.get());
|
||||||
_goto_control.setParamMpcAccHor(_param_mpc_acc_hor.get());
|
_goto_control.setParamMpcAccHor(_param_mpc_acc_hor.get());
|
||||||
_goto_control.setParamMpcAccDownMax(_param_mpc_acc_down_max.get());
|
_goto_control.setParamMpcAccDownMax(_param_mpc_acc_down_max.get());
|
||||||
_goto_control.setParamMpcAccUpMax(_param_mpc_acc_up_max.get());
|
_goto_control.setParamMpcAccUpMax(_param_mpc_acc_up_max.get());
|
||||||
|
|
|
@ -146,6 +146,7 @@ private:
|
||||||
(ParamFloat<px4::params::MPC_TILTMAX_AIR>) _param_mpc_tiltmax_air,
|
(ParamFloat<px4::params::MPC_TILTMAX_AIR>) _param_mpc_tiltmax_air,
|
||||||
(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover,
|
(ParamFloat<px4::params::MPC_THR_HOVER>) _param_mpc_thr_hover,
|
||||||
(ParamBool<px4::params::MPC_USE_HTE>) _param_mpc_use_hte,
|
(ParamBool<px4::params::MPC_USE_HTE>) _param_mpc_use_hte,
|
||||||
|
(ParamBool<px4::params::MPC_ACC_DECOUPLE>) _param_mpc_acc_decouple,
|
||||||
|
|
||||||
// Takeoff / Land
|
// Takeoff / Land
|
||||||
(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time, /**< time to let motors spool up after arming */
|
(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time, /**< time to let motors spool up after arming */
|
||||||
|
|
|
@ -204,13 +204,20 @@ void PositionControl::_velocityControl(const float dt)
|
||||||
void PositionControl::_accelerationControl()
|
void PositionControl::_accelerationControl()
|
||||||
{
|
{
|
||||||
// Assume standard acceleration due to gravity in vertical direction for attitude generation
|
// Assume standard acceleration due to gravity in vertical direction for attitude generation
|
||||||
Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), CONSTANTS_ONE_G).normalized();
|
float z_specific_force = -CONSTANTS_ONE_G;
|
||||||
|
|
||||||
|
if (!_decouple_horizontal_and_vertical_acceleration) {
|
||||||
|
// Include vertical acceleration setpoint for better horizontal acceleration tracking
|
||||||
|
z_specific_force += _acc_sp(2);
|
||||||
|
}
|
||||||
|
|
||||||
|
Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), -z_specific_force).normalized();
|
||||||
ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _lim_tilt);
|
ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _lim_tilt);
|
||||||
// Scale thrust assuming hover thrust produces standard gravity
|
// Convert to thrust assuming hover thrust produces standard gravity
|
||||||
float collective_thrust = _acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust;
|
const float thrust_ned_z = _acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust;
|
||||||
// Project thrust to planned body attitude
|
// Project thrust to planned body attitude
|
||||||
collective_thrust /= (Vector3f(0, 0, 1).dot(body_z));
|
const float cos_ned_body = (Vector3f(0, 0, 1).dot(body_z));
|
||||||
collective_thrust = math::min(collective_thrust, -_lim_thr_min);
|
const float collective_thrust = math::min(thrust_ned_z / cos_ned_body, -_lim_thr_min);
|
||||||
_thr_sp = body_z * collective_thrust;
|
_thr_sp = body_z * collective_thrust;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -163,6 +163,11 @@ public:
|
||||||
*/
|
*/
|
||||||
void resetIntegral() { _vel_int.setZero(); }
|
void resetIntegral() { _vel_int.setZero(); }
|
||||||
|
|
||||||
|
/**
|
||||||
|
* If set, the tilt setpoint is computed by assuming no vertical acceleration
|
||||||
|
*/
|
||||||
|
void decoupleHorizontalAndVecticalAcceleration(bool val) { _decouple_horizontal_and_vertical_acceleration = val; }
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Get the controllers output local position setpoint
|
* Get the controllers output local position setpoint
|
||||||
* These setpoints are the ones which were executed on including PID output and feed-forward.
|
* These setpoints are the ones which were executed on including PID output and feed-forward.
|
||||||
|
@ -211,6 +216,7 @@ private:
|
||||||
float _lim_tilt{}; ///< Maximum tilt from level the output attitude is allowed to have
|
float _lim_tilt{}; ///< Maximum tilt from level the output attitude is allowed to have
|
||||||
|
|
||||||
float _hover_thrust{}; ///< Thrust [HOVER_THRUST_MIN, HOVER_THRUST_MAX] with which the vehicle hovers not accelerating down or up with level orientation
|
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{false}; ///< Ignore vertical acceleration setpoint to remove its effect on the tilt setpoint
|
||||||
|
|
||||||
// States
|
// States
|
||||||
matrix::Vector3f _pos; /**< current position */
|
matrix::Vector3f _pos; /**< current position */
|
||||||
|
|
|
@ -138,3 +138,13 @@ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f);
|
||||||
* @group Multicopter Position Control
|
* @group Multicopter Position Control
|
||||||
*/
|
*/
|
||||||
PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.f);
|
PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.f);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Acceleration to tilt coupling
|
||||||
|
*
|
||||||
|
* Set to decouple tilt from vertical acceleration.
|
||||||
|
*
|
||||||
|
* @boolean
|
||||||
|
* @group Multicopter Position Control
|
||||||
|
*/
|
||||||
|
PARAM_DEFINE_INT32(MPC_ACC_DECOUPLE, 1);
|
||||||
|
|
Loading…
Reference in New Issue