From 088b7ec094236b5e1d4ee45307807426d68d2c5d Mon Sep 17 00:00:00 2001 From: bnsgeyer Date: Sun, 16 Jun 2024 11:41:52 -0400 Subject: [PATCH] AC_AutoTune: incorporate suggested changes --- .../AC_AutoTune/AC_AutoTune_FreqResp.cpp | 4 +-- libraries/AC_AutoTune/AC_AutoTune_Heli.cpp | 28 +++++++++---------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp b/libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp index 804b580091..499770b34a 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp @@ -305,9 +305,9 @@ void AC_AutoTune_FreqResp::pull_from_tgt_buffer(uint16_t &count, float &litud void AC_AutoTune_FreqResp::set_dwell_cycles(uint8_t cycles) { dwell_cycles = cycles; - if (meas_peak_info_buffer != nullptr) { delete meas_peak_info_buffer;} + delete meas_peak_info_buffer; meas_peak_info_buffer = NEW_NOTHROW ObjectBuffer(cycles); - if (tgt_peak_info_buffer != nullptr) { delete tgt_peak_info_buffer;} + delete tgt_peak_info_buffer; tgt_peak_info_buffer = NEW_NOTHROW ObjectBuffer(cycles); } diff --git a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp index 2ba7ffffbf..aa52ed7da0 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Heli.cpp @@ -743,7 +743,7 @@ void AC_AutoTune_Heli::dwell_test_init(float start_frq, float stop_frq, float am test_calc_type = calc_type; test_start_freq = start_frq; //target attitude magnitude - tgt_attitude = amplitude * 0.01745f; + tgt_attitude = radians(amplitude); // initialize frequency response object if (test_input_type == AC_AutoTune_FreqResp::InputType::SWEEP) { @@ -815,7 +815,7 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data) } if (settle_time == 0) { - target_angle_cd = -chirp_input.update((now - dwell_start_time_ms) * 0.001, tgt_attitude * 5730.0f); + target_angle_cd = -chirp_input.update((now - dwell_start_time_ms) * 0.001, degrees(tgt_attitude) * 100.0f); dwell_freq = chirp_input.get_frequency_rads(); const Vector2f att_fdbk { -5730.0f * vel_hold_gain * velocity_bf.y, @@ -841,28 +841,28 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data) attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_angle_cd + trim_angle_cd.x, trim_angle_cd.y, 0.0f); command_reading = motors->get_roll(); if (test_calc_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; + tgt_rate_reading = radians(target_angle_cd * 0.01f); + gyro_reading = radians(((float)ahrs_view->roll_sensor + trim_angle_cd.x - target_angle_cd) * 0.01f); } else if (test_calc_type == RATE) { tgt_rate_reading = attitude_control->rate_bf_targets().x; gyro_reading = ahrs_view->get_gyro().x; } else { - tgt_rate_reading = ((float)attitude_control->get_att_target_euler_cd().x) / 5730.0f; - gyro_reading = ((float)ahrs_view->roll_sensor) / 5730.0f; + tgt_rate_reading = radians((float)attitude_control->get_att_target_euler_cd().x * 0.01f); + gyro_reading = radians((float)ahrs_view->roll_sensor * 0.01f); } 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 (test_calc_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; + tgt_rate_reading = radians(target_angle_cd * 0.01f); + gyro_reading = radians(((float)ahrs_view->pitch_sensor + trim_angle_cd.y - target_angle_cd) * 0.01f); } else if (test_calc_type == RATE) { tgt_rate_reading = attitude_control->rate_bf_targets().y; gyro_reading = ahrs_view->get_gyro().y; } else { - tgt_rate_reading = ((float)attitude_control->get_att_target_euler_cd().y) / 5730.0f; - gyro_reading = ((float)ahrs_view->pitch_sensor) / 5730.0f; + tgt_rate_reading = radians((float)attitude_control->get_att_target_euler_cd().y * 0.01f); + gyro_reading = radians((float)ahrs_view->pitch_sensor * 0.01f); } break; case YAW: @@ -870,14 +870,14 @@ void AC_AutoTune_Heli::dwell_test_run(sweep_info &test_data) 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); command_reading = motors->get_yaw(); if (test_calc_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; + tgt_rate_reading = radians(target_angle_cd * 0.01f); + gyro_reading = radians((wrap_180_cd((float)ahrs_view->yaw_sensor - trim_yaw_heading_reading - target_angle_cd)) * 0.01f); } else if (test_calc_type == RATE) { tgt_rate_reading = attitude_control->rate_bf_targets().z; gyro_reading = ahrs_view->get_gyro().z; } 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; + tgt_rate_reading = radians((wrap_180_cd((float)attitude_control->get_att_target_euler_cd().z - trim_yaw_tgt_reading)) * 0.01f); + gyro_reading = radians((wrap_180_cd((float)ahrs_view->yaw_sensor - trim_yaw_heading_reading)) * 0.01f); } break; }