Line data    Source code

       1             : /*
       2             : This library receives time history data (angular rate or angle) during a dwell test or frequency sweep test and determines the gain and phase of the response to the input. For dwell tests once the designated number of cycles are complete, the average of the gain and phase are determined over the last 5 cycles and the cycle_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.  The init function must be used when initializing the dwell or frequency sweep test.
       3             : */
       4             : 
       5             : #include <AP_HAL/AP_HAL.h>
       6             : #include "AC_AutoTune_FreqResp.h"
       7             : 
       8             : // Initialize the Frequency Response Object. Must be called before running dwell or frequency sweep tests
       9         294 : void AC_AutoTune_FreqResp::init(InputType input_type, ResponseType response_type, uint8_t cycles)
      10             : {
      11         294 :     excitation = input_type;
      12         294 :     response = response_type;
      13         294 :     max_target_cnt = 0;
      14         294 :     min_target_cnt = 0;
      15         294 :     max_meas_cnt = 0;
      16         294 :     min_meas_cnt = 0;
      17         294 :     input_start_time_ms = 0;
      18         294 :     new_tgt_time_ms = 0;
      19         294 :     new_meas_time_ms = 0;
      20         294 :     new_target = false;
      21         294 :     new_meas = false;
      22         294 :     curr_test_freq = 0.0f;
      23         294 :     curr_test_gain = 0.0f;
      24         294 :     curr_test_phase = 0.0f;
      25         294 :     max_accel = 0.0f;
      26         294 :     max_meas_rate = 0.0f;
      27         294 :     max_command = 0.0f;
      28         294 :     dwell_cycles = cycles;
      29         294 :     meas_peak_info_buffer.clear();
      30         294 :     tgt_peak_info_buffer.clear();
      31         294 :     cycle_complete = false;
      32         294 : }
      33             : 
      34             : // update_angle - this function receives time history data during a dwell and frequency sweep tests for angle_p tuning
      35             : // and determines the gain, phase, and max acceleration of the response to the input. For dwell tests once the designated number
      36             : // of cycles are complete, the average of the gain, phase, and max acceleration are determined over the last 5 cycles and the
      37             : // cycles_complete flag is set. For frequency sweep tests, phase and gain are determined for every cycle and cycle_complete flag is set
      38             : // to indicate when to pull the phase and gain data.  The flag is reset to enable the next cycle to be analyzed.
      39      802260 : void AC_AutoTune_FreqResp::update(float command, float tgt_resp, float meas_resp, float tgt_freq)
      40             : {
      41             : 
      42      802260 :     uint32_t now_ms = AP_HAL::millis();
      43      802260 :     float dt = 0.0025;
      44      802260 :     uint32_t half_cycle_time_ms = 0;
      45      802260 :     uint32_t cycle_time_ms = 0;
      46             : 
      47      802260 :     if (cycle_complete) {
      48         198 :         return;
      49             :     }
      50             : 
      51      802062 :     if (!is_zero(tgt_freq)) {
      52      802062 :         half_cycle_time_ms = (uint32_t)(300 * M_2PI / tgt_freq);
      53      802062 :         cycle_time_ms = (uint32_t)(1000 * M_2PI / tgt_freq);
      54             :     }
      55             : 
      56      802062 :     if (input_start_time_ms == 0) {
      57         294 :         input_start_time_ms = now_ms;
      58         294 :         if (response == ANGLE) {
      59          32 :             prev_tgt_resp = tgt_resp;
      60          32 :             prev_meas_resp = meas_resp;
      61          32 :             prev_target = 0.0f;
      62          32 :             prev_meas = 0.0f;
      63             :         }
      64             :     }
      65             : 
      66      802062 :     if (response == ANGLE) {
      67      135634 :         target_rate = (tgt_resp - prev_tgt_resp) / dt;
      68      135634 :         measured_rate = (meas_resp - prev_meas_resp) / dt;
      69             :     } else {
      70      666428 :         target_rate = tgt_resp;
      71      666428 :         measured_rate = meas_resp;
      72             :     }
      73             : 
      74             :     // cycles are complete! determine gain and phase and exit
      75      802062 :     if (max_meas_cnt > dwell_cycles + 1 && max_target_cnt > dwell_cycles + 1 && excitation == DWELL) {
      76         276 :         float delta_time = 0.0f;
      77         276 :         float sum_gain = 0.0f;
      78         276 :         uint8_t cnt = 0;
      79         276 :         uint8_t gcnt = 0;
      80             :         uint16_t meas_cnt, tgt_cnt;
      81         276 :         float meas_ampl = 0.0f;
      82         276 :         float tgt_ampl = 0.0f;
      83         276 :         uint32_t meas_time = 0;
      84         276 :         uint32_t tgt_time = 0;
      85        1722 :         for (uint8_t i = 0;  i < dwell_cycles; i++) {
      86        1446 :             meas_cnt=0;
      87        1446 :             tgt_cnt=0;
      88        1446 :             pull_from_meas_buffer(meas_cnt, meas_ampl, meas_time);
      89        1446 :             pull_from_tgt_buffer(tgt_cnt, tgt_ampl, tgt_time);
      90        1446 :             push_to_meas_buffer(0, 0.0f, 0);
      91        1446 :             push_to_tgt_buffer(0, 0.0f, 0);
      92             : 
      93        1446 :             if (meas_cnt == tgt_cnt && meas_cnt != 0) {
      94        1446 :                 if (tgt_ampl > 0.0f) {
      95        1446 :                     sum_gain += meas_ampl / tgt_ampl;
      96        1446 :                     gcnt++;
      97             :                 }
      98        1446 :                 float d_time = (float)(meas_time - tgt_time);
      99        1446 :                 if (d_time < 2.0f * (float)cycle_time_ms) {
     100        1445 :                     delta_time += d_time;
     101        1445 :                     cnt++;
     102             :                 }
     103        1446 :             } else if (meas_cnt > tgt_cnt) {
     104           0 :                 pull_from_tgt_buffer(tgt_cnt, tgt_ampl, tgt_time);
     105           0 :                 push_to_tgt_buffer(0, 0.0f, 0);
     106           0 :             } else if (meas_cnt < tgt_cnt) {
     107           0 :                 pull_from_meas_buffer(meas_cnt, meas_ampl, meas_time);
     108           0 :                 push_to_meas_buffer(0, 0.0f, 0);
     109             :             }        
     110             :         }
     111         276 :         if (gcnt > 0) {
     112         276 :             curr_test_gain = sum_gain / gcnt;
     113             :         }
     114         276 :         if (cnt > 0) {
     115         276 :             delta_time = delta_time / cnt;
     116             :         }
     117         276 :         curr_test_phase = tgt_freq * delta_time * 0.001f * 360.0f / M_2PI;
     118         276 :         if (curr_test_phase > 360.0f) {
     119           0 :             curr_test_phase = curr_test_phase - 360.0f;
     120             :         }
     121             : 
     122             :         // determine max accel for angle response type test
     123             :         float dwell_max_accel;
     124         276 :         if (response == ANGLE) {
     125          20 :             dwell_max_accel = tgt_freq * max_meas_rate * 5730.0f;
     126          20 :             if (!is_zero(max_command)) {
     127             :                 // normalize accel for input size
     128          20 :                 dwell_max_accel = dwell_max_accel / (2.0f * max_command);
     129             :             }
     130          20 :             if (dwell_max_accel > max_accel) {
     131          20 :                 max_accel = dwell_max_accel;
     132             :             }
     133             :         }
     134             : 
     135         276 :         curr_test_freq = tgt_freq;
     136         276 :         cycle_complete = true;
     137         276 :         return;
     138             :     }
     139             : 
     140             :     // Indicates when the target(input) is positive or negative half of the cycle to notify when the max or min should be sought
     141      135614 :     if (((response == ANGLE && is_positive(prev_target) && !is_positive(target_rate))
     142      799709 :         || (response == RATE && !is_positive(prev_target) && is_positive(target_rate)))
     143     1603572 :         && !new_target && now_ms > new_tgt_time_ms) {
     144        3780 :         new_target = true;
     145        3780 :         new_tgt_time_ms = now_ms + half_cycle_time_ms;
     146             :         // reset max_target
     147        3780 :         max_target = 0.0f;
     148        3780 :         max_target_cnt++;
     149        3780 :         temp_min_target = min_target;
     150        3780 :         if (min_target_cnt > 0) {
     151        3486 :             sweep_tgt.max_time_m1 = temp_max_tgt_time;
     152        3486 :             temp_max_tgt_time = max_tgt_time;
     153        3486 :             sweep_tgt.count_m1 = min_target_cnt - 1;
     154        3486 :             sweep_tgt.amplitude_m1 = temp_tgt_ampl;
     155        3486 :             temp_tgt_ampl = temp_max_target - temp_min_target;
     156        3486 :             if (excitation == DWELL) {
     157        1722 :                 push_to_tgt_buffer(min_target_cnt,temp_tgt_ampl,temp_max_tgt_time);
     158             :             }
     159             :         }
     160             : 
     161      134327 :     } else if (((response == ANGLE && !is_positive(prev_target) && is_positive(target_rate))
     162      795914 :                || (response == RATE && is_positive(prev_target) && !is_positive(target_rate)))
     163     1596012 :                && new_target && now_ms > new_tgt_time_ms && max_target_cnt > 0) {
     164        3582 :         new_target = false;
     165        3582 :         new_tgt_time_ms = now_ms + half_cycle_time_ms;
     166        3582 :         min_target_cnt++;
     167        3582 :         temp_max_target = max_target;
     168        3582 :         min_target = 0.0f;
     169             :     }
     170             : 
     171             :     // Indicates when the measured value (output) is positive or negative half of the cycle to notify when the max or min should be sought
     172      135614 :     if (((response == ANGLE && is_positive(prev_meas) && !is_positive(measured_rate))
     173      800556 :          || (response == RATE && !is_positive(prev_meas) && is_positive(measured_rate)))
     174     1603572 :          && !new_meas && now_ms > new_meas_time_ms && max_target_cnt > 0) {
     175        3638 :         new_meas = true;
     176        3638 :         new_meas_time_ms = now_ms + half_cycle_time_ms;
     177             :         // reset max_meas
     178        3638 :         max_meas = 0.0f;
     179        3638 :         max_meas_cnt++;
     180        3638 :         temp_min_meas = min_meas;
     181        3638 :         if (min_meas_cnt > 0 && min_target_cnt > 0) {
     182        3343 :             sweep_meas.max_time_m1 = temp_max_meas_time;
     183        3343 :             temp_max_meas_time = max_meas_time;
     184        3343 :             sweep_meas.count_m1 = min_meas_cnt - 1;
     185        3343 :             sweep_meas.amplitude_m1 = temp_meas_ampl;
     186        3343 :             temp_meas_ampl = temp_max_meas - temp_min_meas;
     187        3343 :             if (excitation == DWELL) {
     188        1722 :                 push_to_meas_buffer(min_meas_cnt,temp_meas_ampl,temp_max_meas_time);
     189             :             }
     190        3343 :             if (excitation == SWEEP) {
     191        1621 :                 float tgt_period = 0.001f * (temp_max_tgt_time - sweep_tgt.max_time_m1);
     192        1621 :                 if (!is_zero(tgt_period)) {
     193        1599 :                     curr_test_freq = M_2PI / tgt_period;
     194             :                 } else {
     195          22 :                     curr_test_freq = 0.0f;
     196             :                 }
     197        1621 :                 if (!is_zero(sweep_tgt.amplitude_m1)) {
     198        1621 :                     curr_test_gain = sweep_meas.amplitude_m1/sweep_tgt.amplitude_m1;
     199             :                 } else {
     200           0 :                     curr_test_gain = 0.0f;
     201             :                 }
     202        1621 :                 curr_test_phase = curr_test_freq * (float)(sweep_meas.max_time_m1 - sweep_tgt.max_time_m1) * 0.001f * 360.0f / M_2PI;
     203        1621 :                 cycle_complete = true;
     204             :             }
     205             :         } 
     206      134462 :     } else if (((response == ANGLE && !is_positive(prev_meas) && is_positive(measured_rate))
     207      796906 :                 || (response == RATE && is_positive(prev_meas) && !is_positive(measured_rate)))
     208     1596296 :                 && new_meas && now_ms > new_meas_time_ms && max_meas_cnt > 0) {
     209        3360 :         new_meas = false;
     210        3360 :         new_meas_time_ms = now_ms + half_cycle_time_ms;
     211        3360 :         min_meas_cnt++;
     212        3360 :         temp_max_meas = max_meas;
     213        3360 :         min_meas = 0.0f;
     214             :     }
     215             : 
     216      801786 :     if (new_target) {
     217      398432 :         if (tgt_resp > max_target) {
     218      169044 :             max_target = tgt_resp;
     219      169044 :             max_tgt_time = now_ms;
     220             :         }
     221             :     } else {
     222      403354 :         if (tgt_resp < min_target) {
     223      153970 :             min_target = tgt_resp;
     224             :         }
     225             :     }
     226             : 
     227      801786 :     if (new_meas) {
     228      377032 :         if (meas_resp > max_meas) {
     229      157890 :             max_meas = meas_resp;
     230      157890 :             max_meas_time = now_ms;
     231             :         }
     232             :     } else {
     233      424754 :         if (meas_resp < min_meas) {
     234      161489 :             min_meas = meas_resp;
     235             :         }
     236             :     }
     237             : 
     238      801786 :     if (response == ANGLE) {
     239      135614 :         if (now_ms > (uint32_t)(input_start_time_ms + 7.0f * cycle_time_ms) && now_ms < (uint32_t)(input_start_time_ms + 9.0f * cycle_time_ms)) {
     240        6988 :             if (measured_rate > max_meas_rate) {
     241        1158 :                 max_meas_rate = measured_rate;
     242             :             }
     243        6988 :             if (command > max_command) {
     244        1288 :                 max_command = command;
     245             :             }
     246             :         }
     247      135614 :         prev_tgt_resp = tgt_resp;
     248      135614 :         prev_meas_resp = meas_resp;
     249             :     }
     250             : 
     251      801786 :     prev_target = target_rate;
     252      801786 :     prev_meas = measured_rate;
     253             : }
     254             : 
     255             : // push measured peak info to buffer
     256        3168 : void AC_AutoTune_FreqResp::push_to_meas_buffer(uint16_t count, float amplitude, uint32_t time_ms)
     257             : {
     258             :     peak_info sample;
     259        3168 :     sample.curr_count = count;
     260        3168 :     sample.amplitude = amplitude;
     261        3168 :     sample.time_ms = time_ms;
     262        3168 :     meas_peak_info_buffer.push(sample);
     263        3168 : }
     264             : 
     265             : // pull measured peak info from buffer
     266        1446 : void AC_AutoTune_FreqResp::pull_from_meas_buffer(uint16_t &count, float &amplitude, uint32_t &time_ms)
     267             : {
     268             :     peak_info sample;
     269        1446 :     if (!meas_peak_info_buffer.pop(sample)) {
     270             :         // no sample
     271           0 :         return;
     272             :     }
     273        1446 :     count = sample.curr_count;
     274        1446 :     amplitude = sample.amplitude;
     275        1446 :     time_ms = sample.time_ms;
     276             : }
     277             : 
     278             : // push target peak info to buffer
     279        3168 : void AC_AutoTune_FreqResp::push_to_tgt_buffer(uint16_t count, float amplitude, uint32_t time_ms)
     280             : {
     281             :     peak_info sample;
     282        3168 :     sample.curr_count = count;
     283        3168 :     sample.amplitude = amplitude;
     284        3168 :     sample.time_ms = time_ms;
     285        3168 :     tgt_peak_info_buffer.push(sample);
     286        3168 : }
     287             : 
     288             : // pull target peak info from buffer
     289        1446 : void AC_AutoTune_FreqResp::pull_from_tgt_buffer(uint16_t &count, float &amplitude, uint32_t &time_ms)
     290             : {
     291             :     peak_info sample;
     292        1446 :     if (!tgt_peak_info_buffer.pop(sample)) {
     293             :         // no sample
     294           0 :         return;
     295             :     }
     296        1446 :     count = sample.curr_count;
     297        1446 :     amplitude = sample.amplitude;
     298        1446 :     time_ms = sample.time_ms;
     299             : }
     300             :