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

AC_AutoTune: incorporate suggested changes

This commit is contained in:
bnsgeyer 2024-06-16 11:41:52 -04:00 committed by Bill Geyer
parent 41ed4d1321
commit 088b7ec094
2 changed files with 16 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp
View File

@ -305,9 +305,9 @@ void AC_AutoTune_FreqResp::pull_from_tgt_buffer(uint16_t &count, float &amplitud
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<peak_info>(cycles);
if (tgt_peak_info_buffer != nullptr) { delete tgt_peak_info_buffer;}
delete tgt_peak_info_buffer;
tgt_peak_info_buffer = NEW_NOTHROW ObjectBuffer<peak_info>(cycles);
}

28
libraries/AC_AutoTune/AC_AutoTune_Heli.cpp
View File

@ -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;
}