Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Copter: improve shaping of rate controlled axes #20307

Merged
merged 10 commits into from
Jun 27, 2022
Merged

Copter: improve shaping of rate controlled axes #20307

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
efd8ea3
AC_AttitudeControl: incorporate sqrt controller in rate shaping
bnsgeyer Mar 14, 2022
7f9b152
AC_AttitudeControl: only use rate shaping tc if tc is nonzero
bnsgeyer Apr 17, 2022
a221081
AC_AttitudeControl: add command model class for parameters
bnsgeyer Apr 20, 2022
3f9695a
AC_AttitudeControl: add comments
bnsgeyer May 22, 2022
98072c7
Copter: add support for command model class
bnsgeyer Jun 15, 2022
af9a137
AC_AttitudeControl: modify constructor to allow defaults
bnsgeyer Jun 15, 2022
ad7bd40
Copter: delete unused param variable declarations
bnsgeyer Jun 22, 2022
3e3576d
AC_AttitudeControl: incorporate suggested changes
bnsgeyer Jun 22, 2022
7ef0501
AC_AttitudeControl: move input_shaping_rate into input_shaping_ang_vel
bnsgeyer Jun 24, 2022
58ad787
Copter: make rate time constants default to zero
bnsgeyer Jun 24, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions ArduCopter/Copter.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,7 @@
#include <AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h> // 6DoF Attitude control library
#include <AC_AttitudeControl/AC_AttitudeControl_Heli.h> // Attitude control library for traditional helicopter
#include <AC_AttitudeControl/AC_PosControl.h> // Position control library
#include <AC_AttitudeControl/AC_CommandModel.h> // Command model library
#include <AP_Motors/AP_Motors.h> // AP Motors library
#include <AP_Stats/AP_Stats.h> // statistics library
#include <Filter/Filter.h> // Filter library
Expand Down
130 changes: 92 additions & 38 deletions ArduCopter/Parameters.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -443,21 +443,15 @@ const AP_Param::Info Copter::var_info[] = {
GSCALAR(acro_balance_pitch, "ACRO_BAL_PITCH", ACRO_BALANCE_PITCH),
#endif

// ACRO_RP_EXPO moved to Command Model class

#if MODE_ACRO_ENABLED == ENABLED
// @Param: ACRO_TRAINER
// @DisplayName: Acro Trainer
// @Description: Type of trainer used in acro mode
// @Values: 0:Disabled,1:Leveling,2:Leveling and Limited
// @User: Advanced
GSCALAR(acro_trainer, "ACRO_TRAINER", (uint8_t)ModeAcro::Trainer::LIMITED),

// @Param: ACRO_RP_EXPO
// @DisplayName: Acro Roll/Pitch Expo
// @Description: Acro roll/pitch Expo to allow faster rotation when stick at edges
// @Values: 0:Disabled,0.1:Very Low,0.2:Low,0.3:Medium,0.4:High,0.5:Very High
// @Range: -0.5 0.95
// @User: Advanced
GSCALAR(acro_rp_expo, "ACRO_RP_EXPO", ACRO_RP_EXPO_DEFAULT),
#endif

// variables not in the g class which contain EEPROM saved variables
Expand Down Expand Up @@ -807,13 +801,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
AP_SUBGROUPINFO(proximity, "PRX", 8, ParametersG2, AP_Proximity),
#endif

// @Param: ACRO_Y_EXPO
// @DisplayName: Acro Yaw Expo
// @Description: Acro yaw expo to allow faster rotation when stick at edges
// @Values: 0:Disabled,0.1:Very Low,0.2:Low,0.3:Medium,0.4:High,0.5:Very High
// @Range: -1.0 0.95
// @User: Advanced
AP_GROUPINFO("ACRO_Y_EXPO", 9, ParametersG2, acro_y_expo, ACRO_Y_EXPO_DEFAULT),
// ACRO_Y_EXPO (9) moved to Command Model Class

#if MODE_ACRO_ENABLED == ENABLED
// @Param: ACRO_THR_MID
Expand Down Expand Up @@ -1056,14 +1044,53 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
AP_GROUPINFO("GUID_TIMEOUT", 46, ParametersG2, guided_timeout, 3.0),
#endif

// ACRO_PR_RATE (47), ACRO_Y_RATE (48), PILOT_Y_RATE (49) and PILOT_Y_EXPO (50) moved to command model class

// @Param: SURFTRAK_MODE
// @DisplayName: Surface Tracking Mode
// @Description: set which surface to track in surface tracking
// @Values: 0:Do not track, 1:Ground, 2:Ceiling
// @User: Advanced
AP_GROUPINFO("SURFTRAK_MODE", 51, ParametersG2, surftrak_mode, (uint8_t)Copter::SurfaceTracking::Surface::GROUND),

// @Param: FS_DR_ENABLE
// @DisplayName: DeadReckon Failsafe Action
// @Description: Failsafe action taken immediately as deadreckoning starts. Deadreckoning starts when EKF loses position and velocity source and relies only on wind estimates
// @Values: 0:Disabled/NoAction,1:Land, 2:RTL, 3:SmartRTL or RTL, 4:SmartRTL or Land, 6:Auto DO_LAND_START or RTL
// @User: Standard
AP_GROUPINFO("FS_DR_ENABLE", 52, ParametersG2, failsafe_dr_enable, (uint8_t)Copter::FailsafeAction::RTL),

// @Param: FS_DR_TIMEOUT
// @DisplayName: DeadReckon Failsafe Timeout
// @Description: DeadReckoning is available for this many seconds after losing position and/or velocity source. After this timeout elapses the EKF failsafe will trigger in modes requiring a position estimate
// @Range: 0 120
// @User: Standard
AP_GROUPINFO("FS_DR_TIMEOUT", 53, ParametersG2, failsafe_dr_timeout, 30),

#if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED
// @Param: ACRO_RP_RATE
// @DisplayName: Acro Roll and Pitch Rate
// @Description: Acro mode maximum roll and pitch rate. Higher values mean faster rate of rotation
// @Units: deg/s
// @Range: 1 1080
// @User: Standard
AP_GROUPINFO("ACRO_RP_RATE", 47, ParametersG2, acro_rp_rate, ACRO_RP_RATE_DEFAULT),

// @Param: ACRO_RP_EXPO
// @DisplayName: Acro Roll/Pitch Expo
// @Description: Acro roll/pitch Expo to allow faster rotation when stick at edges
// @Values: 0:Disabled,0.1:Very Low,0.2:Low,0.3:Medium,0.4:High,0.5:Very High
// @Range: -0.5 0.95
// @User: Advanced

// @Param: ACRO_RP_RATE_TC
// @DisplayName: Acro roll/pitch rate control input time constant
// @Description: Acro roll and pitch rate control input time constant. Low numbers lead to sharper response, higher numbers to softer response
// @Units: s
// @Range: 0 1
// @Increment: 0.01
// @Values: 0.5:Very Soft, 0.2:Soft, 0.15:Medium, 0.1:Crisp, 0.05:Very Crisp
// @User: Standard
AP_SUBGROUPINFO(command_model_acro_rp, "ACRO_RP_", 54, ParametersG2, AC_CommandModel),
#endif

#if MODE_ACRO_ENABLED == ENABLED || MODE_DRIFT_ENABLED == ENABLED
Expand All @@ -1073,7 +1100,23 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Units: deg/s
// @Range: 1 360
// @User: Standard
AP_GROUPINFO("ACRO_Y_RATE", 48, ParametersG2, acro_y_rate, ACRO_Y_RATE_DEFAULT),

// @Param: ACRO_Y_EXPO
// @DisplayName: Acro Yaw Expo
// @Description: Acro yaw expo to allow faster rotation when stick at edges
// @Values: 0:Disabled,0.1:Very Low,0.2:Low,0.3:Medium,0.4:High,0.5:Very High
// @Range: -1.0 0.95
// @User: Advanced

// @Param: ACRO_Y_RATE_TC
// @DisplayName: Acro yaw rate control input time constant
// @Description: Acro yaw rate control input time constant. Low numbers lead to sharper response, higher numbers to softer response
// @Units: s
// @Range: 0 1
// @Increment: 0.01
// @Values: 0.5:Very Soft, 0.2:Soft, 0.15:Medium, 0.1:Crisp, 0.05:Very Crisp
// @User: Standard
AP_SUBGROUPINFO(command_model_acro_y, "ACRO_Y_", 55, ParametersG2, AC_CommandModel),
#endif

// @Param: PILOT_Y_RATE
Expand All @@ -1082,36 +1125,23 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Units: deg/s
// @Range: 1 360
// @User: Standard
AP_GROUPINFO("PILOT_Y_RATE", 49, ParametersG2, pilot_y_rate, PILOT_Y_RATE_DEFAULT),

// @Param: PILOT_Y_EXPO
// @DisplayName: Pilot controlled yaw expo
// @Description: Pilot controlled yaw expo to allow faster rotation when stick at edges
// @Values: 0:Disabled,0.1:Very Low,0.2:Low,0.3:Medium,0.4:High,0.5:Very High
// @Range: -0.5 1.0
// @User: Advanced
AP_GROUPINFO("PILOT_Y_EXPO", 50, ParametersG2, pilot_y_expo, PILOT_Y_EXPO_DEFAULT),

// @Param: SURFTRAK_MODE
// @DisplayName: Surface Tracking Mode
// @Description: set which surface to track in surface tracking
// @Values: 0:Do not track, 1:Ground, 2:Ceiling
// @User: Advanced
AP_GROUPINFO("SURFTRAK_MODE", 51, ParametersG2, surftrak_mode, (uint8_t)Copter::SurfaceTracking::Surface::GROUND),

// @Param: FS_DR_ENABLE
// @DisplayName: DeadReckon Failsafe Action
// @Description: Failsafe action taken immediately as deadreckoning starts. Deadreckoning starts when EKF loses position and velocity source and relies only on wind estimates
// @Values: 0:Disabled/NoAction,1:Land, 2:RTL, 3:SmartRTL or RTL, 4:SmartRTL or Land, 6:Auto DO_LAND_START or RTL
// @User: Standard
AP_GROUPINFO("FS_DR_ENABLE", 52, ParametersG2, failsafe_dr_enable, (uint8_t)Copter::FailsafeAction::RTL),

// @Param: FS_DR_TIMEOUT
// @DisplayName: DeadReckon Failsafe Timeout
// @Description: DeadReckoning is available for this many seconds after losing position and/or velocity source. After this timeout elapses the EKF failsafe will trigger in modes requiring a position estimate
// @Range: 0 120
// @Param: PILOT_Y_RATE_TC
// @DisplayName: Pilot yaw rate control input time constant
// @Description: Pilot yaw rate control input time constant. Low numbers lead to sharper response, higher numbers to softer response
// @Units: s
// @Range: 0 1
// @Increment: 0.01
// @Values: 0.5:Very Soft, 0.2:Soft, 0.15:Medium, 0.1:Crisp, 0.05:Very Crisp
// @User: Standard
AP_GROUPINFO("FS_DR_TIMEOUT", 53, ParametersG2, failsafe_dr_timeout, 30),
AP_SUBGROUPINFO(command_model_pilot, "PILOT_Y_", 56, ParametersG2, AC_CommandModel),

bnsgeyer marked this conversation as resolved.
Show resolved Hide resolved
AP_GROUPEND
};
Expand Down Expand Up @@ -1157,6 +1187,16 @@ ParametersG2::ParametersG2(void)
#if MODE_ZIGZAG_ENABLED == ENABLED
,mode_zigzag_ptr(&copter.mode_zigzag)
#endif

