Skip to content

Commit e60fb1b

Browse files
peterbarkerpeterbarker
committed
AC_AutoTune_Heli: factor out roll/pitch/yaw values
1 parent d1a1bcb commit e60fb1b

File tree

1 file changed

+50
-42
lines changed

1 file changed

+50
-42
lines changed

‎libraries/AC_AutoTune/AC_AutoTune_Heli.cpp

Lines changed: 50 additions & 42 deletions
Original file line numberDiff line numberDiff line change
@@ -719,6 +719,10 @@ void AC_AutoTune_Heli::rate_ff_test_run(float max_angle_cd, float target_rate_cd
719719
float tgt_rate_reading = 0.0f;
720720
const uint32_t now = AP_HAL::millis();
721721

722+
const int32_t ahrs_roll_cd = ahrs_view->roll_sensor;
723+
const int32_t ahrs_pitch_cd = ahrs_view->pitch_sensor;
724+
const int32_t ahrs_yaw_cd = ahrs_view->yaw_sensor;
725+
722726
target_rate_cds = dir_sign * target_rate_cds;
723727

724728
switch (axis) {
@@ -730,30 +734,30 @@ void AC_AutoTune_Heli::rate_ff_test_run(float max_angle_cd, float target_rate_cd
730734
settle_time--;
731735
trim_command_reading = motors->get_roll();
732736
rate_request_cds.reset(gyro_reading);
733-
} else if (((ahrs_view->roll_sensor <= max_angle_cd + start_angle && is_positive(dir_sign))
734-
|| (ahrs_view->roll_sensor >= -max_angle_cd + start_angle && !is_positive(dir_sign)))
737+
} else if (((ahrs_roll_cd <= max_angle_cd + start_angle && is_positive(dir_sign))
738+
|| (ahrs_roll_cd >= -max_angle_cd + start_angle && !is_positive(dir_sign)))
735739
&& ff_test_phase == 0) {
736740
rate_request_cds.apply(target_rate_cds, AP::scheduler().get_loop_period_s());
737741
attitude_control->input_rate_bf_roll_pitch_yaw(rate_request_cds.get(), 0.0f, 0.0f);
738-
} else if (((ahrs_view->roll_sensor > max_angle_cd + start_angle && is_positive(dir_sign))
739-
|| (ahrs_view->roll_sensor < -max_angle_cd + start_angle && !is_positive(dir_sign)))
742+
} else if (((ahrs_roll_cd > max_angle_cd + start_angle && is_positive(dir_sign))
743+
|| (ahrs_roll_cd < -max_angle_cd + start_angle && !is_positive(dir_sign)))
740744
&& ff_test_phase == 0) {
741745
ff_test_phase = 1;
742746
rate_request_cds.apply(-target_rate_cds, AP::scheduler().get_loop_period_s());
743747
attitude_control->input_rate_bf_roll_pitch_yaw(rate_request_cds.get(), 0.0f, 0.0f);
744748
attitude_control->rate_bf_roll_target(rate_request_cds.get());
745-
} else if (((ahrs_view->roll_sensor >= -max_angle_cd + start_angle && is_positive(dir_sign))
746-
|| (ahrs_view->roll_sensor <= max_angle_cd + start_angle && !is_positive(dir_sign)))
749+
} else if (((ahrs_roll_cd >= -max_angle_cd + start_angle && is_positive(dir_sign))
750+
|| (ahrs_roll_cd <= max_angle_cd + start_angle && !is_positive(dir_sign)))
747751
&& ff_test_phase == 1 ) {
748752
rate_request_cds.apply(-target_rate_cds, AP::scheduler().get_loop_period_s());
749753
attitude_control->input_rate_bf_roll_pitch_yaw(rate_request_cds.get(), 0.0f, 0.0f);
750754
attitude_control->rate_bf_roll_target(rate_request_cds.get());
751-
} else if (((ahrs_view->roll_sensor < -max_angle_cd + start_angle && is_positive(dir_sign))
752-
|| (ahrs_view->roll_sensor > max_angle_cd + start_angle && !is_positive(dir_sign)))
755+
} else if (((ahrs_roll_cd < -max_angle_cd + start_angle && is_positive(dir_sign))
756+
|| (ahrs_roll_cd > max_angle_cd + start_angle && !is_positive(dir_sign)))
753757
&& ff_test_phase == 1 ) {
754758
ff_test_phase = 2;
755759
attitude_control->reset_target_and_rate(false);
756-
angle_request_cd.reset(ahrs_view->roll_sensor);
760+
angle_request_cd.reset(ahrs_roll_cd);
757761
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(angle_request_cd.get(), start_angles.y, 0.0f);
758762
} else if (ff_test_phase == 2 ) {
759763
angle_request_cd.apply(start_angles.x, AP::scheduler().get_loop_period_s());
@@ -769,30 +773,30 @@ void AC_AutoTune_Heli::rate_ff_test_run(float max_angle_cd, float target_rate_cd
769773
settle_time--;
770774
trim_command_reading = motors->get_pitch();
771775
rate_request_cds.reset(gyro_reading);
772-
} else if (((ahrs_view->pitch_sensor <= max_angle_cd + start_angle && is_positive(dir_sign))
773-
|| (ahrs_view->pitch_sensor >= -max_angle_cd + start_angle && !is_positive(dir_sign)))
776+
} else if (((ahrs_pitch_cd <= max_angle_cd + start_angle && is_positive(dir_sign))
777+
|| (ahrs_pitch_cd >= -max_angle_cd + start_angle && !is_positive(dir_sign)))
774778
&& ff_test_phase == 0) {
775779
rate_request_cds.apply(target_rate_cds, AP::scheduler().get_loop_period_s());
776780
attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, rate_request_cds.get(), 0.0f);
777-
} else if (((ahrs_view->pitch_sensor > max_angle_cd + start_angle && is_positive(dir_sign))
778-
|| (ahrs_view->pitch_sensor < -max_angle_cd + start_angle && !is_positive(dir_sign)))
781+
} else if (((ahrs_pitch_cd > max_angle_cd + start_angle && is_positive(dir_sign))
782+
|| (ahrs_pitch_cd < -max_angle_cd + start_angle && !is_positive(dir_sign)))
779783
&& ff_test_phase == 0) {
780784
ff_test_phase = 1;
781785
rate_request_cds.apply(-target_rate_cds, AP::scheduler().get_loop_period_s());
782786
attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, rate_request_cds.get(), 0.0f);
783787
attitude_control->rate_bf_pitch_target(rate_request_cds.get());
784-
} else if (((ahrs_view->pitch_sensor >= -max_angle_cd + start_angle && is_positive(dir_sign))
785-
|| (ahrs_view->pitch_sensor <= max_angle_cd + start_angle && !is_positive(dir_sign)))
788+
} else if (((ahrs_pitch_cd >= -max_angle_cd + start_angle && is_positive(dir_sign))
789+
|| (ahrs_pitch_cd <= max_angle_cd + start_angle && !is_positive(dir_sign)))
786790
&& ff_test_phase == 1 ) {
787791
rate_request_cds.apply(-target_rate_cds, AP::scheduler().get_loop_period_s());
788792
attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, rate_request_cds.get(), 0.0f);
789793
attitude_control->rate_bf_pitch_target(rate_request_cds.get());
790-
} else if (((ahrs_view->pitch_sensor < -max_angle_cd + start_angle && is_positive(dir_sign))
791-
|| (ahrs_view->pitch_sensor > max_angle_cd + start_angle && !is_positive(dir_sign)))
794+
} else if (((ahrs_pitch_cd < -max_angle_cd + start_angle && is_positive(dir_sign))
795+
|| (ahrs_pitch_cd > max_angle_cd + start_angle && !is_positive(dir_sign)))
792796
&& ff_test_phase == 1 ) {
793797
ff_test_phase = 2;
794798
attitude_control->reset_target_and_rate(false);
795-
angle_request_cd.reset(ahrs_view->pitch_sensor);
799+
angle_request_cd.reset(ahrs_pitch_cd);
796800
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(start_angles.x, angle_request_cd.get(), 0.0f);
797801
} else if (ff_test_phase == 2 ) {
798802
angle_request_cd.apply(start_angles.y, AP::scheduler().get_loop_period_s());
@@ -808,32 +812,32 @@ void AC_AutoTune_Heli::rate_ff_test_run(float max_angle_cd, float target_rate_cd
808812
if (settle_time > 0) {
809813
settle_time--;
810814
trim_command_reading = motors->get_yaw();
811-
trim_heading = ahrs_view->yaw_sensor;
815+
trim_heading = ahrs_yaw_cd;
812816
rate_request_cds.reset(gyro_reading);
813-
} else if (((wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) <= 2.0f * max_angle_cd && is_positive(dir_sign))
814-
|| (wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) >= -2.0f * max_angle_cd && !is_positive(dir_sign)))
817+
} else if (((wrap_180_cd(ahrs_yaw_cd - trim_heading) <= 2.0f * max_angle_cd && is_positive(dir_sign))
818+
|| (wrap_180_cd(ahrs_yaw_cd - trim_heading) >= -2.0f * max_angle_cd && !is_positive(dir_sign)))
815819
&& ff_test_phase == 0) {
816820
rate_request_cds.apply(target_rate_cds, AP::scheduler().get_loop_period_s());
817821
attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, rate_request_cds.get());
818-
} else if (((wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) > 2.0f * max_angle_cd && is_positive(dir_sign))
819-
|| (wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) < -2.0f * max_angle_cd && !is_positive(dir_sign)))
822+
} else if (((wrap_180_cd(ahrs_yaw_cd - trim_heading) > 2.0f * max_angle_cd && is_positive(dir_sign))
823+
|| (wrap_180_cd(ahrs_yaw_cd - trim_heading) < -2.0f * max_angle_cd && !is_positive(dir_sign)))
820824
&& ff_test_phase == 0) {
821825
ff_test_phase = 1;
822826
rate_request_cds.apply(-target_rate_cds, AP::scheduler().get_loop_period_s());
823827
attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, rate_request_cds.get());
824828
attitude_control->rate_bf_yaw_target(rate_request_cds.get());
825-
} else if (((wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) >= -2.0f * max_angle_cd && is_positive(dir_sign))
826-
|| (wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) <= 2.0f * max_angle_cd && !is_positive(dir_sign)))
829+
} else if (((wrap_180_cd(ahrs_yaw_cd - trim_heading) >= -2.0f * max_angle_cd && is_positive(dir_sign))
830+
|| (wrap_180_cd(ahrs_yaw_cd - trim_heading) <= 2.0f * max_angle_cd && !is_positive(dir_sign)))
827831
&& ff_test_phase == 1 ) {
828832
rate_request_cds.apply(-target_rate_cds, AP::scheduler().get_loop_period_s());
829833
attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, rate_request_cds.get());
830834
attitude_control->rate_bf_yaw_target(rate_request_cds.get());
831-
} else if (((wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) < -2.0f * max_angle_cd && is_positive(dir_sign))
832-
|| (wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) > 2.0f * max_angle_cd && !is_positive(dir_sign)))
835+
} else if (((wrap_180_cd(ahrs_yaw_cd - trim_heading) < -2.0f * max_angle_cd && is_positive(dir_sign))
836+
|| (wrap_180_cd(ahrs_yaw_cd - trim_heading) > 2.0f * max_angle_cd && !is_positive(dir_sign)))
833837
&& ff_test_phase == 1 ) {
834838
ff_test_phase = 2;
835839
attitude_control->reset_yaw_target_and_rate(false);
836-
angle_request_cd.reset(wrap_180_cd(ahrs_view->yaw_sensor - trim_heading));
840+
angle_request_cd.reset(wrap_180_cd(ahrs_yaw_cd - trim_heading));
837841
attitude_control->input_euler_angle_roll_pitch_yaw(start_angles.x, start_angles.y, angle_request_cd.get(), false);
838842
} else if (ff_test_phase == 2 ) {
839843
angle_request_cd.apply(0.0f, AP::scheduler().get_loop_period_s());
@@ -852,17 +856,17 @@ void AC_AutoTune_Heli::rate_ff_test_run(float max_angle_cd, float target_rate_cd
852856
// record steady state rate and motor command
853857
switch (axis) {
854858
case ROLL:
855-
if (((ahrs_view->roll_sensor >= -max_angle_cd + start_angle && is_positive(dir_sign))
856-
|| (ahrs_view->roll_sensor <= max_angle_cd + start_angle && !is_positive(dir_sign)))
859+
if (((ahrs_roll_cd >= -max_angle_cd + start_angle && is_positive(dir_sign))
860+
|| (ahrs_roll_cd <= max_angle_cd + start_angle && !is_positive(dir_sign)))
857861
&& ff_test_phase == 1 ) {
858862
test_rate_filt = rotation_rate;
859863
test_command_filt = command_out;
860864
test_tgt_rate_filt = filt_target_rate;
861865
}
862866
break;
863867
case PITCH:
864-
if (((ahrs_view->pitch_sensor >= -max_angle_cd + start_angle && is_positive(dir_sign))
865-
|| (ahrs_view->pitch_sensor <= max_angle_cd + start_angle && !is_positive(dir_sign)))
868+
if (((ahrs_pitch_cd >= -max_angle_cd + start_angle && is_positive(dir_sign))
869+
|| (ahrs_pitch_cd <= max_angle_cd + start_angle && !is_positive(dir_sign)))
866870
&& ff_test_phase == 1 ) {
867871
test_rate_filt = rotation_rate;
868872
test_command_filt = command_out;
@@ -871,8 +875,8 @@ void AC_AutoTune_Heli::rate_ff_test_run(float max_angle_cd, float target_rate_cd
871875
break;
872876
case YAW:
873877
case YAW_D:
874-
if (((wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) >= -2.0f * max_angle_cd && is_positive(dir_sign))
875-
|| (wrap_180_cd(ahrs_view->yaw_sensor - trim_heading) <= 2.0f * max_angle_cd && !is_positive(dir_sign)))
878+
if (((wrap_180_cd(ahrs_yaw_cd - trim_heading) >= -2.0f * max_angle_cd && is_positive(dir_sign))
879+
|| (wrap_180_cd(ahrs_yaw_cd - trim_heading) <= 2.0f * max_angle_cd && !is_positive(dir_sign)))
876880
&& ff_test_phase == 1 ) {
877881
test_rate_filt = rotation_rate;
878882
test_command_filt = command_out;
@@ -987,7 +991,11 @@ void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq,
987991
velocity_bf.y = -velocity_ned.x * ahrs_view->sin_yaw() + velocity_ned.y * ahrs_view->cos_yaw();
988992
}
989993

990-
Vector3f attitude_cd = Vector3f((float)ahrs_view->roll_sensor, (float)ahrs_view->pitch_sensor, (float)ahrs_view->yaw_sensor);
994+
const int32_t ahrs_roll_cd = ahrs_view->roll_sensor;
995+
const int32_t ahrs_pitch_cd = ahrs_view->pitch_sensor;
996+
const int32_t ahrs_yaw_cd = ahrs_view->yaw_sensor;
997+
998+
Vector3f attitude_cd = Vector3f((float)ahrs_roll_cd, (float)ahrs_pitch_cd, (float)ahrs_yaw_cd);
991999
if (settle_time == 0) {
9921000
if (dwell_type == RATE) {
9931001
target_rate_cds = -chirp_input.update((now - dwell_start_time_ms) * 0.001, target_rate_mag_cds);
@@ -1012,7 +1020,7 @@ void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq,
10121020
} else {
10131021
target_angle_cd = 0.0f;
10141022
trim_yaw_tgt_reading = (float)attitude_control->get_att_target_euler_cd().z;
1015-
trim_yaw_heading_reading = (float)ahrs_view->yaw_sensor;
1023+
trim_yaw_heading_reading = (float)ahrs_yaw_cd;
10161024
}
10171025
dwell_start_time_ms = now;
10181026
filt_att_fdbk_from_velxy_cd.reset(Vector2f(0.0f,0.0f));
@@ -1100,32 +1108,32 @@ void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq,
11001108
command_reading = motors->get_roll();
11011109
if (dwell_type == DRB) {
11021110
tgt_rate_reading = (target_angle_cd) / 5730.0f;
1103-
gyro_reading = ((float)ahrs_view->roll_sensor + trim_angle_cd.x - target_angle_cd) / 5730.0f;
1111+
gyro_reading = ((float)ahrs_roll_cd + trim_angle_cd.x - target_angle_cd) / 5730.0f;
11041112
} else {
11051113
tgt_rate_reading = ((float)attitude_control->get_att_target_euler_cd().x) / 5730.0f;
1106-
gyro_reading = ((float)ahrs_view->roll_sensor) / 5730.0f;
1114+
gyro_reading = ((float)ahrs_roll_cd) / 5730.0f;
11071115
}
11081116
break;
11091117
case PITCH:
11101118
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(trim_angle_cd.x, target_angle_cd + trim_angle_cd.y, 0.0f);
11111119
command_reading = motors->get_pitch();
11121120
if (dwell_type == DRB) {
11131121
tgt_rate_reading = (target_angle_cd) / 5730.0f;
1114-
gyro_reading = ((float)ahrs_view->pitch_sensor + trim_angle_cd.y - target_angle_cd) / 5730.0f;
1122+
gyro_reading = ((float)ahrs_pitch_cd + trim_angle_cd.y - target_angle_cd) / 5730.0f;
11151123
} else {
11161124
tgt_rate_reading = ((float)attitude_control->get_att_target_euler_cd().y) / 5730.0f;
1117-
gyro_reading = ((float)ahrs_view->pitch_sensor) / 5730.0f;
1125+
gyro_reading = ((float)ahrs_pitch_cd) / 5730.0f;
11181126
}
11191127
break;
11201128
case YAW:
11211129
case YAW_D:
11221130
command_reading = motors->get_yaw();
11231131
if (dwell_type == DRB) {
11241132
tgt_rate_reading = (target_angle_cd) / 5730.0f;
1125-
gyro_reading = (wrap_180_cd((float)ahrs_view->yaw_sensor - trim_yaw_heading_reading - target_angle_cd)) / 5730.0f;
1133+
gyro_reading = (wrap_180_cd((float)ahrs_yaw_cd - trim_yaw_heading_reading - target_angle_cd)) / 5730.0f;
11261134
} else {
11271135
tgt_rate_reading = (wrap_180_cd((float)attitude_control->get_att_target_euler_cd().z - trim_yaw_tgt_reading)) / 5730.0f;
1128-
gyro_reading = (wrap_180_cd((float)ahrs_view->yaw_sensor - trim_yaw_heading_reading)) / 5730.0f;
1136+
gyro_reading = (wrap_180_cd((float)ahrs_yaw_cd - trim_yaw_heading_reading)) / 5730.0f;
11291137
}
11301138
attitude_control->input_euler_angle_roll_pitch_yaw(trim_angle_cd.x, trim_angle_cd.y, wrap_180_cd(trim_yaw_tgt_reading + target_angle_cd), false);
11311139
break;

0 commit comments

Comments
 (0)