Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: sport and acro trainer limits based on target attitude 

previously the trainer used the vehicle's actual attitude meaning that the target could get far past the limits if there was an attitude error
This commit is contained in:
Randy Mackay 2017-05-23 14:02:33 +09:00
parent cffd5e0f0b
commit 7150dfd5f2
2 changed files with 12 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduCopter/control_acro.cpp
View File

@ -100,12 +100,16 @@ void Copter::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, in
// 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) { if (g.acro_trainer != ACRO_TRAINER_DISABLED) {
// get attitude targets
const Vector3f att_target = attitude_control->get_att_target_euler_cd();
// 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(att_target.x);
rate_ef_level.x = -constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_roll; 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 // 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(att_target.y);
rate_ef_level.y = -constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch; 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 // Calculate trainer mode earth frame rate command for yaw

8
ArduCopter/control_sport.cpp
View File

@ -40,11 +40,15 @@ void Copter::sport_run()
float target_roll_rate = channel_roll->get_control_in() * g.acro_rp_p; float target_roll_rate = channel_roll->get_control_in() * g.acro_rp_p;
float target_pitch_rate = channel_pitch->get_control_in() * g.acro_rp_p; float target_pitch_rate = channel_pitch->get_control_in() * g.acro_rp_p;
int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor); // get attitude targets
const Vector3f att_target = attitude_control->get_att_target_euler_cd();
// Calculate trainer mode earth frame rate command for roll
int32_t roll_angle = wrap_180_cd(att_target.x);
target_roll_rate -= constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_roll; target_roll_rate -= 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 // 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(att_target.y);
target_pitch_rate -= constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch; target_pitch_rate -= constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch;
if (roll_angle > aparm.angle_max){ if (roll_angle > aparm.angle_max){