zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Copter: Leonard's control_acro fixes 

get_pilot_desired_angle_rates returns bf rate targets as floats
mission-4.1.18
Randy Mackay 11 years ago committed by Andrew Tridgell
parent
2c6470f87b
commit
bf6bb59cb4
1 changed files with 56 additions and 56 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 20
      ArduCopter/control_acro.pde

20
ArduCopter/control_acro.pde
Unescape View File

@ -16,7 +16,7 @@ static bool acro_init(bool ignore_checks)
// should be called at 100hz or more // should be called at 100hz or more
static void acro_run() static void acro_run()
{ {
int16_t target_roll, target_pitch, target_yaw; float target_roll, target_pitch, target_yaw;
int16_t pilot_throttle_scaled; int16_t pilot_throttle_scaled;
// if motors not running reset angle targets // if motors not running reset angle targets
@ -42,7 +42,7 @@ static void acro_run()
// get_pilot_desired_angle_rates - transform pilot's roll pitch and yaw input into a desired lean angle rates // get_pilot_desired_angle_rates - transform pilot's roll pitch and yaw input into a desired lean angle rates
// 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, int16_t &roll_out, int16_t &pitch_out, int16_t &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)
{ {
// Calculate trainer mode earth frame rate command for roll // Calculate trainer mode earth frame rate command for roll
@ -57,15 +57,14 @@ static void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int
// 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
if (g.acro_trainer != ACRO_TRAINER_DISABLED) {
// Calculate trainer mode earth frame rate command for roll // Calculate trainer mode earth frame rate command for roll
int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor); int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor);
roll_angle = constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE); rate_ef_level.x = -constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_roll;
rate_ef_level.x = -roll_angle * g.acro_balance_roll;
// Calculate trainer mode earth frame rate command for pitch // Calculate trainer mode earth frame rate command for pitch
int32_t pitch_angle = wrap_180_cd(ahrs.pitch_sensor); int32_t pitch_angle = wrap_180_cd(ahrs.pitch_sensor);
pitch_angle = constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE); rate_ef_level.y = -constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch;
rate_ef_level.y = -pitch_angle * g.acro_balance_pitch;
// Calculate trainer mode earth frame rate command for yaw // Calculate trainer mode earth frame rate command for yaw
rate_ef_level.z = 0; rate_ef_level.z = 0;
@ -73,15 +72,15 @@ static void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int
// Calculate angle limiting earth frame rate commands // Calculate angle limiting earth frame rate commands
if (g.acro_trainer == ACRO_TRAINER_LIMITED) { if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
if (roll_angle > aparm.angle_max){ if (roll_angle > aparm.angle_max){
rate_ef_level.x += g.pi_stabilize_roll.get_p(aparm.angle_max-roll_angle); rate_ef_level.x -= g.pi_stabilize_roll.get_p(roll_angle-aparm.angle_max);
}else if (roll_angle < -aparm.angle_max) { }else if (roll_angle < -aparm.angle_max) {
rate_ef_level.x += g.pi_stabilize_roll.get_p(-aparm.angle_max-roll_angle); rate_ef_level.x -= g.pi_stabilize_roll.get_p(roll_angle+aparm.angle_max);
} }
if (pitch_angle > aparm.angle_max){ if (pitch_angle > aparm.angle_max){
rate_ef_level.y += g.pi_stabilize_pitch.get_p(aparm.angle_max-pitch_angle); rate_ef_level.y -= g.pi_stabilize_pitch.get_p(pitch_angle-aparm.angle_max);
}else if (pitch_angle < -aparm.angle_max) { }else if (pitch_angle < -aparm.angle_max) {
rate_ef_level.y += g.pi_stabilize_pitch.get_p(-aparm.angle_max-pitch_angle); rate_ef_level.y -= g.pi_stabilize_pitch.get_p(pitch_angle+aparm.angle_max);
} }
} }
@ -114,6 +113,7 @@ static void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int
rate_bf_request.z += rate_bf_level.z; rate_bf_request.z += rate_bf_level.z;
rate_bf_request.z = constrain_float(rate_bf_request.z, -rate_limit, rate_limit); rate_bf_request.z = constrain_float(rate_bf_request.z, -rate_limit, rate_limit);
} }
}
// hand back rate request // hand back rate request
roll_out = rate_bf_request.x; roll_out = rate_bf_request.x;

Write Preview
Loading…
Cancel
Save