ardupilot/libraries/AC_AutoTune/AC_AutoTune_FreqResp.h

200 lines
5.7 KiB
C
Raw Normal View History

#pragma once
/*
Gain and phase determination algorithm
*/
#include <AP_Math/AP_Math.h>
#define AUTOTUNE_DWELL_CYCLES 6
class AC_AutoTune_FreqResp {
public:
// Constructor
AC_AutoTune_FreqResp()
{
}
// Enumeration of input type
enum InputType {
DWELL = 0,
SWEEP = 1,
};
// Enumeration of type
enum ResponseType {
RATE = 0,
ANGLE = 1,
};
// Initialize the Frequency Response Object.
// Must be called before running dwell or frequency sweep tests
void init(InputType input_type, ResponseType response_type);
// Determines the gain and phase based on angle response for a dwell or sweep
void update(float command, float tgt_resp, float meas_resp, float tgt_freq);
// 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 data accessors
float get_freq() { return curr_test_freq; }
float get_gain() { return curr_test_gain; }
float get_phase() { return curr_test_phase; }
float get_accel_max() { return max_accel; }
private:
// time of the start of a new target value search. keeps noise from prematurely starting the search of a new target value.
uint32_t new_tgt_time_ms;
// flag for searching for a new target peak
bool new_target = false;
// maximum target value
float max_target;
// time of maximum target value in current cycle
uint32_t max_tgt_time;
// counter for target value maximums
uint16_t max_target_cnt;
// holds previously determined maximum target value while current cycle is running
float temp_max_target;
// holds previously determined time of maximum target value while current cycle is running
uint32_t temp_max_tgt_time;
// minimum target value
float min_target;
// counter for target value minimums
uint16_t min_target_cnt;
// holds previously determined minimum target value while current cycle is running
float temp_min_target;
// maximum target value from previous cycle
float prev_target;
// maximum target response from previous cycle
float prev_tgt_resp;
// holds target amplitude for gain calculation
float temp_tgt_ampl;
// time of the start of a new measured value search. keeps noise from prematurely starting the search of a new measured value.
uint32_t new_meas_time_ms;
// flag for searching for a new measured peak
bool new_meas = false;
// maximum measured value
float max_meas;
// time of maximum measured value in current cycle
uint32_t max_meas_time;
// counter for measured value maximums
uint16_t max_meas_cnt;
// holds previously determined maximum measured value while current cycle is running
float temp_max_meas;
// holds previously determined time of maximum measured value while current cycle is running
uint32_t temp_max_meas_time;
// minimum measured value
float min_meas;
// counter for measured value minimums
uint16_t min_meas_cnt;
// holds previously determined minimum measured value while current cycle is running
float temp_min_meas;
// maximum measured value from previous cycle
float prev_meas;
// maximum measured response from previous cycle
float prev_meas_resp;
// holds measured amplitude for gain calculation
float temp_meas_ampl;
// calculated target rate from angle data
float target_rate;
// calculated measured rate from angle data
float measured_rate;
// holds start time of input to track length of time that input in running
uint32_t input_start_time_ms;
// flag indicating when one oscillation cycle is complete
bool cycle_complete = false;
// current test frequency, gain, and phase
float curr_test_freq;
float curr_test_gain;
float curr_test_phase;
// maximum measured rate throughout excitation used for max accel calculation
float max_meas_rate;
// maximum command associated with maximum rate used for max accel calculation
float max_command;
// maximum acceleration in cdss determined during test
float max_accel;
// Input type for frequency response object
InputType excitation;
// Response type for frequency response object
ResponseType response;
// sweep_peak_finding_data tracks the peak data
struct sweep_peak_finding_data {
uint16_t count_m1;
float amplitude_m1;
float max_time_m1;
};
// Measured data for sweep peak
sweep_peak_finding_data sweep_meas;
// Target data for sweep peak
sweep_peak_finding_data sweep_tgt;
//store gain data in ring buffer
struct peak_info {
uint16_t curr_count;
float amplitude;
uint32_t time_ms;
};
// Buffer object for measured peak data
ObjectBuffer<peak_info> meas_peak_info_buffer{AUTOTUNE_DWELL_CYCLES};
// Buffer object for target peak data
ObjectBuffer<peak_info> tgt_peak_info_buffer{AUTOTUNE_DWELL_CYCLES};
// Push data into measured peak data buffer object
void push_to_meas_buffer(uint16_t count, float amplitude, uint32_t time_ms);
// Pull data from measured peak data buffer object
void pull_from_meas_buffer(uint16_t &count, float &amplitude, uint32_t &time_ms);
// Push data into target peak data buffer object
void push_to_tgt_buffer(uint16_t count, float amplitude, uint32_t time_ms);
// Pull data from target peak data buffer object
void pull_from_tgt_buffer(uint16_t &count, float &amplitude, uint32_t &time_ms);
};