#if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED
,command_model_acro_rp(ACRO_RP_RATE_DEFAULT, ACRO_RP_EXPO_DEFAULT, 0.0f)
#endif

#if MODE_ACRO_ENABLED == ENABLED || MODE_DRIFT_ENABLED == ENABLED
,command_model_acro_y(ACRO_Y_RATE_DEFAULT, ACRO_Y_EXPO_DEFAULT, 0.0f)
#endif

,command_model_pilot(PILOT_Y_RATE_DEFAULT, PILOT_Y_EXPO_DEFAULT, 0.0f)
{
AP_Param::setup_object_defaults(this, var_info);
}
Expand Down Expand Up @@ -1453,6 +1493,20 @@ void Copter::convert_pid_parameters(void)
AP_Param::convert_old_parameter(&info, 45.0);
}

// convert rate and expo command model parameters for Copter-4.3
// PARAMETER_CONVERSION - Added: June-2022
const AP_Param::ConversionInfo cmd_mdl_conversion_info[] = {
{ Parameters::k_param_g2, 47, AP_PARAM_FLOAT, "ACRO_RP_RATE" },
{ Parameters::k_param_acro_rp_expo, 0, AP_PARAM_FLOAT, "ACRO_RP_EXPO" },
{ Parameters::k_param_g2, 48, AP_PARAM_FLOAT, "ACRO_Y_RATE" },
{ Parameters::k_param_g2, 9, AP_PARAM_FLOAT, "ACRO_Y_EXPO" },
{ Parameters::k_param_g2, 49, AP_PARAM_FLOAT, "PILOT_Y_RATE" },
{ Parameters::k_param_g2, 50, AP_PARAM_FLOAT, "PILOT_Y_EXPO" },
};
for (const auto &info : cmd_mdl_conversion_info) {
AP_Param::convert_old_parameter(&info, 1.0);
}

