Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AutoTune: clean up FreqResp library and add comments

Browse Source
This commit is contained in:
Bill Geyer 2021-09-09 23:32:31 -04:00 committed by Bill Geyer
parent 9ff3a7e795
commit af1c8dd9bc
3 changed files with 17 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AC_AutoTune/AC_AutoTune.cpp
View File

@ -1823,9 +1823,9 @@ void AC_AutoTune::dwell_test_run(uint8_t freq_resp_input, float start_frq, float
// wait for dwell to start before determining gain and phase or just start if sweep
if ((float)(now - dwell_start_time_ms) > 6.25f * cycle_time_ms || (!is_equal(start_frq,stop_frq) && settle_time == 0)) {
if (freq_resp_input == 1) {
freqresp_rate.determine_gain(filt_target_rate,rotation_rate, dwell_freq);
freqresp_rate.update_rate(filt_target_rate,rotation_rate, dwell_freq);
} else {
freqresp_rate.determine_gain(command_out,rotation_rate, dwell_freq);
freqresp_rate.update_rate(command_out,rotation_rate, dwell_freq);
}
if (freqresp_rate.is_cycle_complete()) {
if (!is_equal(start_frq,stop_frq)) {
@ -2024,7 +2024,7 @@ void AC_AutoTune::angle_dwell_test_run(float start_frq, float stop_frq, float &d
// wait for dwell to start before determining gain and phase
if ((float)(now - dwell_start_time_ms) > 6.25f * cycle_time_ms || (!is_equal(start_frq,stop_frq) && settle_time == 0)) {
freqresp_angle.determine_gain_angle(command_out, filt_target_rate, rotation_rate, dwell_freq);
freqresp_angle.update_angle(command_out, filt_target_rate, rotation_rate, dwell_freq);
if (freqresp_angle.is_cycle_complete()) {
if (!is_equal(start_frq,stop_frq)) {
curr_test_freq = freqresp_angle.get_freq();

14
libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp
View File

@ -7,13 +7,7 @@ is set. This function must be reset using the reset flag prior to the next dwel
#include <AP_HAL/AP_HAL.h>
#include "AC_AutoTune_FreqResp.h"
float AC_AutoTune_FreqResp::update()
{
float dummy = 0.0f;
return dummy;
}
// Initialize the Frequency Response methods. Must be called before running dwell or frequency sweep tests
// Initialize the Frequency Response Object. Must be called before running dwell or frequency sweep tests
void AC_AutoTune_FreqResp::init(InputType input_type)
{
excitation = input_type;
@ -43,7 +37,7 @@ void AC_AutoTune_FreqResp::init(InputType input_type)
// and the cycles_complete flag is set. For frequency sweep tests, phase and gain are determined for every cycle and
// cycle_complete flag is set to indicate when to pull the phase and gain data. The flag is reset to enable the next
// cycle to be analyzed.
void AC_AutoTune_FreqResp::determine_gain(float tgt_rate, float meas_rate, float tgt_freq)
void AC_AutoTune_FreqResp::update_rate(float tgt_rate, float meas_rate, float tgt_freq)
{
uint32_t now = AP_HAL::millis();
uint32_t half_cycle_time_ms = 0;
@ -209,12 +203,12 @@ void AC_AutoTune_FreqResp::determine_gain(float tgt_rate, float meas_rate, float
prev_meas = meas_rate;
}
// determine_gain_angle - this function receives time history data during a dwell and frequency sweep tests for angle_p tuning
// update_angle - this function receives time history data during a dwell and frequency sweep tests for angle_p tuning
// and determines the gain, phase, and max acceleration of the response to the input. For dwell tests once the designated number
// of cycles are complete, the average of the gain, phase, and max acceleration are determined over the last 5 cycles and the
// cycles_complete flag is set. For frequency sweep tests, phase and gain are determined for every cycle and cycle_complete flag is set
// to indicate when to pull the phase and gain data. The flag is reset to enable the next cycle to be analyzed.
void AC_AutoTune_FreqResp::determine_gain_angle(float command, float tgt_angle, float meas_angle, float tgt_freq)
void AC_AutoTune_FreqResp::update_angle(float command, float tgt_angle, float meas_angle, float tgt_freq)
{
uint32_t now = AP_HAL::millis();

22
libraries/AC_AutoTune/AC_AutoTune_FreqResp.h
View File

@ -19,30 +19,28 @@ public:
tgt_peak_info_buffer = new ObjectBuffer<peak_info>(AUTOTUNE_DWELL_CYCLES);
}
// CLASS_NO_COPY(AC_PI);
// update calculations
float update();
enum InputType {
DWELL = 0,
SWEEP = 1,
};
// Initialize the Frequency Response Object.
// Must be called before running dwell or frequency sweep tests
void init(InputType input_type);
// determines the gain and phase for a dwell
void determine_gain(float tgt_rate, float meas_rate, float tgt_freq);
// determines the gain and phase based on rate response for a dwell or sweep
void update_rate(float tgt_rate, float meas_rate, float tgt_freq);
// determines the gain and phase for a dwell
void determine_gain_angle(float command, float tgt_angle, float meas_angle, float tgt_freq);
// determines the gain and phase based on angle response for a dwell or sweep
void update_angle(float command, float tgt_angle, float meas_angle, float tgt_freq);
// enable external query if cycle is complete and phase and gain data are available
// enable external query if cycle is complete and freq response data are available
bool is_cycle_complete() { return cycle_complete;}
// reset cycle_complete flag
void reset_cycle_complete() { cycle_complete = false; }
// frequency response accessors
// frequency response data accessors
float get_freq() { return curr_test_freq; }
float get_gain() { return curr_test_gain; }
float get_phase() { return curr_test_phase; }
@ -72,7 +70,7 @@ private:
sweep_peak_finding_data sweep_meas;
sweep_peak_finding_data sweep_tgt;
//store determine gain data in ring buffer
//store gain data in ring buffer
struct peak_info {
uint16_t curr_count;
float amplitude;