Line data    Source code

       1             : #pragma once
       2             : 
       3             : /*
       4             :  Gain and phase determination algorithm
       5             : */
       6             : 
       7             : #include <AP_Math/AP_Math.h>
       8             : 
       9             : class AC_AutoTune_FreqResp {
      10             : public:
      11             :     // Constructor
      12         166 :     AC_AutoTune_FreqResp()
      13         166 : {
      14         166 : }
      15             : 
      16             :     // Enumeration of input type
      17             :     enum InputType {
      18             :         DWELL = 0,                 
      19             :         SWEEP = 1, 
      20             :     };
      21             : 
      22             :     // Enumeration of type
      23             :     enum ResponseType {
      24             :         RATE = 0,                 
      25             :         ANGLE = 1, 
      26             :     };
      27             : 
      28             :     // Initialize the Frequency Response Object. 
      29             :     // Must be called before running dwell or frequency sweep tests
      30             :     void init(InputType input_type, ResponseType response_type, uint8_t cycles);
      31             : 
      32             :     // Determines the gain and phase based on angle response for a dwell or sweep
      33             :     void update(float command, float tgt_resp, float meas_resp, float tgt_freq);
      34             : 
      35             :     // Enable external query if cycle is complete and freq response data are available
      36     1279115 :     bool is_cycle_complete() { return cycle_complete;}
      37             : 
      38             :     // Reset cycle_complete flag
      39        1621 :     void reset_cycle_complete() { cycle_complete = false; }
      40             : 
      41             :     // Frequency response data accessors
      42        1646 :     float get_freq() { return curr_test_freq; }
      43        2060 :     float get_gain() { return curr_test_gain; }
      44        2060 :     float get_phase() { return curr_test_phase; }
      45         208 :     float get_accel_max() { return max_accel; }
      46             : 
      47             : private:
      48             :     // time of the start of a new target value search.  keeps noise from prematurely starting the search of a new target value.
      49             :     uint32_t new_tgt_time_ms;
      50             : 
      51             :     // flag for searching for a new target peak
      52             :     bool new_target = false;
      53             : 
      54             :     // maximum target value
      55             :     float max_target;
      56             : 
      57             :     // time of maximum target value in current cycle
      58             :     uint32_t max_tgt_time;
      59             : 
      60             :     // counter for target value maximums
      61             :     uint16_t max_target_cnt;
      62             : 
      63             :     // holds previously determined maximum target value while current cycle is running
      64             :     float temp_max_target;
      65             : 
      66             :     // holds previously determined time of maximum target value while current cycle is running
      67             :     uint32_t temp_max_tgt_time;
      68             : 
      69             :     // minimum target value
      70             :     float min_target;
      71             : 
      72             :     // counter for target value minimums
      73             :     uint16_t min_target_cnt;
      74             : 
      75             :     // holds previously determined minimum target value while current cycle is running
      76             :     float temp_min_target;
      77             : 
      78             :     // maximum target value from previous cycle
      79             :     float prev_target;
      80             : 
      81             :     // maximum target response from previous cycle
      82             :     float prev_tgt_resp;
      83             : 
      84             :     // holds target amplitude for gain calculation
      85             :     float temp_tgt_ampl;
      86             : 
      87             :     // time of the start of a new measured value search.  keeps noise from prematurely starting the search of a new measured value.
      88             :     uint32_t new_meas_time_ms;
      89             : 
      90             :     // flag for searching for a new measured peak
      91             :     bool new_meas = false;
      92             : 
      93             :     // maximum measured value
      94             :     float max_meas;
      95             : 
      96             :     // time of maximum measured value in current cycle
      97             :     uint32_t max_meas_time;
      98             : 
      99             :     // counter for measured value maximums
     100             :     uint16_t max_meas_cnt;
     101             : 
     102             :     // holds previously determined maximum measured value while current cycle is running
     103             :     float temp_max_meas;
     104             : 
     105             :     // holds previously determined time of maximum measured value while current cycle is running
     106             :     uint32_t temp_max_meas_time;
     107             : 
     108             :     // minimum measured value
     109             :     float min_meas;
     110             : 
     111             :     // counter for measured value minimums
     112             :     uint16_t min_meas_cnt;
     113             : 
     114             :     // holds previously determined minimum measured value while current cycle is running
     115             :     float temp_min_meas;
     116             : 
     117             :     // maximum measured value from previous cycle
     118             :     float prev_meas;
     119             : 
     120             :     // maximum measured response from previous cycle
     121             :     float prev_meas_resp;
     122             : 
     123             :     // holds measured amplitude for gain calculation
     124             :     float temp_meas_ampl;
     125             : 
     126             :     // calculated target rate from angle data
     127             :     float target_rate;
     128             : 
     129             :     // calculated measured rate from angle data
     130             :     float measured_rate;
     131             : 
     132             :     // holds start time of input to track length of time that input in running
     133             :     uint32_t input_start_time_ms;
     134             : 
     135             :     // flag indicating when one oscillation cycle is complete
     136             :     bool cycle_complete = false;
     137             : 
     138             :     // number of dwell cycles to complete for dwell excitation
     139             :     uint8_t dwell_cycles;
     140             : 
     141             :     // current test frequency, gain, and phase
     142             :     float curr_test_freq; 
     143             :     float curr_test_gain;
     144             :     float curr_test_phase;
     145             : 
     146             :     // maximum measured rate throughout excitation used for max accel calculation
     147             :     float max_meas_rate;
     148             : 
     149             :     // maximum command associated with maximum rate used for max accel calculation
     150             :     float max_command;
     151             : 
     152             :     // maximum acceleration in cdss determined during test
     153             :     float max_accel;
     154             : 
     155             :     // Input type for frequency response object
     156             :     InputType excitation;
     157             : 
     158             :     // Response type for frequency response object
     159             :     ResponseType response;
     160             : 
     161             :     // sweep_peak_finding_data tracks the peak data
     162             :     struct sweep_peak_finding_data {
     163             :         uint16_t count_m1;
     164             :         float amplitude_m1;
     165             :         float max_time_m1;
     166             :     };
     167             : 
     168             :     // Measured data for sweep peak
     169             :     sweep_peak_finding_data sweep_meas;
     170             : 
     171             :     // Target data for sweep peak
     172             :     sweep_peak_finding_data sweep_tgt;
     173             : 
     174             :     //store gain data in ring buffer
     175             :     struct peak_info {
     176             :         uint16_t curr_count;
     177             :         float amplitude;
     178             :         uint32_t time_ms; 
     179             : 
     180             :     };
     181             : 
     182             :     // Buffer object for measured peak data
     183             :     ObjectBuffer<peak_info> meas_peak_info_buffer{12};
     184             : 
     185             :     // Buffer object for target peak data
     186             :     ObjectBuffer<peak_info> tgt_peak_info_buffer{12};
     187             : 
     188             :     // Push data into measured peak data buffer object
     189             :     void push_to_meas_buffer(uint16_t count, float amplitude, uint32_t time_ms);
     190             : 
     191             :     // Pull data from measured peak data buffer object
     192             :     void pull_from_meas_buffer(uint16_t &count, float &amplitude, uint32_t &time_ms);
     193             : 
     194             :     // Push data into target peak data buffer object
     195             :     void push_to_tgt_buffer(uint16_t count, float amplitude, uint32_t time_ms);
     196             : 
     197             :     // Pull data from target peak data buffer object
     198             :     void pull_from_tgt_buffer(uint16_t &count, float &amplitude, uint32_t &time_ms);
     199             : 
     200             : };