AC_AutoTune: incorporated suggested changes

libraries/AC_AutoTune/AC_AutoTune_FreqResp.cpp

@@ -65,7 +65,7 @@ void AC_AutoTune_FreqResp::update_rate(float tgt_rate, float meas_rate, float tg
         float tgt_ampl = 0.0f;
         uint32_t meas_time = 0;
         uint32_t tgt_time = 0;
-        for (int i = 0;  i < AUTOTUNE_DWELL_CYCLES; i++) {
+        for (uint8_t i = 0;  i < AUTOTUNE_DWELL_CYCLES; i++) {
             meas_cnt=0;
             tgt_cnt=0;
             pull_from_meas_buffer(meas_cnt, meas_ampl, meas_time);
@@ -245,7 +245,7 @@ void AC_AutoTune_FreqResp::update_angle(float command, float tgt_angle, float me
         float tgt_ampl = 0.0f;
         uint32_t meas_time = 0;
         uint32_t tgt_time = 0;
-        for (int i = 0;  i < AUTOTUNE_DWELL_CYCLES; i++) {
+        for (uint8_t i = 0;  i < AUTOTUNE_DWELL_CYCLES; i++) {
             meas_cnt=0;
             tgt_cnt=0;
             pull_from_meas_buffer(meas_cnt, meas_ampl, meas_time);

libraries/AC_AutoTune/AC_AutoTune_FreqResp.h

@@ -19,6 +19,7 @@ public:
     tgt_peak_info_buffer = new ObjectBuffer<peak_info>(AUTOTUNE_DWELL_CYCLES);
 }
 
+    // Enumeration of input type
     enum InputType {
         DWELL = 0,                 
         SWEEP = 1, 
@@ -28,37 +29,130 @@ public:
     // Must be called before running dwell or frequency sweep tests
     void init(InputType input_type);
 
-    // determines the gain and phase based on rate response for a dwell or sweep
+    // 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 based on angle response for a dwell or sweep
+    // 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 freq response 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
+    // Reset cycle_complete flag
     void reset_cycle_complete() { cycle_complete = false; }
 
-    // frequency response data 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; }
     float get_accel_max() { return max_accel; }
 
 private:
-    float max_target, max_meas, prev_target, prev_meas, prev_tgt_angle, prev_meas_angle;
+    // time of the start of a new target value search.  keeps noise from prematurely starting the search of a new target value.
-    float min_target, min_meas, temp_meas_ampl, temp_tgt_ampl;
+    uint32_t new_tgt_time_ms;
-    float temp_max_target, temp_min_target, target_rate, measured_rate, max_meas_rate, max_command;
+
-    float temp_max_meas, temp_min_meas;
+    // flag for searching for a new target peak
-    uint32_t temp_max_tgt_time, temp_max_meas_time;
-    uint32_t max_tgt_time, max_meas_time, new_tgt_time_ms, new_meas_time_ms, input_start_time_ms;
-    uint16_t min_target_cnt, max_target_cnt, max_meas_cnt, min_meas_cnt;
     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 angle from previous cycle
+    float prev_tgt_angle;
+
+    // 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 angle from previous cycle
+    float prev_meas_angle;
+
+    // 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;
-    float curr_test_freq, curr_test_gain, curr_test_phase;
+
+    // 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;
 
     // sweep_peak_finding_data tracks the peak data
@@ -67,7 +161,11 @@ private:
         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
@@ -77,11 +175,23 @@ private:
         uint32_t time_ms; 
 
     };
+
+    // Buffer object for measured peak data
     ObjectBuffer<peak_info> *meas_peak_info_buffer;
+
+    // Buffer object for target peak data
     ObjectBuffer<peak_info> *tgt_peak_info_buffer;
+
+    // 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);
 
 };