aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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

108
ArduCopter/control_acro.pde
View File

@ -16,7 +16,7 @@ static bool acro_init(bool ignore_checks)
// should be called at 100hz or more
static void acro_run()
{
int16_t target_roll, target_pitch, target_yaw;
float target_roll, target_pitch, target_yaw;
int16_t pilot_throttle_scaled;
// 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
// 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
@ -57,64 +57,64 @@ 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 trainer mode earth frame rate command for roll
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 = -roll_angle * g.acro_balance_roll;
if (g.acro_trainer != ACRO_TRAINER_DISABLED) {
// Calculate trainer mode earth frame rate command for roll
int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor);
rate_ef_level.x = -constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_roll;
// Calculate trainer mode earth frame rate command for pitch
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 = -pitch_angle * g.acro_balance_pitch;
// Calculate trainer mode earth frame rate command for pitch
int32_t pitch_angle = wrap_180_cd(ahrs.pitch_sensor);
rate_ef_level.y = -constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch;
// Calculate trainer mode earth frame rate command for yaw
rate_ef_level.z = 0;
// Calculate angle limiting earth frame rate commands
if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
if (roll_angle > aparm.angle_max){
rate_ef_level.x += g.pi_stabilize_roll.get_p(aparm.angle_max-roll_angle);
}else if (roll_angle < -aparm.angle_max) {
rate_ef_level.x += g.pi_stabilize_roll.get_p(-aparm.angle_max-roll_angle);
// Calculate trainer mode earth frame rate command for yaw
rate_ef_level.z = 0;
// Calculate angle limiting earth frame rate commands
if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
if (roll_angle > aparm.angle_max){
rate_ef_level.x -= g.pi_stabilize_roll.get_p(roll_angle-aparm.angle_max);
}else if (roll_angle < -aparm.angle_max) {
rate_ef_level.x -= g.pi_stabilize_roll.get_p(roll_angle+aparm.angle_max);
}
if (pitch_angle > aparm.angle_max){
rate_ef_level.y -= g.pi_stabilize_pitch.get_p(pitch_angle-aparm.angle_max);
}else if (pitch_angle < -aparm.angle_max) {
rate_ef_level.y -= g.pi_stabilize_pitch.get_p(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);
}else if (pitch_angle < -aparm.angle_max) {
rate_ef_level.y += g.pi_stabilize_pitch.get_p(-aparm.angle_max-pitch_angle);
// convert earth-frame level rates to body-frame level rates
attitude_control.rate_ef_targets_to_bf(rate_ef_level, rate_bf_level);
// combine earth frame rate corrections with rate requests
if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
rate_bf_request.x += rate_bf_level.x;
rate_bf_request.y += rate_bf_level.y;
rate_bf_request.z += rate_bf_level.z;
}else{
acro_level_mix = constrain_float(1-max(max(abs(roll_in), abs(pitch_in)), abs(yaw_in))/4500.0, 0, 1)*ahrs.cos_pitch();
// Scale leveling rates by stick input
rate_bf_level = rate_bf_level*acro_level_mix;
// Calculate rate limit to prevent change of rate through inverted
rate_limit = fabs(fabs(rate_bf_request.x)-fabs(rate_bf_level.x));
rate_bf_request.x += rate_bf_level.x;
rate_bf_request.x = constrain_float(rate_bf_request.x, -rate_limit, rate_limit);
// Calculate rate limit to prevent change of rate through inverted
rate_limit = fabs(fabs(rate_bf_request.y)-fabs(rate_bf_level.y));
rate_bf_request.y += rate_bf_level.y;
rate_bf_request.y = constrain_float(rate_bf_request.y, -rate_limit, rate_limit);
// Calculate rate limit to prevent change of rate through inverted
rate_limit = fabs(fabs(rate_bf_request.z)-fabs(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);
}
}
// convert earth-frame level rates to body-frame level rates
attitude_control.rate_ef_targets_to_bf(rate_ef_level, rate_bf_level);
// combine earth frame rate corrections with rate requests
if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
rate_bf_request.x += rate_bf_level.x;
rate_bf_request.y += rate_bf_level.y;
rate_bf_request.z += rate_bf_level.z;
}else{
acro_level_mix = constrain_float(1-max(max(abs(roll_in), abs(pitch_in)), abs(yaw_in))/4500.0, 0, 1)*ahrs.cos_pitch();
// Scale leveling rates by stick input
rate_bf_level = rate_bf_level*acro_level_mix;
// Calculate rate limit to prevent change of rate through inverted
rate_limit = fabs(fabs(rate_bf_request.x)-fabs(rate_bf_level.x));
rate_bf_request.x += rate_bf_level.x;
rate_bf_request.x = constrain_float(rate_bf_request.x, -rate_limit, rate_limit);
// Calculate rate limit to prevent change of rate through inverted
rate_limit = fabs(fabs(rate_bf_request.y)-fabs(rate_bf_level.y));
rate_bf_request.y += rate_bf_level.y;
rate_bf_request.y = constrain_float(rate_bf_request.y, -rate_limit, rate_limit);
// Calculate rate limit to prevent change of rate through inverted
rate_limit = fabs(fabs(rate_bf_request.z)-fabs(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);
}
// hand back rate request
roll_out = rate_bf_request.x;
pitch_out = rate_bf_request.y;