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