// make any SRV_Channel upgrades needed
SRV_Channels::upgrade_parameters();
}
Expand Down
27 changes: 12 additions & 15 deletions ArduCopter/Parameters.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -128,7 +128,7 @@ class Parameters {
k_param_rangefinder, // rangefinder object
k_param_fs_ekf_thresh,
k_param_terrain,
k_param_acro_rp_expo,
k_param_acro_rp_expo, // deprecated - remove
k_param_throttle_deadzone,
k_param_optflow,
k_param_dcmcheck_thresh, // deprecated - remove
Expand Down Expand Up @@ -469,7 +469,6 @@ class Parameters {
#if MODE_ACRO_ENABLED == ENABLED
// Acro parameters
AP_Int8 acro_trainer;
AP_Float acro_rp_expo;
#endif

// Note: keep initializers here in the same order as they are declared
Expand Down Expand Up @@ -539,8 +538,6 @@ class ParametersG2 {
// developer options
AP_Int32 dev_options;

// acro exponent parameters
AP_Float acro_y_expo;
#if MODE_ACRO_ENABLED == ENABLED
AP_Float acro_thr_mid;
#endif
Expand Down Expand Up @@ -627,6 +624,17 @@ class ParametersG2 {
void *mode_zigzag_ptr;
#endif

// command model parameters
#if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED
AC_CommandModel command_model_acro_rp;
#endif

#if MODE_ACRO_ENABLED == ENABLED || MODE_DRIFT_ENABLED == ENABLED
AC_CommandModel command_model_acro_y;
#endif

AC_CommandModel command_model_pilot;

#if MODE_ACRO_ENABLED == ENABLED
AP_Int8 acro_options;
#endif
Expand Down Expand Up @@ -655,17 +663,6 @@ class ParametersG2 {
AP_Float guided_timeout;
#endif

#if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED
// Acro parameters
AP_Float acro_rp_rate;
#endif

#if MODE_ACRO_ENABLED == ENABLED || MODE_DRIFT_ENABLED == ENABLED
AP_Float acro_y_rate;
#endif

AP_Float pilot_y_rate;
AP_Float pilot_y_expo;
AP_Int8 surftrak_mode;
AP_Int8 failsafe_dr_enable;
AP_Int16 failsafe_dr_timeout;
Expand Down
17 changes: 16 additions & 1 deletion ArduCopter/mode.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -207,6 +207,10 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
// return immediately if we are already in the desired mode
if (mode == flightmode->mode_number()) {
control_mode_reason = reason;
// set yaw rate time constant during autopilot startup
if (reason == ModeReason::INITIALISED && mode == Mode::Number::STABILIZE) {
bnsgeyer marked this conversation as resolved.
Show resolved Hide resolved
attitude_control->set_yaw_rate_tc(g2.command_model_pilot.get_rate_tc());
}
// make happy noise
if (copter.ap.initialised && (reason != last_reason)) {
AP_Notify::events.user_mode_change = 1;
Expand Down Expand Up @@ -319,6 +323,17 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
camera.set_is_auto_mode(flightmode->mode_number() == Mode::Number::AUTO);
#endif

// set rate shaping time constants
#if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED
attitude_control->set_roll_pitch_rate_tc(g2.command_model_acro_rp.get_rate_tc());
#endif
attitude_control->set_yaw_rate_tc(g2.command_model_pilot.get_rate_tc());
#if MODE_ACRO_ENABLED == ENABLED || MODE_DRIFT_ENABLED == ENABLED
if (mode== Mode::Number::ACRO || mode== Mode::Number::DRIFT) {
attitude_control->set_yaw_rate_tc(g2.command_model_acro_y.get_rate_tc());
}
#endif

// update notify object
notify_flight_mode();

Expand Down Expand Up @@ -962,7 +977,7 @@ float Mode::get_pilot_desired_yaw_rate(float yaw_in)
}

// convert pilot input to the desired yaw rate
return g2.pilot_y_rate * 100.0 * input_expo(yaw_in, g2.pilot_y_expo);
return g2.command_model_pilot.get_rate() * 100.0 * input_expo(yaw_in, g2.command_model_pilot.get_expo());
}

// pass-through functions to reduce code churn on conversion;
Expand Down
14 changes: 7 additions & 7 deletions ArduCopter/mode_acro.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -113,13 +113,13 @@ void ModeAcro::get_pilot_desired_angle_rates(float roll_in, float pitch_in, floa
// calculate roll, pitch rate requests

// roll expo
rate_bf_request_cd.x = g2.acro_rp_rate * 100.0 * input_expo(roll_in, g.acro_rp_expo);
rate_bf_request_cd.x = g2.command_model_acro_rp.get_rate() * 100.0 * input_expo(roll_in, g2.command_model_acro_rp.get_expo());

// pitch expo
rate_bf_request_cd.y = g2.acro_rp_rate * 100.0 * input_expo(pitch_in, g.acro_rp_expo);
rate_bf_request_cd.y = g2.command_model_acro_rp.get_rate() * 100.0 * input_expo(pitch_in, g2.command_model_acro_rp.get_expo());

// yaw expo
rate_bf_request_cd.z = g2.acro_y_rate * 100.0 * input_expo(yaw_in, g2.acro_y_expo);
rate_bf_request_cd.z = g2.command_model_acro_y.get_rate() * 100.0 * input_expo(yaw_in, g2.command_model_acro_y.get_expo());

// calculate earth frame rate corrections to pull the copter back to level while in ACRO mode

Expand All @@ -143,15 +143,15 @@ void ModeAcro::get_pilot_desired_angle_rates(float roll_in, float pitch_in, floa
if (g.acro_trainer == (uint8_t)Trainer::LIMITED) {
const float angle_max = copter.aparm.angle_max;
if (roll_angle > angle_max){
rate_ef_level_cd.x += sqrt_controller(angle_max - roll_angle, g2.acro_rp_rate * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_roll_max_cdss(), G_Dt);
rate_ef_level_cd.x += sqrt_controller(angle_max - roll_angle, g2.command_model_acro_rp.get_rate() * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_roll_max_cdss(), G_Dt);
}else if (roll_angle < -angle_max) {
rate_ef_level_cd.x += sqrt_controller(-angle_max - roll_angle, g2.acro_rp_rate * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_roll_max_cdss(), G_Dt);
rate_ef_level_cd.x += sqrt_controller(-angle_max - roll_angle, g2.command_model_acro_rp.get_rate() * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_roll_max_cdss(), G_Dt);
}

if (pitch_angle > angle_max){
rate_ef_level_cd.y += sqrt_controller(angle_max - pitch_angle, g2.acro_rp_rate * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_pitch_max_cdss(), G_Dt);
rate_ef_level_cd.y += sqrt_controller(angle_max - pitch_angle, g2.command_model_acro_rp.get_rate() * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_pitch_max_cdss(), G_Dt);
}else if (pitch_angle < -angle_max) {
rate_ef_level_cd.y += sqrt_controller(-angle_max - pitch_angle, g2.acro_rp_rate * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_pitch_max_cdss(), G_Dt);
rate_ef_level_cd.y += sqrt_controller(-angle_max - pitch_angle, g2.command_model_acro_rp.get_rate() * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_pitch_max_cdss(), G_Dt);
}
}

Expand Down
2 changes: 1 addition & 1 deletion ArduCopter/mode_drift.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@ void ModeDrift::run()

// gain scheduling for yaw
float pitch_vel2 = MIN(fabsf(pitch_vel), 2000);
float target_yaw_rate = target_roll * (1.0f - (pitch_vel2 / 5000.0f)) * g2.acro_y_rate / 45.0;
float target_yaw_rate = target_roll * (1.0f - (pitch_vel2 / 5000.0f)) * g2.command_model_acro_y.get_rate() / 45.0;

roll_vel = constrain_float(roll_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT);
pitch_vel = constrain_float(pitch_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT);
Expand Down
12 changes: 6 additions & 6 deletions ArduCopter/mode_sport.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,8 +34,8 @@ void ModeSport::run()
// get pilot's desired roll and pitch rates

// calculate rate requests
float target_roll_rate = channel_roll->get_control_in() * g2.acro_rp_rate * 100.0 / ROLL_PITCH_YAW_INPUT_MAX;
float target_pitch_rate = channel_pitch->get_control_in() * g2.acro_rp_rate * 100.0 / ROLL_PITCH_YAW_INPUT_MAX;
float target_roll_rate = channel_roll->get_control_in() * g2.command_model_acro_rp.get_rate() * 100.0 / ROLL_PITCH_YAW_INPUT_MAX;
float target_pitch_rate = channel_pitch->get_control_in() * g2.command_model_acro_rp.get_rate() * 100.0 / ROLL_PITCH_YAW_INPUT_MAX;

// get attitude targets
const Vector3f att_target = attitude_control->get_att_target_euler_cd();
Expand All @@ -50,15 +50,15 @@ void ModeSport::run()

const float angle_max = copter.aparm.angle_max;
if (roll_angle > angle_max){
target_roll_rate += sqrt_controller(angle_max - roll_angle, g2.acro_rp_rate * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_roll_max_cdss(), G_Dt);
target_roll_rate += sqrt_controller(angle_max - roll_angle, g2.command_model_acro_rp.get_rate() * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_roll_max_cdss(), G_Dt);
}else if (roll_angle < -angle_max) {
target_roll_rate += sqrt_controller(-angle_max - roll_angle, g2.acro_rp_rate * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_roll_max_cdss(), G_Dt);
target_roll_rate += sqrt_controller(-angle_max - roll_angle, g2.command_model_acro_rp.get_rate() * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_roll_max_cdss(), G_Dt);
}

if (pitch_angle > angle_max){
target_pitch_rate += sqrt_controller(angle_max - pitch_angle, g2.acro_rp_rate * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_pitch_max_cdss(), G_Dt);
target_pitch_rate += sqrt_controller(angle_max - pitch_angle, g2.command_model_acro_rp.get_rate() * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_pitch_max_cdss(), G_Dt);
}else if (pitch_angle < -angle_max) {
target_pitch_rate += sqrt_controller(-angle_max - pitch_angle, g2.acro_rp_rate * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_pitch_max_cdss(), G_Dt);
target_pitch_rate += sqrt_controller(-angle_max - pitch_angle, g2.command_model_acro_rp.get_rate() * 100.0 / ACRO_LEVEL_MAX_OVERSHOOT, attitude_control->get_accel_pitch_max_cdss(), G_Dt);
}

// get pilot's desired yaw rate
Expand Down
Loading