diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index 77edea2a1f..1d47521cfd 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp @@ -33,15 +33,7 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] PROGMEM = { // @User: Advanced AP_GROUPINFO("SLEW_YAW", 2, AC_AttitudeControl_Heli, _slew_yaw, AC_ATTITUDE_CONTROL_SLEW_YAW_DEFAULT), - // @Param: 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), + // 3 was for ACCEL_RP_MAX // @Param: ACCEL_Y_MAX // @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 // @Increment: 1000 // @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 // @DisplayName: Rate Feedforward Enable @@ -60,6 +52,26 @@ const AP_Param::GroupInfo AC_AttitudeControl_Heli::var_info[] PROGMEM = { // @User: Advanced 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 }; @@ -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; // accel limit desired yaw rate - if (_accel_y_max > 0.0f) { - float rate_change_limit = _accel_y_max * _dt; + if (_accel_yaw_max > 0.0f) { + float rate_change_limit = _accel_yaw_max * _dt; float rate_change = yaw_rate_bf - _rate_bf_desired.z; rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit); _rate_bf_desired.z += rate_change;