auto-measurements
Dependencies: FastPWM3 mbed-dev
Fork of Hobbyking_Cheetah_Compact by
Calibration/calibration.cpp@34:47a55f96fbc4, 2017-10-02 (annotated)
- Committer:
- benkatz
- Date:
- Mon Oct 02 00:55:39 2017 +0000
- Revision:
- 34:47a55f96fbc4
- Parent:
- 28:8c7e29f719c5
Auto-measurement
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
benkatz | 22:60276ba87ac6 | 1 | /// Calibration procedures for determining position sensor offset, |
benkatz | 22:60276ba87ac6 | 2 | /// phase ordering, and position sensor linearization |
benkatz | 22:60276ba87ac6 | 3 | /// |
benkatz | 22:60276ba87ac6 | 4 | |
benkatz | 22:60276ba87ac6 | 5 | #include "calibration.h" |
benkatz | 23:2adf23ee0305 | 6 | #include "foc.h" |
benkatz | 23:2adf23ee0305 | 7 | #include "PreferenceWriter.h" |
benkatz | 23:2adf23ee0305 | 8 | #include "user_config.h" |
benkatz | 34:47a55f96fbc4 | 9 | #include "math.h" |
benkatz | 34:47a55f96fbc4 | 10 | #include "math_ops.h" |
benkatz | 34:47a55f96fbc4 | 11 | |
benkatz | 34:47a55f96fbc4 | 12 | void measure_rl(int n, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs){ |
benkatz | 34:47a55f96fbc4 | 13 | |
benkatz | 34:47a55f96fbc4 | 14 | |
benkatz | 34:47a55f96fbc4 | 15 | } |
benkatz | 22:60276ba87ac6 | 16 | |
benkatz | 23:2adf23ee0305 | 17 | void order_phases(PositionSensor *ps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs){ |
benkatz | 28:8c7e29f719c5 | 18 | |
benkatz | 22:60276ba87ac6 | 19 | ///Checks phase order, to ensure that positive Q current produces |
benkatz | 22:60276ba87ac6 | 20 | ///torque in the positive direction wrt the position sensor. |
benkatz | 22:60276ba87ac6 | 21 | printf("\n\r Checking phase ordering\n\r"); |
benkatz | 22:60276ba87ac6 | 22 | float theta_ref = 0; |
benkatz | 22:60276ba87ac6 | 23 | float theta_actual = 0; |
benkatz | 34:47a55f96fbc4 | 24 | float v_d = 2.0; //Put all volts on the D-Axis |
benkatz | 22:60276ba87ac6 | 25 | float v_q = 0.0; |
benkatz | 22:60276ba87ac6 | 26 | float v_u, v_v, v_w = 0; |
benkatz | 22:60276ba87ac6 | 27 | float dtc_u, dtc_v, dtc_w = .5; |
benkatz | 22:60276ba87ac6 | 28 | int sample_counter = 0; |
benkatz | 22:60276ba87ac6 | 29 | |
benkatz | 22:60276ba87ac6 | 30 | ///Set voltage angle to zero, wait for rotor position to settle |
benkatz | 25:f5741040c4bb | 31 | abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); //inverse dq0 transform on voltages |
benkatz | 34:47a55f96fbc4 | 32 | svm(V_BUS, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); //space vector modulation |
benkatz | 34:47a55f96fbc4 | 33 | TIM1->CCR3 = (PWM_ARR)*(0.5f); // Set duty cycles |
benkatz | 34:47a55f96fbc4 | 34 | TIM1->CCR2 = (PWM_ARR)*(0.5f); |
benkatz | 34:47a55f96fbc4 | 35 | TIM1->CCR1 = (PWM_ARR)*(0.5f); |
benkatz | 34:47a55f96fbc4 | 36 | wait_us(100); |
benkatz | 22:60276ba87ac6 | 37 | for(int i = 0; i<20000; i++){ |
benkatz | 34:47a55f96fbc4 | 38 | wait_us(50); |
benkatz | 34:47a55f96fbc4 | 39 | TIM1->CCR3 = (PWM_ARR)*(1.0f-dtc_u); // Set duty cycles |
benkatz | 34:47a55f96fbc4 | 40 | TIM1->CCR2 = (PWM_ARR)*(1.0f-dtc_v); |
benkatz | 34:47a55f96fbc4 | 41 | TIM1->CCR1 = (PWM_ARR)*(1.0f-dtc_w); |
benkatz | 22:60276ba87ac6 | 42 | } |
benkatz | 34:47a55f96fbc4 | 43 | |
benkatz | 34:47a55f96fbc4 | 44 | |
benkatz | 22:60276ba87ac6 | 45 | //ps->ZeroPosition(); |
benkatz | 22:60276ba87ac6 | 46 | ps->Sample(); |
benkatz | 23:2adf23ee0305 | 47 | wait_us(1000); |
benkatz | 26:2b865c00d7e9 | 48 | //float theta_start = ps->GetMechPosition(); //get initial rotor position |
benkatz | 26:2b865c00d7e9 | 49 | float theta_start; |
benkatz | 34:47a55f96fbc4 | 50 | |
benkatz | 22:60276ba87ac6 | 51 | /// Rotate voltage angle |
benkatz | 34:47a55f96fbc4 | 52 | while(theta_ref < 4.0f*PI){ //rotate for 2 electrical cycles |
benkatz | 25:f5741040c4bb | 53 | abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); //inverse dq0 transform on voltages |
benkatz | 34:47a55f96fbc4 | 54 | svm(V_BUS, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); //space vector modulation |
benkatz | 22:60276ba87ac6 | 55 | wait_us(100); |
benkatz | 34:47a55f96fbc4 | 56 | TIM1->CCR3 = (PWM_ARR)*(1.0f-dtc_u); //Set duty cycles |
benkatz | 34:47a55f96fbc4 | 57 | TIM1->CCR2 = (PWM_ARR)*(1.0f-dtc_v); |
benkatz | 34:47a55f96fbc4 | 58 | TIM1->CCR1 = (PWM_ARR)*(1.0f-dtc_w); |
benkatz | 25:f5741040c4bb | 59 | ps->Sample(); //sample position sensor |
benkatz | 22:60276ba87ac6 | 60 | theta_actual = ps->GetMechPosition(); |
benkatz | 26:2b865c00d7e9 | 61 | if(theta_ref==0){theta_start = theta_actual;} |
benkatz | 22:60276ba87ac6 | 62 | if(sample_counter > 200){ |
benkatz | 22:60276ba87ac6 | 63 | sample_counter = 0 ; |
benkatz | 22:60276ba87ac6 | 64 | printf("%.4f %.4f\n\r", theta_ref/(NPP), theta_actual); |
benkatz | 22:60276ba87ac6 | 65 | } |
benkatz | 22:60276ba87ac6 | 66 | sample_counter++; |
benkatz | 22:60276ba87ac6 | 67 | theta_ref += 0.001f; |
benkatz | 22:60276ba87ac6 | 68 | } |
benkatz | 22:60276ba87ac6 | 69 | float theta_end = ps->GetMechPosition(); |
benkatz | 22:60276ba87ac6 | 70 | int direction = (theta_end - theta_start)>0; |
benkatz | 34:47a55f96fbc4 | 71 | float ratio = abs(4.0f*PI/(theta_end-theta_start)); |
benkatz | 34:47a55f96fbc4 | 72 | int pole_pairs = (int) roundf(ratio); |
benkatz | 34:47a55f96fbc4 | 73 | |
benkatz | 22:60276ba87ac6 | 74 | printf("Theta Start: %f Theta End: %f\n\r", theta_start, theta_end); |
benkatz | 22:60276ba87ac6 | 75 | printf("Direction: %d\n\r", direction); |
benkatz | 23:2adf23ee0305 | 76 | if(direction){printf("Phasing correct\n\r");} |
benkatz | 22:60276ba87ac6 | 77 | else if(!direction){printf("Phasing incorrect. Swapping phases V and W\n\r");} |
benkatz | 23:2adf23ee0305 | 78 | PHASE_ORDER = direction; |
benkatz | 22:60276ba87ac6 | 79 | } |
benkatz | 22:60276ba87ac6 | 80 | |
benkatz | 22:60276ba87ac6 | 81 | |
benkatz | 23:2adf23ee0305 | 82 | void calibrate(PositionSensor *ps, GPIOStruct *gpio, ControllerStruct *controller, PreferenceWriter *prefs){ |
benkatz | 22:60276ba87ac6 | 83 | /// Measures the electrical angle offset of the position sensor |
benkatz | 22:60276ba87ac6 | 84 | /// and (in the future) corrects nonlinearity due to position sensor eccentricity |
benkatz | 22:60276ba87ac6 | 85 | printf("Starting calibration procedure\n\r"); |
benkatz | 22:60276ba87ac6 | 86 | |
benkatz | 25:f5741040c4bb | 87 | const int n = 128*NPP; // number of positions to be sampled per mechanical rotation. Multiple of NPP for filtering reasons (see later) |
benkatz | 25:f5741040c4bb | 88 | const int n2 = 50; // increments between saved samples (for smoothing motion) |
benkatz | 25:f5741040c4bb | 89 | float delta = 2*PI*NPP/(n*n2); // change in angle between samples |
benkatz | 25:f5741040c4bb | 90 | float error_f[n] = {0}; // error vector rotating forwards |
benkatz | 25:f5741040c4bb | 91 | float error_b[n] = {0}; // error vector rotating backwards |
benkatz | 28:8c7e29f719c5 | 92 | const int n_lut = 128; |
benkatz | 28:8c7e29f719c5 | 93 | int lut[n_lut]= {0}; // clear any old lookup table before starting. |
benkatz | 28:8c7e29f719c5 | 94 | ps->WriteLUT(lut); |
benkatz | 22:60276ba87ac6 | 95 | int raw_f[n] = {0}; |
benkatz | 22:60276ba87ac6 | 96 | int raw_b[n] = {0}; |
benkatz | 22:60276ba87ac6 | 97 | float theta_ref = 0; |
benkatz | 22:60276ba87ac6 | 98 | float theta_actual = 0; |
benkatz | 34:47a55f96fbc4 | 99 | float v_d = 2.0f; // Put volts on the D-Axis |
benkatz | 22:60276ba87ac6 | 100 | float v_q = 0.0; |
benkatz | 22:60276ba87ac6 | 101 | float v_u, v_v, v_w = 0; |
benkatz | 22:60276ba87ac6 | 102 | float dtc_u, dtc_v, dtc_w = .5; |
benkatz | 22:60276ba87ac6 | 103 | |
benkatz | 22:60276ba87ac6 | 104 | |
benkatz | 22:60276ba87ac6 | 105 | ///Set voltage angle to zero, wait for rotor position to settle |
benkatz | 25:f5741040c4bb | 106 | abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages |
benkatz | 34:47a55f96fbc4 | 107 | svm(V_BUS, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation |
benkatz | 22:60276ba87ac6 | 108 | for(int i = 0; i<40000; i++){ |
benkatz | 34:47a55f96fbc4 | 109 | TIM1->CCR3 = (PWM_ARR)*(1.0f-dtc_u); // Set duty cycles |
benkatz | 24:58c2d7571207 | 110 | if(PHASE_ORDER){ |
benkatz | 34:47a55f96fbc4 | 111 | TIM1->CCR2 = (PWM_ARR)*(1.0f-dtc_v); |
benkatz | 34:47a55f96fbc4 | 112 | TIM1->CCR1 = (PWM_ARR)*(1.0f-dtc_w); |
benkatz | 22:60276ba87ac6 | 113 | } |
benkatz | 22:60276ba87ac6 | 114 | else{ |
benkatz | 34:47a55f96fbc4 | 115 | TIM1->CCR1 = (PWM_ARR)*(1.0f-dtc_v); |
benkatz | 34:47a55f96fbc4 | 116 | TIM1->CCR2 = (PWM_ARR)*(1.0f-dtc_w); |
benkatz | 22:60276ba87ac6 | 117 | } |
benkatz | 22:60276ba87ac6 | 118 | wait_us(100); |
benkatz | 22:60276ba87ac6 | 119 | } |
benkatz | 22:60276ba87ac6 | 120 | ps->Sample(); |
benkatz | 26:2b865c00d7e9 | 121 | printf(" Current Angle : Rotor Angle : Raw Encoder \n\r\n\r"); |
benkatz | 25:f5741040c4bb | 122 | for(int i = 0; i<n; i++){ // rotate forwards |
benkatz | 22:60276ba87ac6 | 123 | for(int j = 0; j<n2; j++){ |
benkatz | 22:60276ba87ac6 | 124 | theta_ref += delta; |
benkatz | 25:f5741040c4bb | 125 | abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages |
benkatz | 34:47a55f96fbc4 | 126 | svm(V_BUS, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation |
benkatz | 34:47a55f96fbc4 | 127 | TIM1->CCR3 = (PWM_ARR)*(1.0f-dtc_u); |
benkatz | 25:f5741040c4bb | 128 | if(PHASE_ORDER){ // Check phase ordering |
benkatz | 34:47a55f96fbc4 | 129 | TIM1->CCR2 = (PWM_ARR)*(1.0f-dtc_v); // Set duty cycles |
benkatz | 34:47a55f96fbc4 | 130 | TIM1->CCR1 = (PWM_ARR)*(1.0f-dtc_w); |
benkatz | 22:60276ba87ac6 | 131 | } |
benkatz | 22:60276ba87ac6 | 132 | else{ |
benkatz | 34:47a55f96fbc4 | 133 | TIM1->CCR1 = (PWM_ARR)*(1.0f-dtc_v); |
benkatz | 34:47a55f96fbc4 | 134 | TIM1->CCR2 = (PWM_ARR)*(1.0f-dtc_w); |
benkatz | 22:60276ba87ac6 | 135 | } |
benkatz | 22:60276ba87ac6 | 136 | wait_us(100); |
benkatz | 22:60276ba87ac6 | 137 | ps->Sample(); |
benkatz | 22:60276ba87ac6 | 138 | } |
benkatz | 22:60276ba87ac6 | 139 | ps->Sample(); |
benkatz | 22:60276ba87ac6 | 140 | theta_actual = ps->GetMechPosition(); |
benkatz | 22:60276ba87ac6 | 141 | error_f[i] = theta_ref/NPP - theta_actual; |
benkatz | 22:60276ba87ac6 | 142 | raw_f[i] = ps->GetRawPosition(); |
benkatz | 22:60276ba87ac6 | 143 | printf("%.4f %.4f %d\n\r", theta_ref/(NPP), theta_actual, raw_f[i]); |
benkatz | 22:60276ba87ac6 | 144 | //theta_ref += delta; |
benkatz | 22:60276ba87ac6 | 145 | } |
benkatz | 26:2b865c00d7e9 | 146 | |
benkatz | 25:f5741040c4bb | 147 | for(int i = 0; i<n; i++){ // rotate backwards |
benkatz | 22:60276ba87ac6 | 148 | for(int j = 0; j<n2; j++){ |
benkatz | 22:60276ba87ac6 | 149 | theta_ref -= delta; |
benkatz | 25:f5741040c4bb | 150 | abc(theta_ref, v_d, v_q, &v_u, &v_v, &v_w); // inverse dq0 transform on voltages |
benkatz | 34:47a55f96fbc4 | 151 | svm(V_BUS, v_u, v_v, v_w, &dtc_u, &dtc_v, &dtc_w); // space vector modulation |
benkatz | 34:47a55f96fbc4 | 152 | TIM1->CCR3 = (PWM_ARR)*(1.0f-dtc_u); |
benkatz | 24:58c2d7571207 | 153 | if(PHASE_ORDER){ |
benkatz | 34:47a55f96fbc4 | 154 | TIM1->CCR2 = (PWM_ARR)*(1.0f-dtc_v); |
benkatz | 34:47a55f96fbc4 | 155 | TIM1->CCR1 = (PWM_ARR)*(1.0f-dtc_w); |
benkatz | 22:60276ba87ac6 | 156 | } |
benkatz | 22:60276ba87ac6 | 157 | else{ |
benkatz | 34:47a55f96fbc4 | 158 | TIM1->CCR1 = (PWM_ARR)*(1.0f-dtc_v); |
benkatz | 34:47a55f96fbc4 | 159 | TIM1->CCR2 = (PWM_ARR)*(1.0f-dtc_w); |
benkatz | 22:60276ba87ac6 | 160 | } |
benkatz | 22:60276ba87ac6 | 161 | wait_us(100); |
benkatz | 22:60276ba87ac6 | 162 | ps->Sample(); |
benkatz | 22:60276ba87ac6 | 163 | } |
benkatz | 25:f5741040c4bb | 164 | ps->Sample(); // sample position sensor |
benkatz | 25:f5741040c4bb | 165 | theta_actual = ps->GetMechPosition(); // get mechanical position |
benkatz | 22:60276ba87ac6 | 166 | error_b[i] = theta_ref/NPP - theta_actual; |
benkatz | 22:60276ba87ac6 | 167 | raw_b[i] = ps->GetRawPosition(); |
benkatz | 22:60276ba87ac6 | 168 | printf("%.4f %.4f %d\n\r", theta_ref/(NPP), theta_actual, raw_b[i]); |
benkatz | 22:60276ba87ac6 | 169 | //theta_ref -= delta; |
benkatz | 22:60276ba87ac6 | 170 | } |
benkatz | 22:60276ba87ac6 | 171 | |
benkatz | 22:60276ba87ac6 | 172 | float offset = 0; |
benkatz | 22:60276ba87ac6 | 173 | for(int i = 0; i<n; i++){ |
benkatz | 25:f5741040c4bb | 174 | offset += (error_f[i] + error_b[n-1-i])/(2.0f*n); // calclate average position sensor offset |
benkatz | 22:60276ba87ac6 | 175 | } |
benkatz | 25:f5741040c4bb | 176 | offset = fmod(offset*NPP, 2*PI); // convert mechanical angle to electrical angle |
benkatz | 23:2adf23ee0305 | 177 | |
benkatz | 22:60276ba87ac6 | 178 | |
benkatz | 25:f5741040c4bb | 179 | ps->SetElecOffset(offset); // Set position sensor offset |
benkatz | 23:2adf23ee0305 | 180 | __float_reg[0] = offset; |
benkatz | 23:2adf23ee0305 | 181 | E_OFFSET = offset; |
benkatz | 22:60276ba87ac6 | 182 | |
benkatz | 22:60276ba87ac6 | 183 | /// Perform filtering to linearize position sensor eccentricity |
benkatz | 22:60276ba87ac6 | 184 | /// FIR n-sample average, where n = number of samples in one electrical cycle |
benkatz | 22:60276ba87ac6 | 185 | /// This filter has zero gain at electrical frequency and all integer multiples |
benkatz | 25:f5741040c4bb | 186 | /// So cogging effects should be completely filtered out. |
benkatz | 22:60276ba87ac6 | 187 | |
benkatz | 22:60276ba87ac6 | 188 | float error[n] = {0}; |
benkatz | 23:2adf23ee0305 | 189 | const int window = 128; |
benkatz | 22:60276ba87ac6 | 190 | float error_filt[n] = {0}; |
benkatz | 22:60276ba87ac6 | 191 | float mean = 0; |
benkatz | 25:f5741040c4bb | 192 | for (int i = 0; i<n; i++){ //Average the forward and back directions |
benkatz | 22:60276ba87ac6 | 193 | error[i] = 0.5f*(error_f[i] + error_b[n-i-1]); |
benkatz | 22:60276ba87ac6 | 194 | } |
benkatz | 22:60276ba87ac6 | 195 | for (int i = 0; i<n; i++){ |
benkatz | 22:60276ba87ac6 | 196 | for(int j = 0; j<window; j++){ |
benkatz | 25:f5741040c4bb | 197 | int ind = -window/2 + j + i; // Indexes from -window/2 to + window/2 |
benkatz | 22:60276ba87ac6 | 198 | if(ind<0){ |
benkatz | 25:f5741040c4bb | 199 | ind += n;} // Moving average wraps around |
benkatz | 22:60276ba87ac6 | 200 | else if(ind > n-1) { |
benkatz | 22:60276ba87ac6 | 201 | ind -= n;} |
benkatz | 22:60276ba87ac6 | 202 | error_filt[i] += error[ind]/(float)window; |
benkatz | 22:60276ba87ac6 | 203 | } |
benkatz | 34:47a55f96fbc4 | 204 | |
benkatz | 22:60276ba87ac6 | 205 | //printf("%.4f %4f %.4f %.4f\n\r", error[i], error_filt[i], error_f[i], error_b[i]); |
benkatz | 22:60276ba87ac6 | 206 | mean += error_filt[i]/n; |
benkatz | 22:60276ba87ac6 | 207 | } |
benkatz | 25:f5741040c4bb | 208 | int raw_offset = (raw_f[0] + raw_b[n-1])/2; //Insensitive to errors in this direction, so 2 points is plenty |
benkatz | 28:8c7e29f719c5 | 209 | |
benkatz | 28:8c7e29f719c5 | 210 | |
benkatz | 23:2adf23ee0305 | 211 | printf("\n\r Encoder non-linearity compensation table\n\r"); |
benkatz | 34:47a55f96fbc4 | 212 | printf(" Sample Number : Lookup Index : Lookup Value\n\r\n\r"); |
benkatz | 25:f5741040c4bb | 213 | for (int i = 0; i<n_lut; i++){ // build lookup table |
benkatz | 22:60276ba87ac6 | 214 | int ind = (raw_offset>>7) + i; |
benkatz | 22:60276ba87ac6 | 215 | if(ind > (n_lut-1)){ |
benkatz | 22:60276ba87ac6 | 216 | ind -= n_lut; |
benkatz | 22:60276ba87ac6 | 217 | } |
benkatz | 22:60276ba87ac6 | 218 | lut[ind] = (int) ((error_filt[i*NPP] - mean)*(float)(ps->GetCPR())/(2.0f*PI)); |
benkatz | 34:47a55f96fbc4 | 219 | printf("%d %d %d\n\r", i, ind, lut[ind]); |
benkatz | 26:2b865c00d7e9 | 220 | wait(.001); |
benkatz | 22:60276ba87ac6 | 221 | } |
benkatz | 23:2adf23ee0305 | 222 | |
benkatz | 25:f5741040c4bb | 223 | ps->WriteLUT(lut); // write lookup table to position sensor object |
benkatz | 25:f5741040c4bb | 224 | memcpy(&ENCODER_LUT, lut, sizeof(lut)); // copy the lookup table to the flash array |
benkatz | 23:2adf23ee0305 | 225 | printf("\n\rEncoder Electrical Offset (rad) %f\n\r", offset); |
benkatz | 22:60276ba87ac6 | 226 | |
benkatz | 23:2adf23ee0305 | 227 | if (!prefs->ready()) prefs->open(); |
benkatz | 25:f5741040c4bb | 228 | prefs->flush(); // write offset and lookup table to flash |
benkatz | 25:f5741040c4bb | 229 | prefs->close(); |
benkatz | 23:2adf23ee0305 | 230 | |
benkatz | 23:2adf23ee0305 | 231 | |
benkatz | 22:60276ba87ac6 | 232 | } |