Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: Update ACRO to use rate parameters and update expo function

This commit is contained in:
Leonard Hall 2021-09-17 15:24:19 +09:30 committed by Randy Mackay
parent 77fbcf1741
commit 5d5ccc8c78
28 changed files with 128 additions and 156 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
ArduCopter/Parameters.cpp
View File

@ -336,7 +336,7 @@ const AP_Param::Info Copter::var_info[] = {
// @DisplayName: Channel 6 Tuning
// @Description: Controls which parameters (normally PID gains) are being tuned with transmitter's channel 6 knob
// @User: Standard
// @Values: 0:None,1:Stab Roll/Pitch kP,4:Rate Roll/Pitch kP,5:Rate Roll/Pitch kI,21:Rate Roll/Pitch kD,3:Stab Yaw kP,6:Rate Yaw kP,26:Rate Yaw kD,56:Rate Yaw Filter,55:Motor Yaw Headroom,14:AltHold kP,7:Throttle Rate kP,34:Throttle Accel kP,35:Throttle Accel kI,36:Throttle Accel kD,12:Loiter Pos kP,22:Velocity XY kP,28:Velocity XY kI,10:WP Speed,25:Acro RollPitch kP,40:Acro Yaw kP,45:RC Feel,13:Heli Ext Gyro,38:Declination,39:Circle Rate,46:Rate Pitch kP,47:Rate Pitch kI,48:Rate Pitch kD,49:Rate Roll kP,50:Rate Roll kI,51:Rate Roll kD,52:Rate Pitch FF,53:Rate Roll FF,54:Rate Yaw FF,58:SysID Magnitude
// @Values: 0:None,1:Stab Roll/Pitch kP,4:Rate Roll/Pitch kP,5:Rate Roll/Pitch kI,21:Rate Roll/Pitch kD,3:Stab Yaw kP,6:Rate Yaw kP,26:Rate Yaw kD,56:Rate Yaw Filter,55:Motor Yaw Headroom,14:AltHold kP,7:Throttle Rate kP,34:Throttle Accel kP,35:Throttle Accel kI,36:Throttle Accel kD,12:Loiter Pos kP,22:Velocity XY kP,28:Velocity XY kI,10:WP Speed,25:Acro Roll/Pitch deg/s,40:Acro Yaw deg/s,45:RC Feel,13:Heli Ext Gyro,38:Declination,39:Circle Rate,46:Rate Pitch kP,47:Rate Pitch kI,48:Rate Pitch kD,49:Rate Roll kP,50:Rate Roll kI,51:Rate Roll kD,52:Rate Pitch FF,53:Rate Roll FF,54:Rate Yaw FF,58:SysID Magnitude
GSCALAR(radio_tuning, "TUNE", 0),
// @Param: FRAME_TYPE
@ -429,14 +429,12 @@ const AP_Param::Info Copter::var_info[] = {
// @Description: Converts pilot roll and pitch into a desired rate of rotation in ACRO and SPORT mode. Higher values mean faster rate of rotation.
// @Range: 1 10
// @User: Standard
GSCALAR(acro_rp_p, "ACRO_RP_P", ACRO_RP_P),
// @Param: ACRO_YAW_P
// @DisplayName: Acro Yaw P gain
// @Description: Converts pilot yaw input into a desired rate of rotation. Higher values mean faster rate of rotation.
// @Range: 1 10
// @User: Standard
GSCALAR(acro_yaw_p, "ACRO_YAW_P", ACRO_YAW_P),
#if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED
// @Param: ACRO_BAL_ROLL
@ -468,7 +466,7 @@ const AP_Param::Info Copter::var_info[] = {
// @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 1.0
// @Range: -0.5 0.95
// @User: Advanced
GSCALAR(acro_rp_expo, "ACRO_RP_EXPO", ACRO_RP_EXPO_DEFAULT),
#endif
@ -824,7 +822,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @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: -0.5 1.0
// @Range: -1.0 0.95
// @User: Advanced
AP_GROUPINFO("ACRO_Y_EXPO", 9, ParametersG2, acro_y_expo, ACRO_Y_EXPO_DEFAULT),
@ -1069,17 +1067,19 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
AP_GROUPINFO("GUID_TIMEOUT", 46, ParametersG2, guided_timeout, 3.0),
#endif
#if MODE_ACRO_ENABLED == ENABLED
#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
// @Range: 1 360
// @Units: deg/s
// @Range: 1 1080
// @User: Standard
AP_GROUPINFO("ACRO_RP_RATE", 47, ParametersG2, acro_rp_rate, ACRO_RP_RATE_DEFAULT),
// @Param: ACRO_Y_RATE
// @DisplayName: Acro Yaw Rate
// @Description: Acro mode maximum yaw rate. Higher value means faster rate of rotation
// @Units: deg/s
// @Range: 1 360
// @User: Standard
AP_GROUPINFO("ACRO_Y_RATE", 48, ParametersG2, acro_y_rate, ACRO_Y_RATE_DEFAULT),
@ -1088,15 +1088,17 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Param: PILOT_Y_RATE
// @DisplayName: Pilot controlled yaw rate
// @Description: Pilot controlled yaw rate max. Used in all pilot controlled modes except Acro
// @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. Used in all pilot controlled modes except Acrxo
// @Range: 1 360
// @User: Standard
// @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),
AP_GROUPEND

23
ArduCopter/Parameters.h
View File

@ -339,8 +339,8 @@ public:
//
// 220: PI/D Controllers
//
k_param_acro_rp_p = 221,
k_param_axis_lock_p, // remove
k_param_acro_rp_p = 221, // remove
k_param_axis_lock_p, // remove
k_param_pid_rate_roll, // remove
k_param_pid_rate_pitch, // remove
k_param_pid_rate_yaw, // remove
@ -460,13 +460,17 @@ public:
AP_Int16 rc_speed; // speed of fast RC Channels in Hz
#if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED
// Acro parameters
AP_Float acro_rp_p;
AP_Float acro_yaw_p;
AP_Float acro_balance_roll;
AP_Float acro_balance_pitch;
#endif
#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
// above.
@ -651,13 +655,14 @@ public:
AP_Float guided_timeout;
#endif
#if MODE_ACRO_ENABLED == ENABLED
AP_Int16 acro_rp_rate;
AP_Int16 acro_y_rate;
#if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED
// Acro parameters
AP_Float acro_rp_rate;
AP_Float acro_y_rate;
#endif
AP_Int16 pilot_y_rate;
AP_Int16 pilot_y_expo;
AP_Float pilot_y_rate;
AP_Float pilot_y_expo;
};
extern const AP_Param::Info var_info[];

28
ArduCopter/config.h
View File

@ -450,16 +450,12 @@
//
// Acro Mode
#ifndef ACRO_RP_P
# define ACRO_RP_P 4.5f
#endif
#ifndef ACRO_YAW_P
# define ACRO_YAW_P 4.5f
#endif
#ifndef ACRO_LEVEL_MAX_ANGLE
# define ACRO_LEVEL_MAX_ANGLE 3000
# define ACRO_LEVEL_MAX_ANGLE 3000 // maximum lean angle in trainer mode measured in centidegrees
#endif
#ifndef ACRO_LEVEL_MAX_OVERSHOOT
# define ACRO_LEVEL_MAX_OVERSHOOT 1000 // maximum overshoot angle in trainer mode when full roll or pitch stick is held in centidegrees
#endif
#ifndef ACRO_BALANCE_ROLL
@ -471,11 +467,11 @@
#endif
#ifndef ACRO_RP_EXPO_DEFAULT
#define ACRO_RP_EXPO_DEFAULT 0.3f
#define ACRO_RP_EXPO_DEFAULT 0.3f // ACRO roll and pitch expo parameter default
#endif
#ifndef ACRO_Y_EXPO_DEFAULT
#define ACRO_Y_EXPO_DEFAULT 0.0f
#define ACRO_Y_EXPO_DEFAULT 0.0f // ACRO yaw expo parameter default
#endif
#ifndef ACRO_THR_MID_DEFAULT
@ -483,16 +479,16 @@
#endif
#ifndef ACRO_RP_RATE_DEFAULT
#define ACRO_RP_RATE_DEFAULT 90
#define ACRO_RP_RATE_DEFAULT 360 // ACRO roll and pitch rotation rate parameter default in deg/s
#endif
#ifndef ACRO_Y_RATE_DEFAULT
#define ACRO_Y_RATE_DEFAULT 90
#define ACRO_Y_RATE_DEFAULT 202.5 // ACRO yaw rotation rate parameter default in deg/s
#endif
// RTL Mode
#ifndef RTL_ALT_FINAL
# define RTL_ALT_FINAL 0 // the altitude the vehicle will move to as the final stage of Returning to Launch. Set to zero to land.
# define RTL_ALT_FINAL 0 // the altitude, in cm, the vehicle will move to as the final stage of Returning to Launch. Set to zero to land.
#endif
#ifndef RTL_ALT
@ -584,10 +580,10 @@
#endif
#ifndef PILOT_Y_RATE_DEFAULT
# define PILOT_Y_RATE_DEFAULT 90 // pilot controlled yaw rotation rate parameter default
# define PILOT_Y_RATE_DEFAULT 202.5 // yaw rotation rate parameter default in deg/s for all mode except ACRO
#endif
#ifndef PILOT_Y_EXPO_DEFAULT
# define PILOT_Y_EXPO_DEFAULT 90 // pilot controlled yaw expo parameter default
# define PILOT_Y_EXPO_DEFAULT 0.0 // yaw expo parameter default for all mode except ACRO
#endif
#ifndef AUTO_DISARMING_DELAY

4
ArduCopter/defines.h
View File

@ -43,7 +43,7 @@ enum tuning_func {
TUNING_ALTITUDE_HOLD_KP = 14, // altitude hold controller's P term (alt error to desired rate)
TUNING_RATE_ROLL_PITCH_KD = 21, // body frame roll/pitch rate controller's D term
TUNING_VEL_XY_KP = 22, // loiter rate controller's P term (speed error to tilt angle)
TUNING_ACRO_RP_KP = 25, // acro controller's P term. converts pilot input to a desired roll, pitch or yaw rate
TUNING_ACRO_RP_RATE = 25, // acro controller's desired roll and pitch rate in deg/s
TUNING_YAW_RATE_KD = 26, // body frame yaw rate controller's D term
TUNING_VEL_XY_KI = 28, // loiter rate controller's I term (speed error to tilt angle)
TUNING_AHRS_YAW_KP = 30, // ahrs's compass effect on yaw angle (0 = very low, 1 = very high)
@ -53,7 +53,7 @@ enum tuning_func {
TUNING_ACCEL_Z_KD = 36, // accel based throttle controller's D term
TUNING_DECLINATION = 38, // compass declination in radians
TUNING_CIRCLE_RATE = 39, // circle turn rate in degrees (hard coded to about 45 degrees in either direction)
TUNING_ACRO_YAW_KP = 40, // acro controller's P term. converts pilot input to a desired roll, pitch or yaw rate
TUNING_ACRO_YAW_RATE = 40, // acro controller's desired yaw rate in deg/s
TUNING_RANGEFINDER_GAIN = 41, // unused
TUNING_EKF_VERTICAL_POS = 42, // unused
TUNING_EKF_HORIZONTAL_POS = 43, // unused

23
ArduCopter/mode.cpp
View File

@ -635,7 +635,7 @@ void Mode::land_run_horizontal_control()
}
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
if (!is_zero(target_yaw_rate)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
@ -918,32 +918,15 @@ Mode::AltHoldModeState Mode::get_alt_hold_state(float target_climb_rate_cms)
// transform pilot's yaw input into a desired yaw rate
// returns desired yaw rate in centi-degrees per second
float Mode::get_pilot_desired_yaw_rate(int16_t stick_angle)
float Mode::get_pilot_desired_yaw_rate(float yaw_in)
{
// throttle failsafe check
if (copter.failsafe.radio || !copter.ap.rc_receiver_present) {
return 0.0f;
}
// range check expo
g2.acro_y_expo = constrain_float(g2.acro_y_expo, -0.5f, 1.0f);
// calculate yaw rate request
float yaw_request;
if (is_zero(g2.acro_y_expo)) {
yaw_request = stick_angle * g.acro_yaw_p;
} else {
// expo variables
float y_in, y_in3, y_out;
// yaw expo
y_in = float(stick_angle)/ROLL_PITCH_YAW_INPUT_MAX;
y_in3 = y_in*y_in*y_in;
y_out = (g2.acro_y_expo * y_in3) + ((1.0f - g2.acro_y_expo) * y_in);
yaw_request = ROLL_PITCH_YAW_INPUT_MAX * y_out * g.acro_yaw_p;
}
// convert pilot input to the desired yaw rate
return yaw_request;
return g2.pilot_y_rate * 100.0 * input_expo(yaw_in, g2.pilot_y_expo);
}
// pass-through functions to reduce code churn on conversion;

6
ArduCopter/mode.h
View File

@ -90,7 +90,7 @@ public:
// pilot input processing
void get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, float angle_max, float angle_limit) const;
float get_pilot_desired_yaw_rate(int16_t stick_angle);
float get_pilot_desired_yaw_rate(float yaw_in);
float get_pilot_desired_throttle() const;
// returns climb target_rate reduced to avoid obstacles and
@ -315,7 +315,9 @@ protected:
const char *name() const override { return "ACRO"; }
const char *name4() const override { return "ACRO"; }
void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out);
// get_pilot_desired_angle_rates - transform pilot's normalised roll pitch and yaw input into a desired lean angle rates
// inputs are -1 to 1 and the function returns desired angle rates in centi-degrees-per-second
void get_pilot_desired_angle_rates(float roll_in, float pitch_in, float yaw_in, float &roll_out, float &pitch_out, float &yaw_out);
float throttle_hover() const override;

94
ArduCopter/mode_acro.cpp
View File

@ -12,7 +12,7 @@ void ModeAcro::run()
{
// convert the input to the desired body frame rate
float target_roll, target_pitch, target_yaw;
get_pilot_desired_angle_rates(channel_roll->get_control_in(), channel_pitch->get_control_in(), channel_yaw->get_control_in(), target_roll, target_pitch, target_yaw);
get_pilot_desired_angle_rates(channel_roll->norm_input_dz(), channel_pitch->norm_input_dz(), channel_yaw->norm_input_dz(), target_roll, target_pitch, target_yaw);
if (!motors->armed()) {
// Motors should be Stopped
@ -94,48 +94,32 @@ float ModeAcro::throttle_hover() const
return Mode::throttle_hover();
}
// get_pilot_desired_angle_rates - transform pilot's roll pitch and yaw input into a desired lean angle rates
// returns desired angle rates in centi-degrees-per-second
void ModeAcro::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out)
// get_pilot_desired_angle_rates - transform pilot's normalised roll pitch and yaw input into a desired lean angle rates
// inputs are -1 to 1 and the function returns desired angle rates in centi-degrees-per-second
void ModeAcro::get_pilot_desired_angle_rates(float roll_in, float pitch_in, float yaw_in, float &roll_out, float &pitch_out, float &yaw_out)
{
float rate_limit;
Vector3f rate_ef_level, rate_bf_level, rate_bf_request;
Vector3f rate_ef_level_cd, rate_bf_level_cd, rate_bf_request_cd;
// apply circular limit to pitch and roll inputs
float total_in = norm(pitch_in, roll_in);
if (total_in > ROLL_PITCH_YAW_INPUT_MAX) {
float ratio = (float)ROLL_PITCH_YAW_INPUT_MAX / total_in;
if (total_in > 1.0) {
float ratio = 1.0 / total_in;
roll_in *= ratio;
pitch_in *= ratio;
}
// range check expo
g.acro_rp_expo = constrain_float(g.acro_rp_expo, -0.5f, 1.0f);
// calculate roll, pitch rate requests
if (is_zero(g.acro_rp_expo)) {
rate_bf_request.x = roll_in * g.acro_rp_p;
rate_bf_request.y = pitch_in * g.acro_rp_p;
} else {
// expo variables
float rp_in, rp_in3, rp_out;
// roll expo
rate_bf_request_cd.x = g2.acro_rp_rate * 100.0 * input_expo(roll_in, g.acro_rp_expo);
// roll expo
rp_in = float(roll_in)/ROLL_PITCH_YAW_INPUT_MAX;
rp_in3 = rp_in*rp_in*rp_in;
rp_out = (g.acro_rp_expo * rp_in3) + ((1.0f - g.acro_rp_expo) * rp_in);
rate_bf_request.x = ROLL_PITCH_YAW_INPUT_MAX * rp_out * g.acro_rp_p;
// pitch expo
rate_bf_request_cd.y = g2.acro_rp_rate * 100.0 * input_expo(pitch_in, g.acro_rp_expo);
// pitch expo
rp_in = float(pitch_in)/ROLL_PITCH_YAW_INPUT_MAX;
rp_in3 = rp_in*rp_in*rp_in;
rp_out = (g.acro_rp_expo * rp_in3) + ((1.0f - g.acro_rp_expo) * rp_in);
rate_bf_request.y = ROLL_PITCH_YAW_INPUT_MAX * rp_out * g.acro_rp_p;
}
// calculate yaw rate request
rate_bf_request.z = get_pilot_desired_yaw_rate(yaw_in);
// yaw expo
rate_bf_request_cd.z = g2.acro_y_rate * 100.0 * input_expo(yaw_in, g2.acro_y_expo);
// calculate earth frame rate corrections to pull the copter back to level while in ACRO mode
@ -146,65 +130,65 @@ void ModeAcro::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in,
// Calculate trainer mode earth frame rate command for roll
int32_t roll_angle = wrap_180_cd(att_target.x);
rate_ef_level.x = -constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_roll;
rate_ef_level_cd.x = -constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_roll;
// Calculate trainer mode earth frame rate command for pitch
int32_t pitch_angle = wrap_180_cd(att_target.y);
rate_ef_level.y = -constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch;
rate_ef_level_cd.y = -constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch;
// Calculate trainer mode earth frame rate command for yaw
rate_ef_level.z = 0;
rate_ef_level_cd.z = 0;
// Calculate angle limiting earth frame rate commands
if (g.acro_trainer == (uint8_t)Trainer::LIMITED) {
const float angle_max = copter.aparm.angle_max;
if (roll_angle > angle_max){
rate_ef_level.x += sqrt_controller(angle_max - roll_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_roll_max_cdss(), G_Dt);
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);
}else if (roll_angle < -angle_max) {
rate_ef_level.x += sqrt_controller(-angle_max - roll_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_roll_max_cdss(), G_Dt);
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);
}
if (pitch_angle > angle_max){
rate_ef_level.y += sqrt_controller(angle_max - pitch_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_pitch_max_cdss(), G_Dt);
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);
}else if (pitch_angle < -angle_max) {
rate_ef_level.y += sqrt_controller(-angle_max - pitch_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_pitch_max_cdss(), G_Dt);
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);
}
}
// convert earth-frame level rates to body-frame level rates
attitude_control->euler_rate_to_ang_vel(attitude_control->get_att_target_euler_cd()*radians(0.01f), rate_ef_level, rate_bf_level);
attitude_control->euler_rate_to_ang_vel(attitude_control->get_att_target_euler_cd() * radians(0.01f), rate_ef_level_cd, rate_bf_level_cd);
// combine earth frame rate corrections with rate requests
if (g.acro_trainer == (uint8_t)Trainer::LIMITED) {
rate_bf_request.x += rate_bf_level.x;
rate_bf_request.y += rate_bf_level.y;
rate_bf_request.z += rate_bf_level.z;
rate_bf_request_cd.x += rate_bf_level_cd.x;
rate_bf_request_cd.y += rate_bf_level_cd.y;
rate_bf_request_cd.z += rate_bf_level_cd.z;
}else{
float acro_level_mix = constrain_float(1-float(MAX(MAX(abs(roll_in), abs(pitch_in)), abs(yaw_in))/4500.0), 0, 1)*ahrs.cos_pitch();
// Scale leveling rates by stick input
rate_bf_level = rate_bf_level*acro_level_mix;
// Scale levelling rates by stick input
rate_bf_level_cd = rate_bf_level_cd * acro_level_mix;
// Calculate rate limit to prevent change of rate through inverted
rate_limit = fabsf(fabsf(rate_bf_request.x)-fabsf(rate_bf_level.x));
rate_bf_request.x += rate_bf_level.x;
rate_bf_request.x = constrain_float(rate_bf_request.x, -rate_limit, rate_limit);
rate_limit = fabsf(fabsf(rate_bf_request_cd.x)-fabsf(rate_bf_level_cd.x));
rate_bf_request_cd.x += rate_bf_level_cd.x;
rate_bf_request_cd.x = constrain_float(rate_bf_request_cd.x, -rate_limit, rate_limit);
// Calculate rate limit to prevent change of rate through inverted
rate_limit = fabsf(fabsf(rate_bf_request.y)-fabsf(rate_bf_level.y));
rate_bf_request.y += rate_bf_level.y;
rate_bf_request.y = constrain_float(rate_bf_request.y, -rate_limit, rate_limit);
rate_limit = fabsf(fabsf(rate_bf_request_cd.y)-fabsf(rate_bf_level_cd.y));
rate_bf_request_cd.y += rate_bf_level_cd.y;
rate_bf_request_cd.y = constrain_float(rate_bf_request_cd.y, -rate_limit, rate_limit);
// Calculate rate limit to prevent change of rate through inverted
rate_limit = fabsf(fabsf(rate_bf_request.z)-fabsf(rate_bf_level.z));
rate_bf_request.z += rate_bf_level.z;
rate_bf_request.z = constrain_float(rate_bf_request.z, -rate_limit, rate_limit);
rate_limit = fabsf(fabsf(rate_bf_request_cd.z)-fabsf(rate_bf_level_cd.z));
rate_bf_request_cd.z += rate_bf_level_cd.z;
rate_bf_request_cd.z = constrain_float(rate_bf_request_cd.z, -rate_limit, rate_limit);
}
}
// hand back rate request
roll_out = rate_bf_request.x;
pitch_out = rate_bf_request.y;
yaw_out = rate_bf_request.z;
roll_out = rate_bf_request_cd.x;
pitch_out = rate_bf_request_cd.y;
yaw_out = rate_bf_request_cd.z;
}
#endif

