Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 15:53:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: Leonard's control_acro fixes

Browse Source 
get_pilot_desired_angle_rates returns bf rate targets as floats
This commit is contained in:
Randy Mackay 2014-02-10 10:26:49 +09:00 committed by Andrew Tridgell
parent 2c6470f87b
commit bf6bb59cb4
1 changed files with 54 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
ArduCopter/control_acro.pde
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;