int
MulticopterAttitudeControl::parameters_update()
{
float v;
float roll_tc, pitch_tc;
param_get(_params_handles.roll_tc, &roll_tc);
param_get(_params_handles.pitch_tc, &pitch_tc);
/* roll gains */
param_get(_params_handles.roll_p, &v);
_params.att_p(0) = v * (ATTITUDE_TC_DEFAULT / roll_tc);
param_get(_params_handles.roll_rate_p, &v);
_params.rate_p(0) = v * (ATTITUDE_TC_DEFAULT / roll_tc);
param_get(_params_handles.roll_rate_i, &v);
_params.rate_i(0) = v;
param_get(_params_handles.roll_rate_d, &v);
_params.rate_d(0) = v * (ATTITUDE_TC_DEFAULT / roll_tc);
param_get(_params_handles.roll_rate_ff, &v);
_params.rate_ff(0) = v;
/* pitch gains */
param_get(_params_handles.pitch_p, &v);
_params.att_p(1) = v * (ATTITUDE_TC_DEFAULT / pitch_tc);
param_get(_params_handles.pitch_rate_p, &v);
_params.rate_p(1) = v * (ATTITUDE_TC_DEFAULT / pitch_tc);
param_get(_params_handles.pitch_rate_i, &v);
_params.rate_i(1) = v;
param_get(_params_handles.pitch_rate_d, &v);
_params.rate_d(1) = v * (ATTITUDE_TC_DEFAULT / pitch_tc);
param_get(_params_handles.pitch_rate_ff, &v);
_params.rate_ff(1) = v;
/* yaw gains */
param_get(_params_handles.yaw_p, &v);
_params.att_p(2) = v;
param_get(_params_handles.yaw_rate_p, &v);
_params.rate_p(2) = v;
param_get(_params_handles.yaw_rate_i, &v);
_params.rate_i(2) = v;
param_get(_params_handles.yaw_rate_d, &v);
_params.rate_d(2) = v;
param_get(_params_handles.yaw_rate_ff, &v);
_params.rate_ff(2) = v;
param_get(_params_handles.yaw_ff, &_params.yaw_ff);
/* angular rate limits */
param_get(_params_handles.roll_rate_max, &_params.roll_rate_max);
_params.mc_rate_max(0) = math::radians(_params.roll_rate_max);
param_get(_params_handles.pitch_rate_max, &_params.pitch_rate_max);
_params.mc_rate_max(1) = math::radians(_params.pitch_rate_max);
param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max);
_params.mc_rate_max(2) = math::radians(_params.yaw_rate_max);
/* auto angular rate limits */
param_get(_params_handles.roll_rate_max, &_params.roll_rate_max);
_params.auto_rate_max(0) = math::radians(_params.roll_rate_max);
param_get(_params_handles.pitch_rate_max, &_params.pitch_rate_max);
_params.auto_rate_max(1) = math::radians(_params.pitch_rate_max);
param_get(_params_handles.yaw_auto_max, &_params.yaw_auto_max);
_params.auto_rate_max(2) = math::radians(_params.yaw_auto_max);
/* manual rate control scale and auto mode roll/pitch rate limits */
param_get(_params_handles.acro_roll_max, &v);
_params.acro_rate_max(0) = math::radians(v);
param_get(_params_handles.acro_pitch_max, &v);
_params.acro_rate_max(1) = math::radians(v);
param_get(_params_handles.acro_yaw_max, &v);
_params.acro_rate_max(2) = math::radians(v);
/* stick deflection needed in rattitude mode to control rates not angles */
param_get(_params_handles.rattitude_thres, &_params.rattitude_thres);
param_get(_params_handles.vtol_type, &_params.vtol_type);
int tmp;
param_get(_params_handles.vtol_opt_recovery_enabled, &tmp);
_params.vtol_opt_recovery_enabled = (bool)tmp;
param_get(_params_handles.vtol_wv_yaw_rate_scale, &_params.vtol_wv_yaw_rate_scale);
_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
return OK;
}
/**
* Attitude controller.
* Input: 'vehicle_attitude_setpoint' topics (depending on mode)
* Output: '_rates_sp' vector, '_thrust_sp'
*/
void
MulticopterAttitudeControl::control_attitude(float dt)
{
vehicle_attitude_setpoint_poll();
_thrust_sp = _v_att_sp.thrust;
/* construct attitude setpoint rotation matrix */
math::Matrix<3, 3> R_sp;
R_sp.set(_v_att_sp.R_body);
/* get current rotation matrix from control state quaternions */
math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
math::Matrix<3, 3> R = q_att.to_dcm();
/* all input data is ready, run controller itself */
/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2));
math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
/* axis and sin(angle) of desired rotation */
math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);
/* calculate angle error */
float e_R_z_sin = e_R.length();
float e_R_z_cos = R_z * R_sp_z;
/* calculate weight for yaw control */
float yaw_w = R_sp(2, 2) * R_sp(2, 2);
/* calculate rotation matrix after roll/pitch only rotation */
math::Matrix<3, 3> R_rp;
if (e_R_z_sin > 0.0f) {
/* get axis-angle representation */
float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);
math::Vector<3> e_R_z_axis = e_R / e_R_z_sin;
e_R = e_R_z_axis * e_R_z_angle;
/* cross product matrix for e_R_axis */
math::Matrix<3, 3> e_R_cp;
e_R_cp.zero();
e_R_cp(0, 1) = -e_R_z_axis(2);
e_R_cp(0, 2) = e_R_z_axis(1);
e_R_cp(1, 0) = e_R_z_axis(2);
e_R_cp(1, 2) = -e_R_z_axis(0);
e_R_cp(2, 0) = -e_R_z_axis(1);
e_R_cp(2, 1) = e_R_z_axis(0);
/* rotation matrix for roll/pitch only rotation */
R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
} else {
/* zero roll/pitch rotation */
R_rp = R;
}
/* R_rp and R_sp has the same Z axis, calculate yaw error */
math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;
if (e_R_z_cos < 0.0f) {
/* for large thrust vector rotations use another rotation method:
* calculate angle and axis for R -> R_sp rotation directly */
math::Quaternion q;
q.from_dcm(R.transposed() * R_sp);
math::Vector<3> e_R_d = q.imag();
e_R_d.normalize();
e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));
/* use fusion of Z axis based rotation and direct rotation */
float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;
}
/* calculate angular rates setpoint */
_rates_sp = _params.att_p.emult(e_R);
/* limit rates */
for (int i = 0; i < 3; i++) {
if (_v_control_mode.flag_control_velocity_enabled && !_v_control_mode.flag_control_manual_enabled) {
_rates_sp(i) = math::constrain(_rates_sp(i), -_params.auto_rate_max(i), _params.auto_rate_max(i));
} else {
_rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i));
}
}
/* weather-vane mode, dampen yaw rate */
if (_v_att_sp.disable_mc_yaw_control == true && _v_control_mode.flag_control_velocity_enabled && !_v_control_mode.flag_control_manual_enabled) {
float wv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale;
_rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max);
// prevent integrator winding up in weathervane mode
_rates_int(2) = 0.0f;
}
/* feed forward yaw setpoint rate */
_rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff;
/* weather-vane mode, scale down yaw rate */
if (_v_att_sp.disable_mc_yaw_control == true && _v_control_mode.flag_control_velocity_enabled && !_v_control_mode.flag_control_manual_enabled) {
float wv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale;
_rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max);
// prevent integrator winding up in weathervane mode
_rates_int(2) = 0.0f;
}
}
int
MulticopterAttitudeControl::parameters_update()
{
float v;
float roll_tc, pitch_tc;
param_get(_params_handles.roll_tc, &roll_tc);
param_get(_params_handles.pitch_tc, &pitch_tc);
/* roll gains */
param_get(_params_handles.roll_p, &v);
_params.att_p(0) = v * (ATTITUDE_TC_DEFAULT / roll_tc);
param_get(_params_handles.roll_rate_p, &v);
_params.rate_p(0) = v * (ATTITUDE_TC_DEFAULT / roll_tc);
param_get(_params_handles.roll_rate_i, &v);
_params.rate_i(0) = v;
param_get(_params_handles.roll_rate_integ_lim, &v);
_params.rate_int_lim(0) = v;
param_get(_params_handles.roll_rate_d, &v);
_params.rate_d(0) = v * (ATTITUDE_TC_DEFAULT / roll_tc);
param_get(_params_handles.roll_rate_ff, &v);
_params.rate_ff(0) = v;
/* pitch gains */
param_get(_params_handles.pitch_p, &v);
_params.att_p(1) = v * (ATTITUDE_TC_DEFAULT / pitch_tc);
param_get(_params_handles.pitch_rate_p, &v);
_params.rate_p(1) = v * (ATTITUDE_TC_DEFAULT / pitch_tc);
param_get(_params_handles.pitch_rate_i, &v);
_params.rate_i(1) = v;
param_get(_params_handles.pitch_rate_integ_lim, &v);
_params.rate_int_lim(1) = v;
param_get(_params_handles.pitch_rate_d, &v);
_params.rate_d(1) = v * (ATTITUDE_TC_DEFAULT / pitch_tc);
param_get(_params_handles.pitch_rate_ff, &v);
_params.rate_ff(1) = v;
param_get(_params_handles.tpa_breakpoint_p, &_params.tpa_breakpoint_p);
param_get(_params_handles.tpa_breakpoint_i, &_params.tpa_breakpoint_i);
param_get(_params_handles.tpa_breakpoint_d, &_params.tpa_breakpoint_d);
param_get(_params_handles.tpa_rate_p, &_params.tpa_rate_p);
param_get(_params_handles.tpa_rate_i, &_params.tpa_rate_i);
param_get(_params_handles.tpa_rate_d, &_params.tpa_rate_d);
/* yaw gains */
param_get(_params_handles.yaw_p, &v);
_params.att_p(2) = v;
param_get(_params_handles.yaw_rate_p, &v);
_params.rate_p(2) = v;
param_get(_params_handles.yaw_rate_i, &v);
_params.rate_i(2) = v;
param_get(_params_handles.yaw_rate_integ_lim, &v);
_params.rate_int_lim(2) = v;
param_get(_params_handles.yaw_rate_d, &v);
_params.rate_d(2) = v;
param_get(_params_handles.yaw_rate_ff, &v);
_params.rate_ff(2) = v;
param_get(_params_handles.yaw_ff, &_params.yaw_ff);
/* angular rate limits */
param_get(_params_handles.roll_rate_max, &_params.roll_rate_max);
_params.mc_rate_max(0) = math::radians(_params.roll_rate_max);
param_get(_params_handles.pitch_rate_max, &_params.pitch_rate_max);
_params.mc_rate_max(1) = math::radians(_params.pitch_rate_max);
param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max);
_params.mc_rate_max(2) = math::radians(_params.yaw_rate_max);
/* auto angular rate limits */
param_get(_params_handles.roll_rate_max, &_params.roll_rate_max);
_params.auto_rate_max(0) = math::radians(_params.roll_rate_max);
param_get(_params_handles.pitch_rate_max, &_params.pitch_rate_max);
_params.auto_rate_max(1) = math::radians(_params.pitch_rate_max);
param_get(_params_handles.yaw_auto_max, &_params.yaw_auto_max);
_params.auto_rate_max(2) = math::radians(_params.yaw_auto_max);
/* manual rate control scale and auto mode roll/pitch rate limits */
param_get(_params_handles.acro_roll_max, &v);
_params.acro_rate_max(0) = math::radians(v);
param_get(_params_handles.acro_pitch_max, &v);
_params.acro_rate_max(1) = math::radians(v);
param_get(_params_handles.acro_yaw_max, &v);
_params.acro_rate_max(2) = math::radians(v);
/* stick deflection needed in rattitude mode to control rates not angles */
param_get(_params_handles.rattitude_thres, &_params.rattitude_thres);
param_get(_params_handles.vtol_type, &_params.vtol_type);
int tmp;
param_get(_params_handles.vtol_opt_recovery_enabled, &tmp);
_params.vtol_opt_recovery_enabled = (bool)tmp;
param_get(_params_handles.vtol_wv_yaw_rate_scale, &_params.vtol_wv_yaw_rate_scale);
param_get(_params_handles.bat_scale_en, &_params.bat_scale_en);
_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
/* rotation of the autopilot relative to the body */
param_get(_params_handles.board_rotation, &(_params.board_rotation));
/* fine adjustment of the rotation */
param_get(_params_handles.board_offset[0], &(_params.board_offset[0]));
param_get(_params_handles.board_offset[1], &(_params.board_offset[1]));
param_get(_params_handles.board_offset[2], &(_params.board_offset[2]));
return OK;
}