4
ArduCopter/mode_acro_heli.cpp
View File

@ -69,7 +69,7 @@ void ModeAcro_Heli::run()
if (!motors->has_flybar()){
// convert the input to the desired body frame rate
get_pilot_desired_angle_rates(channel_roll->get_control_in(), channel_pitch->get_control_in(), channel_yaw->get_control_in(), target_roll, target_pitch, target_yaw);
get_pilot_desired_angle_rates(channel_roll->norm_input_dz(), channel_pitch->norm_input_dz(), channel_yaw->norm_input_dz(), target_roll, target_pitch, target_yaw);
// only mimic flybar response when trainer mode is disabled
if ((Trainer)g.acro_trainer.get() == Trainer::OFF) {
// while landed always leak off target attitude to current attitude
@ -111,7 +111,7 @@ void ModeAcro_Heli::run()
// if there is no external gyro then run the usual
// ACRO_YAW_P gain on the input control, including
// deadzone
yaw_in = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
yaw_in = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
}
// run attitude controller

2
ArduCopter/mode_althold.cpp
View File

@ -36,7 +36,7 @@ void ModeAltHold::run()
get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max_cd());
// get pilot's desired yaw rate
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
// get pilot desired climb rate
float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());

6
ArduCopter/mode_auto.cpp
View File

