mirror of https://github.com/ArduPilot/ardupilot
parent
ad5fd361f0
commit
3ba1c0c4aa
|
@ -44,14 +44,28 @@ static void acro_run()
|
|||
// 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)
|
||||
{
|
||||
|
||||
// Calculate trainer mode earth frame rate command for roll
|
||||
// expo variables
|
||||
static float _expo_factor = 0.3;
|
||||
float rp_in, rp_in3, rp_out;
|
||||
|
||||
float rate_limit;
|
||||
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
|
||||
rate_bf_request.x = roll_in * g.acro_rp_p;
|
||||
rate_bf_request.y = pitch_in * g.acro_rp_p;
|
||||
rp_in = float(roll_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.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;
|
||||
|
||||
// calculate earth frame rate corrections to pull the copter back to level while in ACRO mode
|
||||
|
|
Loading…
Reference in New Issue