Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 09:28:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: ACRO add expo

Browse Source 
adds 30% expo to roll and pitch
This commit is contained in:
lthall 2014-08-02 18:27:42 +09:30 committed by Randy Mackay
parent ad5fd361f0
commit 3ba1c0c4aa
1 changed files with 18 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
ArduCopter/control_acro.pde
View File

@ -44,14 +44,28 @@ static void acro_run()
// returns desired angle rates in centi-degrees-per-second // returns desired angle rates in centi-degrees-per-second
static 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) static 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)
{ {
// expo variables
static float _expo_factor = 0.3;
float rp_in, rp_in3, rp_out;
// Calculate trainer mode earth frame rate command for roll
float rate_limit; float rate_limit;
Vector3f rate_ef_level, rate_bf_level, rate_bf_request; Vector3f rate_ef_level, rate_bf_level, rate_bf_request;
// range check the input
roll_in = constrain_int16(roll_in, -ROLL_PITCH_INPUT_MAX, ROLL_PITCH_INPUT_MAX);
pitch_in = constrain_int16(pitch_in, -ROLL_PITCH_INPUT_MAX, ROLL_PITCH_INPUT_MAX);
// calculate rate requests // calculate rate requests
rate_bf_request.x = roll_in * g.acro_rp_p; rp_in = float(roll_in)/ROLL_PITCH_INPUT_MAX;
rate_bf_request.y = pitch_in * g.acro_rp_p; rp_in3 = rp_in*rp_in*rp_in;
rp_out = (_expo_factor * rp_in3) + ((1 - _expo_factor) * rp_in);
rate_bf_request.x = ROLL_PITCH_INPUT_MAX * rp_out * g.acro_rp_p;
rp_in = float(pitch_in)/ROLL_PITCH_INPUT_MAX;
rp_in3 = rp_in*rp_in*rp_in;
rp_out = (_expo_factor * rp_in3) + ((1 - _expo_factor) * rp_in);
rate_bf_request.y = ROLL_PITCH_INPUT_MAX * rp_out * g.acro_rp_p;
rate_bf_request.z = yaw_in * g.acro_yaw_p; rate_bf_request.z = yaw_in * 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