/*
* Attitude rates controller.
* Input: '_rates_sp' vector, '_thrust_sp'
* Output: '_att_control' vector
*/
void
MulticopterAttitudeControl::control_attitude_rates(float dt)
{
/* reset integral if disarmed */
if (!_armed.armed) {
_rates_int.zero();
}
/* current body angular rates */
math::Vector<3> rates;
rates(0) = _v_att.rollspeed;
rates(1) = _v_att.pitchspeed;
rates(2) = _v_att.yawspeed;
/* angular rates error */
math::Vector<3> rates_err = _rates_sp - rates;
_att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int;
_rates_prev = rates;
/* update integral only if not saturated on low limit */
if (_thrust_sp > MIN_TAKEOFF_THRUST) {
for (int i = 0; i < 3; i++) {
if (fabsf(_att_control(i)) < _thrust_sp) {
float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
_att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
_rates_int(i) = rate_i;
}
}
}
}
}
/*
* Attitude rates controller.
* Input: '_rates_sp' vector, '_thrust_sp'
* Output: '_att_control' vector
*/
void
MulticopterAttitudeControl::control_attitude_rates(float dt)
{
/* reset integral if disarmed */
if (!_armed.armed || !_vehicle_status.is_rotary_wing) {
_rates_int.zero();
}
/* current body angular rates */
math::Vector<3> rates;
rates(0) = _ctrl_state.roll_rate;
rates(1) = _ctrl_state.pitch_rate;
rates(2) = _ctrl_state.yaw_rate;
/* angular rates error */
math::Vector<3> rates_err = _rates_sp - rates;
_att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int +
_params.rate_ff.emult(_rates_sp - _rates_sp_prev) / dt;
_rates_sp_prev = _rates_sp;
_rates_prev = rates;
/* update integral only if not saturated on low limit and if motor commands are not saturated */
if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit) {
for (int i = 0; i < 3; i++) {
if (fabsf(_att_control(i)) < _thrust_sp) {
float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
if (PX4_ISFINITE(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
_att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
_rates_int(i) = rate_i;
}
}
}
}
}
/*
* Attitude rates controller.
* Input: '_rates_sp' vector, '_thrust_sp'
* Output: '_att_control' vector
*/
void
MulticopterAttitudeControl::control_attitude_rates(float dt)
{
/* reset integral if disarmed */
if (!_armed.armed || !_vehicle_status.is_rotary_wing) {
_rates_int.zero();
}
/* current body angular rates */
math::Vector<3> rates;
rates(0) = _ctrl_state.roll_rate;
rates(1) = _ctrl_state.pitch_rate;
rates(2) = _ctrl_state.yaw_rate;
/* throttle pid attenuation factor */
float tpa = fmaxf(0.0f, fminf(1.0f, 1.0f - _params.tpa_slope * (fabsf(_v_rates_sp.thrust) - _params.tpa_breakpoint)));
/* angular rates error */
math::Vector<3> rates_err = _rates_sp - rates;
_att_control = _params.rate_p.emult(rates_err * tpa) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int +
_params.rate_ff.emult(_rates_sp);
_rates_sp_prev = _rates_sp;
_rates_prev = rates;
/* update integral only if not saturated on low limit and if motor commands are not saturated */
if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit) {
for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) {
if (fabsf(_att_control(i)) < _thrust_sp) {
float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
if (PX4_ISFINITE(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
_att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT &&
/* if the axis is the yaw axis, do not update the integral if the limit is hit */
!((i == AXIS_INDEX_YAW) && _motor_limits.yaw)) {
_rates_int(i) = rate_i;
}
}
}
}
}
/*
* Attitude rates controller.
* Input: '_rates_sp' vector, '_thrust_sp'
* Output: '_att_control' vector
*/
void
MulticopterAttitudeControl::control_attitude_rates(float dt)
{
/* reset integral if disarmed */
if (!_armed.armed || !_vehicle_status.is_rotary_wing) {
_rates_int.zero();
}
/* get transformation matrix from sensor/board to body frame */
get_rot_matrix((enum Rotation)_params.board_rotation, &_board_rotation);
/* fine tune the rotation */
math::Matrix<3, 3> board_rotation_offset;
board_rotation_offset.from_euler(M_DEG_TO_RAD_F * _params.board_offset[0],
M_DEG_TO_RAD_F * _params.board_offset[1],
M_DEG_TO_RAD_F * _params.board_offset[2]);
_board_rotation = board_rotation_offset * _board_rotation;
// get the raw gyro data and correct for thermal errors
math::Vector<3> rates(_sensor_gyro.x * _sensor_correction.gyro_scale[0] + _sensor_correction.gyro_offset[0],
_sensor_gyro.y * _sensor_correction.gyro_scale[1] + _sensor_correction.gyro_offset[1],
_sensor_gyro.z * _sensor_correction.gyro_scale[2] + _sensor_correction.gyro_offset[2]);
// rotate corrected measurements from sensor to body frame
rates = _board_rotation * rates;
// correct for in-run bias errors
rates(0) -= _ctrl_state.roll_rate_bias;
rates(1) -= _ctrl_state.pitch_rate_bias;
rates(2) -= _ctrl_state.yaw_rate_bias;
math::Vector<3> rates_p_scaled = _params.rate_p.emult(pid_attenuations(_params.tpa_breakpoint_p, _params.tpa_rate_p));
math::Vector<3> rates_i_scaled = _params.rate_i.emult(pid_attenuations(_params.tpa_breakpoint_i, _params.tpa_rate_i));
math::Vector<3> rates_d_scaled = _params.rate_d.emult(pid_attenuations(_params.tpa_breakpoint_d, _params.tpa_rate_d));
/* angular rates error */
math::Vector<3> rates_err = _rates_sp - rates;
_att_control = rates_p_scaled.emult(rates_err) +
_rates_int +
rates_d_scaled.emult(_rates_prev - rates) / dt +
_params.rate_ff.emult(_rates_sp);
_rates_sp_prev = _rates_sp;
_rates_prev = rates;
/* update integral only if motors are providing enough thrust to be effective */
if (_thrust_sp > MIN_TAKEOFF_THRUST) {
for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) {
// Check for positive control saturation
bool positive_saturation =
((i == AXIS_INDEX_ROLL) && _saturation_status.flags.roll_pos) ||
((i == AXIS_INDEX_PITCH) && _saturation_status.flags.pitch_pos) ||
((i == AXIS_INDEX_YAW) && _saturation_status.flags.yaw_pos);
// Check for negative control saturation
bool negative_saturation =
((i == AXIS_INDEX_ROLL) && _saturation_status.flags.roll_neg) ||
((i == AXIS_INDEX_PITCH) && _saturation_status.flags.pitch_neg) ||
((i == AXIS_INDEX_YAW) && _saturation_status.flags.yaw_neg);
// prevent further positive control saturation
if (positive_saturation) {
rates_err(i) = math::min(rates_err(i), 0.0f);
}
// prevent further negative control saturation
if (negative_saturation) {
rates_err(i) = math::max(rates_err(i), 0.0f);
}
// Perform the integration using a first order method and do not propaate the result if out of range or invalid
float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
if (PX4_ISFINITE(rate_i) && rate_i > -_params.rate_int_lim(i) && rate_i < _params.rate_int_lim(i)) {
_rates_int(i) = rate_i;
}
}
}
/* explicitly limit the integrator state */
for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) {
_rates_int(i) = math::constrain(_rates_int(i), -_params.rate_int_lim(i), _params.rate_int_lim(i));
}
}
