Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AttControlHeli: separate accel max for roll, pitch

Browse Source 
renamed _accel_y_max to _accel_yaw_max
This commit is contained in:
Randy Mackay 2015-03-04 16:23:20 +09:00
parent 51455af51a
commit e4d48fdc92
1 changed files with 24 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
View File

@ -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;