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

Copter: apply yaw expo to all modes

This commit is contained in:
Leonard Hall 2016-10-23 17:25:33 +10:30 committed by Randy Mackay
parent 160df59c79
commit 2698f14d39
2 changed files with 22 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
ArduCopter/Attitude.cpp
View File

@ -43,8 +43,28 @@ void Copter::get_pilot_desired_lean_angles(float roll_in, float pitch_in, float
// returns desired yaw rate in centi-degrees per second // returns desired yaw rate in centi-degrees per second
float Copter::get_pilot_desired_yaw_rate(int16_t stick_angle) float Copter::get_pilot_desired_yaw_rate(int16_t stick_angle)
{ {
float yaw_request;
// calculate yaw rate request
if (g2.acro_y_expo <= 0) {
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_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_INPUT_MAX * y_out * g.acro_yaw_p;
}
// convert pilot input to the desired yaw rate // convert pilot input to the desired yaw rate
return stick_angle * g.acro_yaw_p; return yaw_request;
} }
/************************************************************* /*************************************************************

19
ArduCopter/control_acro.cpp
View File

@ -95,24 +95,7 @@ void Copter::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, in
} }
// calculate yaw rate request // calculate yaw rate request
if (g.acro_rp_expo <= 0) { rate_bf_request.z = get_pilot_desired_yaw_rate(yaw_in);
rate_bf_request.z = yaw_in * 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(yaw_in)/ROLL_PITCH_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);
rate_bf_request.z = ROLL_PITCH_INPUT_MAX * y_out * g.acro_yaw_p;
}
// calculate earth frame rate corrections to pull the copter back to level while in ACRO mode // calculate earth frame rate corrections to pull the copter back to level while in ACRO mode