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

Copter: Fix Rate expo calculation 

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 Andrew Tridgell
parent 768dcb6768
commit c90e31fc75
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;
// range check expo
g2.acro_y_expo = constrain_float(g2.acro_y_expo, 0.0f, 1.0f);
// 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;
} else {
// expo variables
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
y_in = float(stick_angle)/ROLL_PITCH_YAW_INPUT_MAX;
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;
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
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.y = pitch_in * g.acro_rp_p;
} else {
// expo variables
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
rp_in = float(roll_in)/ROLL_PITCH_YAW_INPUT_MAX;
rp_in3 = rp_in*rp_in*rp_in;