From 7150dfd5f2341cc59c7578a7dc585622b0343c58 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 23 May 2017 14:02:33 +0900 Subject: [PATCH] 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 --- ArduCopter/control_acro.cpp | 8 ++++++-- ArduCopter/control_sport.cpp | 8 ++++++-- 2 files changed, 12 insertions(+), 4 deletions(-) diff --git a/ArduCopter/control_acro.cpp b/ArduCopter/control_acro.cpp index 01ee6e7790..87a2a0725e 100644 --- a/ArduCopter/control_acro.cpp +++ b/ArduCopter/control_acro.cpp @@ -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 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 - 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; // 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; // Calculate trainer mode earth frame rate command for yaw diff --git a/ArduCopter/control_sport.cpp b/ArduCopter/control_sport.cpp index 492abb22b2..f56cf5ab08 100644 --- a/ArduCopter/control_sport.cpp +++ b/ArduCopter/control_sport.cpp @@ -40,11 +40,15 @@ void Copter::sport_run() 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; - 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; // 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; if (roll_angle > aparm.angle_max){