x

Author: peterbarker

libraries/AC_AutoTune/AC_AutoTune_Heli.cpp

@@ -991,7 +991,11 @@ void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq,
         velocity_bf.y = -velocity_ned.x * ahrs_view->sin_yaw() + velocity_ned.y * ahrs_view->cos_yaw();
     }
 
-    Vector3f attitude_cd = Vector3f((float)ahrs_view->roll_sensor, (float)ahrs_view->pitch_sensor, (float)ahrs_view->yaw_sensor);
+    const int32_t ahrs_roll_cd = ahrs_view->roll_sensor;
+    const int32_t ahrs_pitch_cd = ahrs_view->pitch_sensor;
+    const int32_t ahrs_yaw_cd = ahrs_view->yaw_sensor;
+
+    Vector3f attitude_cd = Vector3f((float)ahrs_roll_cd, (float)ahrs_pitch_cd, (float)ahrs_yaw_cd);
     if (settle_time == 0) {
         if (dwell_type == RATE) {
             target_rate_cds = -chirp_input.update((now - dwell_start_time_ms) * 0.001, target_rate_mag_cds);
@@ -1016,7 +1020,7 @@ void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq,
         } else {
             target_angle_cd = 0.0f;
             trim_yaw_tgt_reading = (float)attitude_control->get_att_target_euler_cd().z;
-            trim_yaw_heading_reading = (float)ahrs_view->yaw_sensor;
+            trim_yaw_heading_reading = (float)ahrs_yaw_cd;
         }
         dwell_start_time_ms = now;
         filt_att_fdbk_from_velxy_cd.reset(Vector2f(0.0f,0.0f));
@@ -1104,32 +1108,32 @@ void AC_AutoTune_Heli::dwell_test_run(uint8_t freq_resp_input, float start_frq,
             command_reading = motors->get_roll();
             if (dwell_type == DRB) {
                 tgt_rate_reading = (target_angle_cd) / 5730.0f;
-                gyro_reading = ((float)ahrs_view->roll_sensor + trim_angle_cd.x - target_angle_cd) / 5730.0f;
+                gyro_reading = ((float)ahrs_roll_cd + trim_angle_cd.x - target_angle_cd) / 5730.0f;
             } else {
                 tgt_rate_reading = ((float)attitude_control->get_att_target_euler_cd().x) / 5730.0f;
-                gyro_reading = ((float)ahrs_view->roll_sensor) / 5730.0f;
+                gyro_reading = ((float)ahrs_roll_cd) / 5730.0f;
             }
             break;
         case PITCH:
             attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(trim_angle_cd.x, target_angle_cd + trim_angle_cd.y, 0.0f);
             command_reading = motors->get_pitch();
             if (dwell_type == DRB) {
                 tgt_rate_reading = (target_angle_cd) / 5730.0f;
-                gyro_reading = ((float)ahrs_view->pitch_sensor + trim_angle_cd.y - target_angle_cd) / 5730.0f;
+                gyro_reading = ((float)ahrs_pitch_cd + trim_angle_cd.y - target_angle_cd) / 5730.0f;
             } else {
                 tgt_rate_reading = ((float)attitude_control->get_att_target_euler_cd().y) / 5730.0f;
-                gyro_reading = ((float)ahrs_view->pitch_sensor) / 5730.0f;
+                gyro_reading = ((float)ahrs_pitch_cd) / 5730.0f;
             }
             break;
         case YAW:
         case YAW_D:
             command_reading = motors->get_yaw();
             if (dwell_type == DRB) {
                 tgt_rate_reading = (target_angle_cd) / 5730.0f;
-                gyro_reading = (wrap_180_cd((float)ahrs_view->yaw_sensor - trim_yaw_heading_reading - target_angle_cd)) / 5730.0f;
+                gyro_reading = (wrap_180_cd((float)ahrs_yaw_cd - trim_yaw_heading_reading - target_angle_cd)) / 5730.0f;
             } else {
                 tgt_rate_reading = (wrap_180_cd((float)attitude_control->get_att_target_euler_cd().z - trim_yaw_tgt_reading)) / 5730.0f;
-                gyro_reading = (wrap_180_cd((float)ahrs_view->yaw_sensor - trim_yaw_heading_reading)) / 5730.0f;
+                gyro_reading = (wrap_180_cd((float)ahrs_yaw_cd - trim_yaw_heading_reading)) / 5730.0f;
             }
             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);
             break;