Details | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
2189 | - | 1 | /* |
2 | APM_AHRS_DCM.cpp |
||
3 | |||
4 | AHRS system using DCM matrices |
||
5 | |||
6 | Based on DCM code by Doug Weibel, Jordi Mu�oz and Jose Julio. DIYDrones.com |
||
7 | |||
8 | Adapted for the general ArduPilot AHRS interface by Andrew Tridgell |
||
9 | |||
10 | This library is free software; you can redistribute it and/or |
||
11 | modify it under the terms of the GNU Lesser General Public License |
||
12 | as published by the Free Software Foundation; either version 2.1 |
||
13 | of the License, or (at your option) any later version. |
||
14 | */ |
||
15 | #include "AP_AHRS.h" |
||
16 | #include "output.h" |
||
17 | #include "profiler.h" |
||
18 | #include "analog.h" |
||
19 | #include "debug.h" |
||
20 | #include <stdio.h> |
||
21 | |||
22 | // this is the speed in cm/s above which we first get a yaw lock with |
||
23 | // the GPS |
||
24 | #define GPS_SPEED_MIN 300 |
||
25 | |||
26 | // this is the speed in cm/s at which we stop using drift correction |
||
27 | // from the GPS and wait for the ground speed to get above GPS_SPEED_MIN |
||
28 | #define GPS_SPEED_RESET 100 |
||
29 | |||
30 | // run a full DCM update round |
||
31 | void |
||
32 | AP_AHRS_DCM::update(int16_t attitude[3], float delta_t) |
||
33 | { |
||
34 | // Get current values for gyros |
||
35 | _gyro_vector = gyro_attitude; |
||
36 | _accel_vector = accel; |
||
37 | |||
38 | // Integrate the DCM matrix using gyro inputs |
||
39 | matrix_update(delta_t); |
||
40 | |||
41 | // Normalize the DCM matrix |
||
42 | normalize(); |
||
43 | |||
44 | // Perform drift correction |
||
45 | //setCurrentProfiledActivity(DRIFT_CORRECTION); |
||
46 | drift_correction(delta_t); |
||
47 | |||
48 | // paranoid check for bad values in the DCM matrix |
||
49 | //setCurrentProfiledActivity(CHECK_MATRIX); |
||
50 | check_matrix(); |
||
51 | |||
52 | // Calculate pitch, roll, yaw for stabilization and navigation |
||
53 | //setCurrentProfiledActivity(EULER_ANGLES); |
||
54 | euler_angles(); |
||
55 | |||
56 | //setCurrentProfiledActivity(ANGLESOUTPUT); |
||
57 | attitude[0] = roll * INT16DEG_PI_FACTOR; |
||
58 | attitude[1] = pitch* INT16DEG_PI_FACTOR; |
||
59 | attitude[2] = yaw * INT16DEG_PI_FACTOR; |
||
60 | |||
61 | // Just for info: |
||
62 | int16_t sensor = degrees(roll) * 10; |
||
63 | debugOut.analog[0] = sensor; |
||
64 | |||
65 | sensor = degrees(pitch) * 10; |
||
66 | debugOut.analog[1] = sensor; |
||
67 | |||
68 | sensor = degrees(yaw) * 10; |
||
69 | if (sensor < 0) |
||
70 | sensor += 3600; |
||
71 | debugOut.analog[2] = sensor; |
||
72 | } |
||
73 | |||
74 | // update the DCM matrix using only the gyros |
||
75 | void |
||
76 | AP_AHRS_DCM::matrix_update(float _G_Dt) |
||
77 | { |
||
78 | //setCurrentProfiledActivity(MATRIX_UPDATE1); |
||
79 | |||
80 | // _omega_integ_corr is used for _centripetal correction |
||
81 | // (theoretically better than _omega) |
||
82 | _omega_integ_corr = _gyro_vector + _omega_I; |
||
83 | |||
84 | // Equation 16, adding proportional and integral correction terms |
||
85 | _omega = _omega_integ_corr + _omega_P + _omega_yaw_P; |
||
86 | |||
87 | // this is a replacement of the DCM matrix multiply (equation |
||
88 | // 17), with known zero elements removed and the matrix |
||
89 | // operations inlined. This runs much faster than the original |
||
90 | // version of this code, as the compiler was doing a terrible |
||
91 | // job of realising that so many of the factors were in common |
||
92 | // or zero. It also uses much less stack, as we no longer need |
||
93 | // two additional local matrices |
||
94 | |||
95 | Vector3f r = _omega * _G_Dt; |
||
96 | |||
97 | //setCurrentProfiledActivity(MATRIX_UPDATE2); |
||
98 | |||
99 | _dcm_matrix.rotate(r); |
||
100 | } |
||
101 | |||
102 | |||
103 | // adjust an accelerometer vector for known acceleration forces |
||
104 | void |
||
105 | AP_AHRS_DCM::accel_adjust(Vector3f &accel) |
||
106 | { |
||
107 | float veloc; |
||
108 | // compensate for linear acceleration. This makes a |
||
109 | // surprisingly large difference in the pitch estimate when |
||
110 | // turning, plus on takeoff and landing |
||
111 | //float acceleration = _gps->acceleration(); |
||
112 | //accel.x -= acceleration; |
||
113 | |||
114 | // compensate for centripetal acceleration |
||
115 | veloc = 0; //_gps->ground_speed * 0.01; |
||
116 | |||
117 | // We are working with a modified version of equation 26 as |
||
118 | // our IMU object reports acceleration in the positive axis |
||
119 | // direction as positive |
||
120 | |||
121 | // Equation 26 broken up into separate pieces |
||
122 | accel.y -= _omega_integ_corr.z * veloc; |
||
123 | accel.z += _omega_integ_corr.y * veloc; |
||
124 | } |
||
125 | |||
126 | /* |
||
127 | reset the DCM matrix and omega. Used on ground start, and on |
||
128 | extreme errors in the matrix |
||
129 | */ |
||
130 | void |
||
131 | AP_AHRS_DCM::reset(bool recover_eulers) |
||
132 | { |
||
133 | if (_compass != NULL) { |
||
134 | _compass->null_offsets_disable(); |
||
135 | } |
||
136 | |||
137 | // reset the integration terms |
||
138 | _omega_I.zero(); |
||
139 | _omega_P.zero(); |
||
140 | _omega_yaw_P.zero(); |
||
141 | _omega_integ_corr.zero(); |
||
142 | _omega.zero(); |
||
143 | |||
144 | // if the caller wants us to try to recover to the current |
||
145 | // attitude then calculate the dcm matrix from the current |
||
146 | // roll/pitch/yaw values |
||
147 | if (recover_eulers && !isnan(roll) && !isnan(pitch) && !isnan(yaw)) { |
||
148 | _dcm_matrix.from_euler(roll, pitch, yaw); |
||
149 | } else { |
||
150 | // otherwise make it flat |
||
151 | _dcm_matrix.from_euler(0, 0, 0); |
||
152 | } |
||
153 | |||
154 | if (_compass != NULL) { |
||
155 | _compass->null_offsets_enable(); // This call is needed to restart the nulling |
||
156 | // Otherwise the reset in the DCM matrix can mess up |
||
157 | // the nulling |
||
158 | } |
||
159 | } |
||
160 | |||
161 | /* |
||
162 | check the DCM matrix for pathological values |
||
163 | */ |
||
164 | void |
||
165 | AP_AHRS_DCM::check_matrix(void) |
||
166 | { |
||
167 | if (_dcm_matrix.is_nan()) { |
||
168 | //Serial.printf("ERROR: DCM matrix NAN\n"); |
||
169 | printf("ERROR: DCM matrix NAN\n"); |
||
170 | renorm_blowup_count++; |
||
171 | reset(true); |
||
172 | return; |
||
173 | } |
||
174 | // some DCM matrix values can lead to an out of range error in |
||
175 | // the pitch calculation via asin(). These NaN values can |
||
176 | // feed back into the rest of the DCM matrix via the |
||
177 | // error_course value. |
||
178 | if (!(_dcm_matrix.c.x < 1.0 && |
||
179 | _dcm_matrix.c.x > -1.0)) { |
||
180 | // We have an invalid matrix. Force a normalisation. |
||
181 | renorm_range_count++; |
||
182 | normalize(); |
||
183 | |||
184 | if (_dcm_matrix.is_nan() || |
||
185 | fabs(_dcm_matrix.c.x) > 10) { |
||
186 | // normalisation didn't fix the problem! We're |
||
187 | // in real trouble. All we can do is reset |
||
188 | //Serial.printf("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n", |
||
189 | // _dcm_matrix.c.x); |
||
190 | printf("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n", (double)_dcm_matrix.c.x); |
||
191 | renorm_blowup_count++; |
||
192 | reset(true); |
||
193 | } |
||
194 | } |
||
195 | } |
||
196 | |||
197 | // renormalise one vector component of the DCM matrix |
||
198 | // this will return false if renormalization fails |
||
199 | bool |
||
200 | AP_AHRS_DCM::renorm(Vector3f const &a, Vector3f &result) |
||
201 | { |
||
202 | float renorm_val; |
||
203 | |||
204 | // numerical errors will slowly build up over time in DCM, |
||
205 | // causing inaccuracies. We can keep ahead of those errors |
||
206 | // using the renormalization technique from the DCM IMU paper |
||
207 | // (see equations 18 to 21). |
||
208 | |||
209 | // For APM we don't bother with the taylor expansion |
||
210 | // optimisation from the paper as on our 2560 CPU the cost of |
||
211 | // the sqrt() is 44 microseconds, and the small time saving of |
||
212 | // the taylor expansion is not worth the potential of |
||
213 | // additional error buildup. |
||
214 | |||
215 | // Note that we can get significant renormalisation values |
||
216 | // when we have a larger delta_t due to a glitch eleswhere in |
||
217 | // APM, such as a I2c timeout or a set of EEPROM writes. While |
||
218 | // we would like to avoid these if possible, if it does happen |
||
219 | // we don't want to compound the error by making DCM less |
||
220 | // accurate. |
||
221 | |||
222 | renorm_val = 1.0 / a.length(); |
||
223 | |||
224 | // keep the average for reporting |
||
225 | _renorm_val_sum += renorm_val; |
||
226 | _renorm_val_count++; |
||
227 | |||
228 | if (!(renorm_val < 2.0 && renorm_val > 0.5)) { |
||
229 | // this is larger than it should get - log it as a warning |
||
230 | renorm_range_count++; |
||
231 | if (!(renorm_val < 1.0e6 && renorm_val > 1.0e-6)) { |
||
232 | // we are getting values which are way out of |
||
233 | // range, we will reset the matrix and hope we |
||
234 | // can recover our attitude using drift |
||
235 | // correction before we hit the ground! |
||
236 | //Serial.printf("ERROR: DCM renormalisation error. renorm_val=%f\n", |
||
237 | // renorm_val); |
||
238 | printf("ERROR: DCM renormalisation error. renorm_val=%f\n", (double)renorm_val); |
||
239 | renorm_blowup_count++; |
||
240 | return false; |
||
241 | } |
||
242 | } |
||
243 | |||
244 | result = a * renorm_val; |
||
245 | return true; |
||
246 | } |
||
247 | |||
248 | /************************************************* |
||
249 | Direction Cosine Matrix IMU: Theory |
||
250 | William Premerlani and Paul Bizard |
||
251 | |||
252 | Numerical errors will gradually reduce the orthogonality conditions expressed by equation 5 |
||
253 | to approximations rather than identities. In effect, the axes in the two frames of reference no |
||
254 | longer describe a rigid body. Fortunately, numerical error accumulates very slowly, so it is a |
||
255 | simple matter to stay ahead of it. |
||
256 | We call the process of enforcing the orthogonality conditions �renormalization�. |
||
257 | */ |
||
258 | void |
||
259 | AP_AHRS_DCM::normalize(void) |
||
260 | { |
||
261 | float error; |
||
262 | Vector3f t0, t1, t2; |
||
263 | |||
264 | //setCurrentProfiledActivity(MATRIX_NORMALIZE1); |
||
265 | |||
266 | error = _dcm_matrix.a * _dcm_matrix.b; // eq.18 |
||
267 | t0 = _dcm_matrix.a - (_dcm_matrix.b * (0.5f * error)); // eq.19 |
||
268 | t1 = _dcm_matrix.b - (_dcm_matrix.a * (0.5f * error)); // eq.19 |
||
269 | t2 = t0 % t1; |
||
270 | |||
271 | //setCurrentProfiledActivity(MATRIX_NORMALIZE2); |
||
272 | if (!renorm(t0, _dcm_matrix.a) || |
||
273 | !renorm(t1, _dcm_matrix.b) || |
||
274 | !renorm(t2, _dcm_matrix.c)) { |
||
275 | // Our solution is blowing up and we will force back |
||
276 | // to last euler angles |
||
277 | reset(true); |
||
278 | } |
||
279 | } |
||
280 | |||
281 | |||
282 | // perform drift correction. This function aims to update _omega_P and |
||
283 | // _omega_I with our best estimate of the short term and long term |
||
284 | // gyro error. The _omega_P value is what pulls our attitude solution |
||
285 | // back towards the reference vector quickly. The _omega_I term is an |
||
286 | // attempt to learn the long term drift rate of the gyros. |
||
287 | // |
||
288 | // This function also updates _omega_yaw_P with a yaw correction term |
||
289 | // from our yaw reference vector |
||
290 | void |
||
291 | AP_AHRS_DCM::drift_correction(float deltat) |
||
292 | { |
||
293 | float error_course = 0; |
||
294 | Vector3f accel; |
||
295 | Vector3f error; |
||
296 | float error_norm = 0; |
||
297 | float yaw_deltat = 0; |
||
298 | |||
299 | accel = _accel_vector; |
||
300 | |||
301 | // if enabled, use the GPS to correct our accelerometer vector |
||
302 | // for centripetal forces |
||
303 | if(_centripetal && |
||
304 | _gps != NULL && |
||
305 | _gps->status() == GPS::GPS_OK) { |
||
306 | accel_adjust(accel); |
||
307 | } |
||
308 | |||
309 | |||
310 | //*****Roll and Pitch*************** |
||
311 | |||
312 | // normalise the accelerometer vector to a standard length |
||
313 | // this is important to reduce the impact of noise on the |
||
314 | // drift correction, as very noisy vectors tend to have |
||
315 | // abnormally high lengths. By normalising the length we |
||
316 | // reduce their impact. |
||
317 | float accel_length = accel.length(); |
||
318 | |||
319 | accel *= (_gravity / accel_length); |
||
320 | if (accel.is_inf()) { |
||
321 | // we can't do anything useful with this sample |
||
322 | _omega_P.zero(); |
||
323 | return; |
||
324 | } |
||
325 | |||
326 | // calculate the error, in m/s^2, between the attitude |
||
327 | // implied by the accelerometers and the attitude |
||
328 | // in the current DCM matrix |
||
329 | error = _dcm_matrix.c % accel; |
||
330 | |||
331 | // Limit max error to limit the effect of noisy values |
||
332 | // on the algorithm. This limits the error to about 11 |
||
333 | // degrees |
||
334 | error_norm = error.length(); |
||
335 | |||
336 | if (error_norm > 2) { |
||
337 | error *= (2 / error_norm); |
||
338 | } |
||
339 | |||
340 | // we now want to calculate _omega_P and _omega_I. The |
||
341 | // _omega_P value is what drags us quickly to the |
||
342 | // accelerometer reading. |
||
343 | _omega_P = error * _kp_roll_pitch; |
||
344 | |||
345 | // the _omega_I is the long term accumulated gyro |
||
346 | // error. This determines how much gyro drift we can |
||
347 | // handle. |
||
348 | _omega_I_sum += error * (_ki_roll_pitch * deltat); |
||
349 | _omega_I_sum_time += deltat; |
||
350 | |||
351 | // these sums support the reporting of the DCM state via MAVLink |
||
352 | _error_rp_sum += error_norm; |
||
353 | _error_rp_count++; |
||
354 | |||
355 | // yaw drift correction |
||
356 | |||
357 | // we only do yaw drift correction when we get a new yaw |
||
358 | // reference vector. In between times we rely on the gyros for |
||
359 | // yaw. Avoiding this calculation on every call to |
||
360 | // update_DCM() saves a lot of time |
||
361 | if (_compass && _compass->use_for_yaw()) { |
||
362 | if (_compass->last_update != _compass_last_update) { |
||
363 | yaw_deltat = 1.0e-6*(_compass->last_update - _compass_last_update); |
||
364 | if (_have_initial_yaw && yaw_deltat < 2.0) { |
||
365 | // Equation 23, Calculating YAW error |
||
366 | // We make the gyro YAW drift correction based |
||
367 | // on compass magnetic heading |
||
368 | error_course = (_dcm_matrix.a.x * _compass->heading_y) - (_dcm_matrix.b.x * _compass->heading_x); |
||
369 | _compass_last_update = _compass->last_update; |
||
370 | } else { |
||
371 | // this is our first estimate of the yaw, |
||
372 | // or the compass has come back online after |
||
373 | // no readings for 2 seconds. |
||
374 | // |
||
375 | // construct a DCM matrix based on the current |
||
376 | // roll/pitch and the compass heading. |
||
377 | // First ensure the compass heading has been |
||
378 | // calculated |
||
379 | _compass->calculate(_dcm_matrix); |
||
380 | |||
381 | // now construct a new DCM matrix |
||
382 | _compass->null_offsets_disable(); |
||
383 | _dcm_matrix.from_euler(roll, pitch, _compass->heading); |
||
384 | _compass->null_offsets_enable(); |
||
385 | _have_initial_yaw = true; |
||
386 | _compass_last_update = _compass->last_update; |
||
387 | error_course = 0; |
||
388 | } |
||
389 | } |
||
390 | } else if (_gps && _gps->status() == GPS::GPS_OK) { |
||
391 | if (_gps->last_fix_time != _gps_last_update) { |
||
392 | // Use GPS Ground course to correct yaw gyro drift |
||
393 | if (_gps->ground_speed >= GPS_SPEED_MIN) { |
||
394 | yaw_deltat = 1.0e-3*(_gps->last_fix_time - _gps_last_update); |
||
395 | if (_have_initial_yaw && yaw_deltat < 2.0) { |
||
396 | float course_over_ground_x = cos(ToRad(_gps->ground_course/100.0)); |
||
397 | float course_over_ground_y = sin(ToRad(_gps->ground_course/100.0)); |
||
398 | // Equation 23, Calculating YAW error |
||
399 | error_course = (_dcm_matrix.a.x * course_over_ground_y) - (_dcm_matrix.b.x * course_over_ground_x); |
||
400 | _gps_last_update = _gps->last_fix_time; |
||
401 | } else { |
||
402 | // when we first start moving, set the |
||
403 | // DCM matrix to the current |
||
404 | // roll/pitch values, but with yaw |
||
405 | // from the GPS |
||
406 | if (_compass) { |
||
407 | _compass->null_offsets_disable(); |
||
408 | } |
||
409 | _dcm_matrix.from_euler(roll, pitch, ToRad(_gps->ground_course)); |
||
410 | if (_compass) { |
||
411 | _compass->null_offsets_enable(); |
||
412 | } |
||
413 | _have_initial_yaw = true; |
||
414 | error_course = 0; |
||
415 | _gps_last_update = _gps->last_fix_time; |
||
416 | } |
||
417 | } else if (_gps->ground_speed >= GPS_SPEED_RESET) { |
||
418 | // we are not going fast enough to use GPS for |
||
419 | // course correction, but we won't reset |
||
420 | // _have_initial_yaw yet, instead we just let |
||
421 | // the gyro handle yaw |
||
422 | error_course = 0; |
||
423 | } else { |
||
424 | // we are moving very slowly. Reset |
||
425 | // _have_initial_yaw and adjust our heading |
||
426 | // rapidly next time we get a good GPS ground |
||
427 | // speed |
||
428 | error_course = 0; |
||
429 | _have_initial_yaw = false; |
||
430 | } |
||
431 | } |
||
432 | } |
||
433 | |||
434 | // see if there is any error in our heading relative to the |
||
435 | // yaw reference. This will be zero most of the time, as we |
||
436 | // only calculate it when we get new data from the yaw |
||
437 | // reference source |
||
438 | if (yaw_deltat == 0 || error_course == 0) { |
||
439 | // we don't have a new reference heading. Slowly |
||
440 | // decay the _omega_yaw_P to ensure that if we have |
||
441 | // lost the yaw reference sensor completely we don't |
||
442 | // keep using a stale offset |
||
443 | _omega_yaw_P *= 0.97; |
||
444 | goto check_sum_time; |
||
445 | } |
||
446 | |||
447 | // ensure the course error is scaled from -PI to PI |
||
448 | if (error_course > PI) { |
||
449 | error_course -= 2*PI; |
||
450 | } else if (error_course < -PI) { |
||
451 | error_course += 2*PI; |
||
452 | } |
||
453 | |||
454 | // Equation 24, Applys the yaw correction to the XYZ rotation of the aircraft |
||
455 | // this gives us an error in radians |
||
456 | error = _dcm_matrix.c * error_course; |
||
457 | |||
458 | // Adding yaw correction to proportional correction vector. We |
||
459 | // allow the yaw reference source to affect all 3 components |
||
460 | // of _omega_yaw_P as we need to be able to correctly hold a |
||
461 | // heading when roll and pitch are non-zero |
||
462 | _omega_yaw_P = error * _kp_yaw; |
||
463 | |||
464 | // add yaw correction to integrator correction vector, but |
||
465 | // only for the z gyro. We rely on the accelerometers for x |
||
466 | // and y gyro drift correction. Using the compass or GPS for |
||
467 | // x/y drift correction is too inaccurate, and can lead to |
||
468 | // incorrect builups in the x/y drift. We rely on the |
||
469 | // accelerometers to get the x/y components right |
||
470 | _omega_I_sum.z += error.z * (_ki_yaw * yaw_deltat); |
||
471 | |||
472 | // we keep the sum of yaw error for reporting via MAVLink. |
||
473 | _error_yaw_sum += error_course; |
||
474 | _error_yaw_count++; |
||
475 | |||
476 | check_sum_time: |
||
477 | if (_omega_I_sum_time > 10) { |
||
478 | // every 10 seconds we apply the accumulated |
||
479 | // _omega_I_sum changes to _omega_I. We do this to |
||
480 | // prevent short term feedback between the P and I |
||
481 | // terms of the controller. The _omega_I vector is |
||
482 | // supposed to refect long term gyro drift, but with |
||
483 | // high noise it can start to build up due to short |
||
484 | // term interactions. By summing it over 10 seconds we |
||
485 | // prevent the short term interactions and can apply |
||
486 | // the slope limit more accurately |
||
487 | float drift_limit = _gyro_drift_limit * _omega_I_sum_time; |
||
488 | _omega_I_sum.x = constrain(_omega_I_sum.x, -drift_limit, drift_limit); |
||
489 | _omega_I_sum.y = constrain(_omega_I_sum.y, -drift_limit, drift_limit); |
||
490 | _omega_I_sum.z = constrain(_omega_I_sum.z, -drift_limit, drift_limit); |
||
491 | |||
492 | _omega_I += _omega_I_sum; |
||
493 | |||
494 | // zero the sum |
||
495 | _omega_I_sum.zero(); |
||
496 | _omega_I_sum_time = 0; |
||
497 | } |
||
498 | } |
||
499 | |||
500 | |||
501 | // calculate the euler angles which will be used for high level |
||
502 | // navigation control |
||
503 | void |
||
504 | AP_AHRS_DCM::euler_angles(void) |
||
505 | { |
||
506 | _dcm_matrix.to_euler(&roll, &pitch, &yaw); |
||
507 | } |
||
508 | |||
509 | /* reporting of DCM state for MAVLink */ |
||
510 | |||
511 | // average error_roll_pitch since last call |
||
512 | float AP_AHRS_DCM::get_error_rp(void) |
||
513 | { |
||
514 | if (_error_rp_count == 0) { |
||
515 | // this happens when telemetry is setup on two |
||
516 | // serial ports |
||
517 | return _error_rp_last; |
||
518 | } |
||
519 | _error_rp_last = _error_rp_sum / _error_rp_count; |
||
520 | _error_rp_sum = 0; |
||
521 | _error_rp_count = 0; |
||
522 | return _error_rp_last; |
||
523 | } |
||
524 | |||
525 | // average error_yaw since last call |
||
526 | float AP_AHRS_DCM::get_error_yaw(void) |
||
527 | { |
||
528 | if (_error_yaw_count == 0) { |
||
529 | // this happens when telemetry is setup on two |
||
530 | // serial ports |
||
531 | return _error_yaw_last; |
||
532 | } |
||
533 | _error_yaw_last = _error_yaw_sum / _error_yaw_count; |
||
534 | _error_yaw_sum = 0; |
||
535 | _error_yaw_count = 0; |
||
536 | return _error_yaw_last; |
||
537 | } |