aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: Fix Rate expo calculation

Browse Source 
This fixes a bug in the yaw rate expo and makes these changes consistent with roll and pitch rate expo.
This commit is contained in:
Leonard Hall 2020-04-05 16:12:32 +09:30 committed by Randy Mackay
parent 95f896b556
commit 7fe700b738
2 changed files with 8 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduCopter/Attitude.cpp
View File

@ -10,18 +10,16 @@ float Copter::get_pilot_desired_yaw_rate(int16_t stick_angle)
} }
float yaw_request; float yaw_request;
// range check expo
g2.acro_y_expo = constrain_float(g2.acro_y_expo, 0.0f, 1.0f);
// calculate yaw rate request // calculate yaw rate request
if (g2.acro_y_expo <= 0) { if (is_zero(g2.acro_y_expo)) {
yaw_request = stick_angle * g.acro_yaw_p; yaw_request = stick_angle * g.acro_yaw_p;
} else { } else {
// expo variables // expo variables
float y_in, y_in3, y_out; float y_in, y_in3, y_out;
// range check expo
if (g2.acro_y_expo > 1.0f || g2.acro_y_expo < 0.5f) {
g2.acro_y_expo = 1.0f;
}
// yaw expo // yaw expo
y_in = float(stick_angle)/ROLL_PITCH_YAW_INPUT_MAX; y_in = float(stick_angle)/ROLL_PITCH_YAW_INPUT_MAX;
y_in3 = y_in*y_in*y_in; y_in3 = y_in*y_in*y_in;

10
ArduCopter/mode_acro.cpp
View File

@ -82,20 +82,18 @@ void ModeAcro::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in,
roll_in *= ratio; roll_in *= ratio;
pitch_in *= ratio; pitch_in *= ratio;
} }
// range check expo
g.acro_rp_p = constrain_float(g.acro_rp_p, 0.0f, 1.0f);
// calculate roll, pitch rate requests // calculate roll, pitch rate requests
if (g.acro_rp_expo <= 0) { if (is_zero(g.acro_rp_expo)) {
rate_bf_request.x = roll_in * g.acro_rp_p; rate_bf_request.x = roll_in * g.acro_rp_p;
rate_bf_request.y = pitch_in * g.acro_rp_p; rate_bf_request.y = pitch_in * g.acro_rp_p;
} else { } else {
// expo variables // expo variables
float rp_in, rp_in3, rp_out; float rp_in, rp_in3, rp_out;
// range check expo
if (g.acro_rp_expo > 1.0f) {
g.acro_rp_expo = 1.0f;
}
// roll expo // roll expo
rp_in = float(roll_in)/ROLL_PITCH_YAW_INPUT_MAX; rp_in = float(roll_in)/ROLL_PITCH_YAW_INPUT_MAX;
rp_in3 = rp_in*rp_in*rp_in; rp_in3 = rp_in*rp_in*rp_in;