|
|
@ -33,15 +33,7 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] PROGMEM = { |
|
|
|
// @User: Advanced
|
|
|
|
// @User: Advanced
|
|
|
|
AP_GROUPINFO("SLEW_YAW", 2, AC_AttitudeControl_Heli, _slew_yaw, AC_ATTITUDE_CONTROL_SLEW_YAW_DEFAULT), |
|
|
|
AP_GROUPINFO("SLEW_YAW", 2, AC_AttitudeControl_Heli, _slew_yaw, AC_ATTITUDE_CONTROL_SLEW_YAW_DEFAULT), |
|
|
|
|
|
|
|
|
|
|
|
// @Param: ACCEL_RP_MAX
|
|
|
|
// 3 was for ACCEL_RP_MAX
|
|
|
|
// @DisplayName: Acceleration Max for Roll/Pitch
|
|
|
|
|
|
|
|
// @Description: Maximum acceleration in roll/pitch axis
|
|
|
|
|
|
|
|
// @Units: Centi-Degrees/Sec/Sec
|
|
|
|
|
|
|
|
// @Range: 0 180000
|
|
|
|
|
|
|
|
// @Increment: 1000
|
|
|
|
|
|
|
|
// @Values: 0:Disabled, 72000:Slow, 108000:Medium, 162000:Fast
|
|
|
|
|
|
|
|
// @User: Advanced
|
|
|
|
|
|
|
|
AP_GROUPINFO("ACCEL_RP_MAX", 3, AC_AttitudeControl_Heli, _accel_rp_max, AC_ATTITUDE_CONTROL_ACCEL_RP_MAX_DEFAULT), |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// @Param: ACCEL_Y_MAX
|
|
|
|
// @Param: ACCEL_Y_MAX
|
|
|
|
// @DisplayName: Acceleration Max for Yaw
|
|
|
|
// @DisplayName: Acceleration Max for Yaw
|
|
|
@ -51,7 +43,7 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] PROGMEM = { |
|
|
|
// @Values: 0:Disabled, 18000:Slow, 36000:Medium, 54000:Fast
|
|
|
|
// @Values: 0:Disabled, 18000:Slow, 36000:Medium, 54000:Fast
|
|
|
|
// @Increment: 1000
|
|
|
|
// @Increment: 1000
|
|
|
|
// @User: Advanced
|
|
|
|
// @User: Advanced
|
|
|
|
AP_GROUPINFO("ACCEL_Y_MAX", 4, AC_AttitudeControl_Heli, _accel_y_max, AC_ATTITUDE_CONTROL_ACCEL_Y_MAX_DEFAULT), |
|
|
|
AP_GROUPINFO("ACCEL_Y_MAX", 4, AC_AttitudeControl_Heli, _accel_yaw_max, AC_ATTITUDE_CONTROL_ACCEL_Y_MAX_DEFAULT), |
|
|
|
|
|
|
|
|
|
|
|
// @Param: RATE_FF_ENAB
|
|
|
|
// @Param: RATE_FF_ENAB
|
|
|
|
// @DisplayName: Rate Feedforward Enable
|
|
|
|
// @DisplayName: Rate Feedforward Enable
|
|
|
@ -60,6 +52,26 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] PROGMEM = { |
|
|
|
// @User: Advanced
|
|
|
|
// @User: Advanced
|
|
|
|
AP_GROUPINFO("RATE_FF_ENAB", 5, AC_AttitudeControl_Heli, _rate_bf_ff_enabled, AC_ATTITUDE_CONTROL_RATE_BF_FF_DEFAULT), |
|
|
|
AP_GROUPINFO("RATE_FF_ENAB", 5, AC_AttitudeControl_Heli, _rate_bf_ff_enabled, AC_ATTITUDE_CONTROL_RATE_BF_FF_DEFAULT), |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// @Param: ACCEL_R_MAX
|
|
|
|
|
|
|
|
// @DisplayName: Acceleration Max for Roll
|
|
|
|
|
|
|
|
// @Description: Maximum acceleration in roll axis
|
|
|
|
|
|
|
|
// @Units: Centi-Degrees/Sec/Sec
|
|
|
|
|
|
|
|
// @Range: 0 180000
|
|
|
|
|
|
|
|
// @Increment: 1000
|
|
|
|
|
|
|
|
// @Values: 0:Disabled, 72000:Slow, 108000:Medium, 162000:Fast
|
|
|
|
|
|
|
|
// @User: Advanced
|
|
|
|
|
|
|
|
AP_GROUPINFO("ACCEL_R_MAX", 6, AC_AttitudeControl_Heli, _accel_roll_max, AC_ATTITUDE_CONTROL_ACCEL_RP_MAX_DEFAULT), |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// @Param: ACCEL_P_MAX
|
|
|
|
|
|
|
|
// @DisplayName: Acceleration Max for Pitch
|
|
|
|
|
|
|
|
// @Description: Maximum acceleration in pitch axis
|
|
|
|
|
|
|
|
// @Units: Centi-Degrees/Sec/Sec
|
|
|
|
|
|
|
|
// @Range: 0 180000
|
|
|
|
|
|
|
|
// @Increment: 1000
|
|
|
|
|
|
|
|
// @Values: 0:Disabled, 72000:Slow, 108000:Medium, 162000:Fast
|
|
|
|
|
|
|
|
// @User: Advanced
|
|
|
|
|
|
|
|
AP_GROUPINFO("ACCEL_P_MAX", 7, AC_AttitudeControl_Heli, _accel_pitch_max, AC_ATTITUDE_CONTROL_ACCEL_RP_MAX_DEFAULT), |
|
|
|
|
|
|
|
|
|
|
|
AP_GROUPEND |
|
|
|
AP_GROUPEND |
|
|
|
}; |
|
|
|
}; |
|
|
|
|
|
|
|
|
|
|
@ -78,8 +90,8 @@ void AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(float roll_pass |
|
|
|
_rate_bf_desired.y = _ahrs.get_gyro().y * AC_ATTITUDE_CONTROL_DEGX100; |
|
|
|
_rate_bf_desired.y = _ahrs.get_gyro().y * AC_ATTITUDE_CONTROL_DEGX100; |
|
|
|
|
|
|
|
|
|
|
|
// accel limit desired yaw rate
|
|
|
|
// accel limit desired yaw rate
|
|
|
|
if (_accel_y_max > 0.0f) { |
|
|
|
if (_accel_yaw_max > 0.0f) { |
|
|
|
float rate_change_limit = _accel_y_max * _dt; |
|
|
|
float rate_change_limit = _accel_yaw_max * _dt; |
|
|
|
float rate_change = yaw_rate_bf - _rate_bf_desired.z; |
|
|
|
float rate_change = yaw_rate_bf - _rate_bf_desired.z; |
|
|
|
rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit); |
|
|
|
rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit); |
|
|
|
_rate_bf_desired.z += rate_change; |
|
|
|
_rate_bf_desired.z += rate_change; |
|
|
|