@ -858,7 +858,7 @@ void ModeAuto::wp_run()
float target_yaw_rate = 0;
if (!copter.failsafe.radio && use_pilot_yaw()) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
if (!is_zero(target_yaw_rate)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
@ -927,7 +927,7 @@ void ModeAuto::circle_run()
float target_yaw_rate = 0;
if (!copter.failsafe.radio && use_pilot_yaw()) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
if (!is_zero(target_yaw_rate)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
@ -973,7 +973,7 @@ void ModeAuto::loiter_run()
// accept pilot input of yaw
float target_yaw_rate = 0;
if (!copter.failsafe.radio && use_pilot_yaw()) {
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
}
// set motors to full range

2
ArduCopter/mode_autorotate.cpp
View File

@ -282,7 +282,7 @@ void ModeAutorotate::run()
get_pilot_desired_lean_angles(pilot_roll, pilot_pitch, copter.aparm.angle_max, copter.aparm.angle_max);
// Get pilot's desired yaw rate
float pilot_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
float pilot_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
// Pitch target is calculated in autorotation phase switch above
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pilot_roll, _pitch_target, pilot_yaw_rate);

2
ArduCopter/mode_autotune.cpp
View File

@ -85,7 +85,7 @@ void AutoTune::get_pilot_desired_rp_yrate_cd(float &des_roll_cd, float &des_pitc
{
copter.mode_autotune.get_pilot_desired_lean_angles(des_roll_cd, des_pitch_cd, copter.aparm.angle_max,
copter.attitude_control->get_althold_lean_angle_max_cd());
yaw_rate_cds = copter.mode_autotune.get_pilot_desired_yaw_rate(copter.channel_yaw->get_control_in());
yaw_rate_cds = copter.mode_autotune.get_pilot_desired_yaw_rate(copter.channel_yaw->norm_input_dz());
}
/*

2
ArduCopter/mode_circle.cpp
View File

@ -33,7 +33,7 @@ void ModeCircle::run()
pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
// get pilot's desired yaw rate (or zero if in radio failsafe)
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
if (!is_zero(target_yaw_rate)) {
pilot_yaw_override = true;
}

2
ArduCopter/mode_drift.cpp
View File

@ -54,7 +54,7 @@ void ModeDrift::run()
// gain scheduling for yaw
float pitch_vel2 = MIN(fabsf(pitch_vel), 2000);
float target_yaw_rate = ((float)target_roll/1.0f) * (1.0f - (pitch_vel2 / 5000.0f)) * g.acro_yaw_p;
float target_yaw_rate = target_roll * (1.0f - (pitch_vel2 / 5000.0f)) * g2.acro_y_rate / 45.0;
roll_vel = constrain_float(roll_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT);
pitch_vel = constrain_float(pitch_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT);

2
ArduCopter/mode_flowhold.cpp
View File

@ -246,7 +246,7 @@ void ModeFlowHold::run()
target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), copter.g.pilot_speed_up);
// get pilot's desired yaw rate
float target_yaw_rate = get_pilot_desired_yaw_rate(copter.channel_yaw->get_control_in());
float target_yaw_rate = get_pilot_desired_yaw_rate(copter.channel_yaw->norm_input_dz());
// Flow Hold State Machine Determination
AltHoldModeState flowhold_state = get_alt_hold_state(target_climb_rate);

10
ArduCopter/mode_guided.cpp
View File

@ -168,7 +168,7 @@ void ModeGuided::wp_control_run()
float target_yaw_rate = 0;
if (!copter.failsafe.radio && use_pilot_yaw()) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
if (!is_zero(target_yaw_rate)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
@ -649,7 +649,7 @@ void ModeGuided::pos_control_run()
if (!copter.failsafe.radio && use_pilot_yaw()) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
if (!is_zero(target_yaw_rate)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
@ -708,7 +708,7 @@ void ModeGuided::accel_control_run()
float target_yaw_rate = 0;
if (!copter.failsafe.radio && use_pilot_yaw()) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
if (!is_zero(target_yaw_rate)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
@ -772,7 +772,7 @@ void ModeGuided::velaccel_control_run()
float target_yaw_rate = 0;
if (!copter.failsafe.radio && use_pilot_yaw()) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
if (!is_zero(target_yaw_rate)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
@ -848,7 +848,7 @@ void ModeGuided::posvelaccel_control_run()
if (!copter.failsafe.radio && use_pilot_yaw()) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
if (!is_zero(target_yaw_rate)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
}

2
ArduCopter/mode_land.cpp
View File

@ -114,7 +114,7 @@ void ModeLand::nogps_run()
}
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
if (!is_zero(target_yaw_rate)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
}

2
ArduCopter/mode_loiter.cpp
View File

@ -96,7 +96,7 @@ void ModeLoiter::run()
loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch);
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
// get pilot desired climb rate
target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());

2
ArduCopter/mode_poshold.cpp
View File

@ -83,7 +83,7 @@ void ModePosHold::run()
get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max_cd());
// get pilot's desired yaw rate
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
// get pilot desired climb rate (for alt-hold mode and take-off)
float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());

6
ArduCopter/mode_rtl.cpp
View File

@ -167,7 +167,7 @@ void ModeRTL::climb_return_run()
float target_yaw_rate = 0;
if (!copter.failsafe.radio && use_pilot_yaw()) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
if (!is_zero(target_yaw_rate)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
@ -225,7 +225,7 @@ void ModeRTL::loiterathome_run()
float target_yaw_rate = 0;
if (!copter.failsafe.radio && use_pilot_yaw()) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
if (!is_zero(target_yaw_rate)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
}
@ -332,7 +332,7 @@ void ModeRTL::descent_run()
if (g.land_repositioning || use_pilot_yaw()) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
}
}

2
ArduCopter/mode_smart_rtl.cpp
View File

@ -84,7 +84,7 @@ void ModeSmartRTL::path_follow_run()
float target_yaw_rate = 0.0f;
if (!copter.failsafe.radio && use_pilot_yaw()) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
if (!is_zero(target_yaw_rate)) {
auto_yaw.set_mode(AUTO_YAW_HOLD);
}

14
ArduCopter/mode_sport.cpp
View File

@ -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() * g.acro_rp_p;
float target_pitch_rate = channel_pitch->get_control_in() * g.acro_rp_p;
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;
// get attitude targets
const Vector3f att_target = attitude_control->get_att_target_euler_cd();
@ -50,19 +50,19 @@ 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, g.acro_rp_p * 4.5, attitude_control->get_accel_roll_max_cdss(), G_Dt);
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);
}else if (roll_angle < -angle_max) {
target_roll_rate += sqrt_controller(-angle_max - roll_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_roll_max_cdss(), G_Dt);
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);
}
if (pitch_angle > angle_max){
target_pitch_rate += sqrt_controller(angle_max - pitch_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_pitch_max_cdss(), G_Dt);
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);
}else if (pitch_angle < -angle_max) {
target_pitch_rate += sqrt_controller(-angle_max - pitch_angle, g.acro_rp_p * 4.5, attitude_control->get_accel_pitch_max_cdss(), G_Dt);
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);
}
// get pilot's desired yaw rate
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
// get pilot desired climb rate
float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());

2
ArduCopter/mode_stabilize.cpp
View File

@ -16,7 +16,7 @@ void ModeStabilize::run()
get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max);
// get pilot's desired yaw rate
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
if (!motors->armed()) {
// Motors should be Stopped

2
ArduCopter/mode_stabilize_heli.cpp
View File

@ -29,7 +29,7 @@ void ModeStabilize_Heli::run()
get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max);
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
// get pilot's desired throttle
pilot_throttle_scaled = copter.input_manager.get_pilot_desired_collective(channel_throttle->get_control_in());

2
ArduCopter/mode_systemid.cpp
View File

@ -114,7 +114,7 @@ void ModeSystemId::run()
get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max);
// get pilot's desired yaw rate
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
if (!motors->armed()) {
// Motors should be Stopped

4
ArduCopter/mode_zigzag.cpp
View File

@ -264,7 +264,7 @@ void ModeZigZag::auto_control()
float target_yaw_rate = 0;
if (!copter.failsafe.radio) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
}
// set motors to full range
@ -305,7 +305,7 @@ void ModeZigZag::manual_control()
// process pilot's roll and pitch input
loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch);
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
// get pilot desired climb rate
target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());

2
ArduCopter/takeoff.cpp
View File

@ -110,7 +110,7 @@ void Mode::auto_takeoff_run()
float target_yaw_rate = 0;
if (!copter.failsafe.radio && copter.flightmode->use_pilot_yaw()) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->norm_input_dz());
}
// aircraft stays in landed state until rotor speed runup has finished

12
ArduCopter/tuning.cpp
View File

@ -104,14 +104,14 @@ void Copter::tuning()
wp_nav->set_speed_xy(tuning_value);
break;
// Acro roll pitch gain
case TUNING_ACRO_RP_KP:
g.acro_rp_p = tuning_value;
// Acro roll pitch rates
case TUNING_ACRO_RP_RATE:
g2.acro_rp_rate = tuning_value;
break;
// Acro yaw gain
case TUNING_ACRO_YAW_KP:
g.acro_yaw_p = tuning_value;
// Acro yaw rate
case TUNING_ACRO_YAW_RATE:
g2.acro_y_rate = tuning_value;
break;
#if FRAME_CONFIG == HELI_FRAME