PX4 Firmware
PX4 Autopilot Software http://px4.io
df_mpu6050_wrapper.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2016 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file df_mpu6050_wrapper.cpp
36  * Lightweight driver to access the MPU6050 of the DriverFramework.
37  */
38 
39 #include <px4_platform_common/px4_config.h>
40 
41 #include <sys/types.h>
42 #include <sys/stat.h>
43 #include <stdint.h>
44 #include <stddef.h>
45 #include <stdlib.h>
46 #include <string.h>
47 #include <math.h>
48 #include <unistd.h>
49 #include <px4_platform_common/getopt.h>
50 #include <errno.h>
51 
52 #include <systemlib/err.h>
53 #include <lib/parameters/param.h>
54 #include <perf/perf_counter.h>
55 #include <systemlib/mavlink_log.h>
56 
57 #include <drivers/drv_hrt.h>
58 #include <drivers/drv_accel.h>
59 #include <drivers/drv_gyro.h>
61 
63 
65 
66 #include <mpu6050/MPU6050.hpp>
67 #include <DevMgr.hpp>
68 
69 // We don't want to auto publish, therefore set this to 0.
70 #define MPU6050_NEVER_AUTOPUBLISH_US 0
71 
72 
73 extern "C" { __EXPORT int df_mpu6050_wrapper_main(int argc, char *argv[]); }
74 
75 using namespace DriverFramework;
76 
77 
78 class DfMPU6050Wrapper : public MPU6050
79 {
80 public:
81  DfMPU6050Wrapper(enum Rotation rotation);
83 
84 
85  /**
86  * Start automatic measurement.
87  *
88  * @return 0 on success
89  */
90  int start();
91 
92  /**
93  * Stop automatic measurement.
94  *
95  * @return 0 on success
96  */
97  int stop();
98 
99  /**
100  * Print some debug info.
101  */
102  void info();
103 
104 private:
105  int _publish(struct imu_sensor_data &data);
106 
107  void _update_accel_calibration();
108  void _update_gyro_calibration();
109 
112 
114 
116 
118  float x_offset;
119  float x_scale;
120  float y_offset;
121  float y_scale;
122  float z_offset;
123  float z_scale;
124  } _accel_calibration;
125 
127  float x_offset;
128  float x_scale;
129  float y_offset;
130  float y_scale;
131  float z_offset;
132  float z_scale;
133  } _gyro_calibration;
134 
136 
139 
142 
143  unsigned _publish_count;
144 
152 
155 
156 };
157 
159  MPU6050(IMU_DEVICE_PATH),
160  _accel_topic(nullptr),
161  _gyro_topic(nullptr),
162  _mavlink_log_pub(nullptr),
163  _param_update_sub(-1),
164  _accel_calibration{},
170  _publish_count(0),
171  _read_counter(perf_alloc(PC_COUNT, "mpu6050_reads")),
172  _error_counter(perf_alloc(PC_COUNT, "mpu6050_errors")),
173  _fifo_overflow_counter(perf_alloc(PC_COUNT, "mpu6050_fifo_overflows")),
174  _fifo_corruption_counter(perf_alloc(PC_COUNT, "mpu6050_fifo_corruptions")),
175  _gyro_range_hit_counter(perf_alloc(PC_COUNT, "mpu6050_gyro_range_hits")),
176  _accel_range_hit_counter(perf_alloc(PC_COUNT, "mpu6050_accel_range_hits")),
177  _publish_perf(perf_alloc(PC_ELAPSED, "mpu6050_publish")),
180 {
181  // Set sane default calibration values
188 
189  _gyro_calibration.x_scale = 1.0f;
190  _gyro_calibration.y_scale = 1.0f;
191  _gyro_calibration.z_scale = 1.0f;
195 
196  // Get sensor rotation matrix
197  _rotation_matrix = get_rot_matrix(rotation);
198 }
199 
201 {
208 
210 }
211 
213 {
214  /* Subscribe to param update topic. */
215  if (_param_update_sub < 0) {
216  _param_update_sub = orb_subscribe(ORB_ID(parameter_update));
217  }
218 
219  /* Init device and start sensor. */
220  int ret = init();
221 
222  if (ret != 0) {
223  PX4_ERR("MPU6050 init fail: %d", ret);
224  return ret;
225  }
226 
227  ret = MPU6050::start();
228 
229  if (ret != 0) {
230  PX4_ERR("MPU6050 start fail: %d", ret);
231  return ret;
232  }
233 
234  PX4_DEBUG("MPU6050 device id is: %d", m_id.dev_id);
235 
236  /* Force getting the calibration values. */
239 
240  return 0;
241 }
242 
244 {
245  /* Stop sensor. */
246  int ret = MPU6050::stop();
247 
248  if (ret != 0) {
249  PX4_ERR("MPU6050 stop fail: %d", ret);
250  return ret;
251  }
252 
253  return 0;
254 }
255 
257 {
264 
266 }
267 
269 {
270  // TODO: replace magic number
271  for (unsigned i = 0; i < 3; ++i) {
272 
273  // TODO: remove printfs and add error counter
274 
275  char str[30];
276  (void)sprintf(str, "CAL_GYRO%u_ID", i);
277  int32_t device_id;
278  int res = param_get(param_find(str), &device_id);
279 
280  if (res != OK) {
281  PX4_ERR("Could not access param %s", str);
282  continue;
283  }
284 
285  if ((uint32_t)device_id != m_id.dev_id) {
286  continue;
287  }
288 
289  (void)sprintf(str, "CAL_GYRO%u_XSCALE", i);
291 
292  if (res != OK) {
293  PX4_ERR("Could not access param %s", str);
294  }
295 
296  (void)sprintf(str, "CAL_GYRO%u_YSCALE", i);
298 
299  if (res != OK) {
300  PX4_ERR("Could not access param %s", str);
301  }
302 
303  (void)sprintf(str, "CAL_GYRO%u_ZSCALE", i);
305 
306  if (res != OK) {
307  PX4_ERR("Could not access param %s", str);
308  }
309 
310  (void)sprintf(str, "CAL_GYRO%u_XOFF", i);
312 
313  if (res != OK) {
314  PX4_ERR("Could not access param %s", str);
315  }
316 
317  (void)sprintf(str, "CAL_GYRO%u_YOFF", i);
319 
320  if (res != OK) {
321  PX4_ERR("Could not access param %s", str);
322  }
323 
324  (void)sprintf(str, "CAL_GYRO%u_ZOFF", i);
326 
327  if (res != OK) {
328  PX4_ERR("Could not access param %s", str);
329  }
330 
331  // We got calibration values, let's exit.
332  return;
333  }
334 
335  _gyro_calibration.x_scale = 1.0f;
336  _gyro_calibration.y_scale = 1.0f;
337  _gyro_calibration.z_scale = 1.0f;
341 }
342 
344 {
345  // TODO: replace magic number
346  for (unsigned i = 0; i < 3; ++i) {
347 
348  // TODO: remove printfs and add error counter
349 
350  char str[30];
351  (void)sprintf(str, "CAL_ACC%u_ID", i);
352  int32_t device_id;
353  int res = param_get(param_find(str), &device_id);
354 
355  if (res != OK) {
356  PX4_ERR("Could not access param %s", str);
357  continue;
358  }
359 
360  if ((uint32_t)device_id != m_id.dev_id) {
361  continue;
362  }
363 
364  (void)sprintf(str, "CAL_ACC%u_XSCALE", i);
366 
367  if (res != OK) {
368  PX4_ERR("Could not access param %s", str);
369  }
370 
371  (void)sprintf(str, "CAL_ACC%u_YSCALE", i);
373 
374  if (res != OK) {
375  PX4_ERR("Could not access param %s", str);
376  }
377 
378  (void)sprintf(str, "CAL_ACC%u_ZSCALE", i);
380 
381  if (res != OK) {
382  PX4_ERR("Could not access param %s", str);
383  }
384 
385  (void)sprintf(str, "CAL_ACC%u_XOFF", i);
387 
388  if (res != OK) {
389  PX4_ERR("Could not access param %s", str);
390  }
391 
392  (void)sprintf(str, "CAL_ACC%u_YOFF", i);
394 
395  if (res != OK) {
396  PX4_ERR("Could not access param %s", str);
397  }
398 
399  (void)sprintf(str, "CAL_ACC%u_ZOFF", i);
401 
402  if (res != OK) {
403  PX4_ERR("Could not access param %s", str);
404  }
405 
406  // We got calibration values, let's exit.
407  return;
408  }
409 
410  // Set sane default calibration values
417 }
418 
419 int DfMPU6050Wrapper::_publish(struct imu_sensor_data &data)
420 {
421  /* Check if calibration values are still up-to-date. */
422  bool updated;
423  orb_check(_param_update_sub, &updated);
424 
425  if (updated) {
426  parameter_update_s parameter_update;
427  orb_copy(ORB_ID(parameter_update), _param_update_sub, &parameter_update);
428 
431  }
432 
433  uint64_t now = hrt_absolute_time();
434 
435  matrix::Vector3f vec_integrated_unused;
436  uint32_t integral_dt_unused;
437 
438  matrix::Vector3f accel_val(data.accel_m_s2_x, data.accel_m_s2_y, data.accel_m_s2_z);
439 
440  // apply sensor rotation on the accel measurement
441  accel_val = _rotation_matrix * accel_val;
442 
443  accel_val(0) = (accel_val(0) - _accel_calibration.x_offset) * _accel_calibration.x_scale;
444  accel_val(1) = (accel_val(1) - _accel_calibration.y_offset) * _accel_calibration.y_scale;
445  accel_val(2) = (accel_val(2) - _accel_calibration.z_offset) * _accel_calibration.z_scale;
446 
447  _accel_int.put(now,
448  accel_val,
449  vec_integrated_unused,
450  integral_dt_unused);
451 
452  matrix::Vector3f gyro_val(data.gyro_rad_s_x, data.gyro_rad_s_y, data.gyro_rad_s_z);
453 
454  // apply sensor rotation on the gyro measurement
455  gyro_val = _rotation_matrix * gyro_val;
456 
457  gyro_val(0) = (gyro_val(0) - _gyro_calibration.x_offset) * _gyro_calibration.x_scale;
458  gyro_val(1) = (gyro_val(1) - _gyro_calibration.y_offset) * _gyro_calibration.y_scale;
459  gyro_val(2) = (gyro_val(2) - _gyro_calibration.z_offset) * _gyro_calibration.z_scale;
460 
461  _gyro_int.put(now,
462  gyro_val,
463  vec_integrated_unused,
464  integral_dt_unused);
465 
466  // If we are not receiving the last sample from the FIFO buffer yet, let's stop here
467  // and wait for more packets.
468  if (!data.is_last_fifo_sample) {
469  return 0;
470  }
471 
472  // The driver empties the FIFO buffer at 1kHz, however we only need to publish at 250Hz.
473  // Therefore, only publish every forth time.
474  ++_publish_count;
475 
476  if (_publish_count < 4) {
477  return 0;
478  }
479 
480  _publish_count = 0;
481 
482  // Update all the counters.
483  perf_set_count(_read_counter, data.read_counter);
484  perf_set_count(_error_counter, data.error_counter);
485  perf_set_count(_fifo_overflow_counter, data.fifo_overflow_counter);
486  perf_set_count(_fifo_corruption_counter, data.fifo_overflow_counter);
487  perf_set_count(_gyro_range_hit_counter, data.gyro_range_hit_counter);
488  perf_set_count(_accel_range_hit_counter, data.accel_range_hit_counter);
489 
491 
492  sensor_accel_s accel_report = {};
493  sensor_gyro_s gyro_report = {};
494 
495  accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time();
496 
497  // TODO: get these right
498  gyro_report.scaling = -1.0f;
499  gyro_report.device_id = m_id.dev_id;
500 
501  accel_report.scaling = -1.0f;
502  accel_report.device_id = m_id.dev_id;
503 
504  // TODO: remove these (or get the values)
505  gyro_report.x_raw = 0;
506  gyro_report.y_raw = 0;
507  gyro_report.z_raw = 0;
508 
509  accel_report.x_raw = 0;
510  accel_report.y_raw = 0;
511  accel_report.z_raw = 0;
512 
513  matrix::Vector3f gyro_val_filt;
514  matrix::Vector3f accel_val_filt;
515 
516  // Read and reset.
517  matrix::Vector3f gyro_val_integ = _gyro_int.get_and_filtered(true, gyro_report.integral_dt, gyro_val_filt);
518  matrix::Vector3f accel_val_integ = _accel_int.get_and_filtered(true, accel_report.integral_dt, accel_val_filt);
519 
520  // Use the filtered (by integration) values to get smoother / less noisy data.
521  gyro_report.x = gyro_val_filt(0);
522  gyro_report.y = gyro_val_filt(1);
523  gyro_report.z = gyro_val_filt(2);
524 
525  accel_report.x = accel_val_filt(0);
526  accel_report.y = accel_val_filt(1);
527  accel_report.z = accel_val_filt(2);
528 
529  gyro_report.x_integral = gyro_val_integ(0);
530  gyro_report.y_integral = gyro_val_integ(1);
531  gyro_report.z_integral = gyro_val_integ(2);
532 
533  accel_report.x_integral = accel_val_integ(0);
534  accel_report.y_integral = accel_val_integ(1);
535  accel_report.z_integral = accel_val_integ(2);
536 
537  // TODO: when is this ever blocked?
538  if (!(m_pub_blocked)) {
539 
540  if (_gyro_topic == nullptr) {
541  _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &gyro_report,
543 
544  } else {
545  orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report);
546  }
547 
548  if (_accel_topic == nullptr) {
549  _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &accel_report,
551 
552  } else {
553  orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
554  }
555 
556  // Report if there are high vibrations, every 10 times it happens.
557  const bool threshold_reached = (data.accel_range_hit_counter - _last_accel_range_hit_count > 10);
558 
559  // Report every 5s.
560  const bool due_to_report = (hrt_elapsed_time(&_last_accel_range_hit_time) > 5000000);
561 
562  if (threshold_reached && due_to_report) {
564  "High accelerations, range exceeded %llu times",
565  data.accel_range_hit_counter);
566 
568  _last_accel_range_hit_count = data.accel_range_hit_counter;
569  }
570  }
571 
573 
574  // TODO: check the return codes of this function
575  return 0;
576 };
577 
578 
580 {
581 
583 
584 int start(enum Rotation rotation);
585 int stop();
586 int info();
587 void usage();
588 
589 int start(enum Rotation rotation)
590 {
591  g_dev = new DfMPU6050Wrapper(rotation);
592 
593  if (g_dev == nullptr) {
594  PX4_ERR("failed instantiating DfMPU6050Wrapper object");
595  return -1;
596  }
597 
598  int ret = g_dev->start();
599 
600  if (ret != 0) {
601  PX4_ERR("DfMPU6050Wrapper start failed");
602  return ret;
603  }
604 
605  // Open the IMU sensor
606  DevHandle h;
607  DevMgr::getHandle(IMU_DEVICE_PATH, h);
608 
609  if (!h.isValid()) {
610  DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
611  IMU_DEVICE_PATH, h.getError());
612  return -1;
613  }
614 
615  DevMgr::releaseHandle(h);
616 
617  return 0;
618 }
619 
620 int stop()
621 {
622  if (g_dev == nullptr) {
623  PX4_ERR("driver not running");
624  return 1;
625  }
626 
627  int ret = g_dev->stop();
628 
629  if (ret != 0) {
630  PX4_ERR("driver could not be stopped");
631  return ret;
632  }
633 
634  delete g_dev;
635  g_dev = nullptr;
636  return 0;
637 }
638 
639 /**
640  * Print a little info about the driver.
641  */
642 int
644 {
645  if (g_dev == nullptr) {
646  PX4_ERR("driver not running");
647  return 1;
648  }
649 
650  PX4_INFO("state @ %p", g_dev);
651  g_dev->info();
652 
653  return 0;
654 }
655 
656 void
658 {
659  PX4_INFO("Usage: df_mpu6050_wrapper 'start', 'info', 'stop'");
660  PX4_INFO("options:");
661  PX4_INFO(" -R rotation");
662 }
663 
664 } // namespace df_mpu6050_wrapper
665 
666 
667 int
668 df_mpu6050_wrapper_main(int argc, char *argv[])
669 {
670  int ch;
671  enum Rotation rotation = ROTATION_NONE;
672  int ret = 0;
673  int myoptind = 1;
674  const char *myoptarg = NULL;
675 
676  /* jump over start/off/etc and look at options first */
677  while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
678  switch (ch) {
679  case 'R':
680  rotation = (enum Rotation)atoi(myoptarg);
681  break;
682 
683  default:
685  return 0;
686  }
687  }
688 
689  if (argc <= 1) {
691  return 1;
692  }
693 
694  const char *verb = argv[myoptind];
695 
696  if (!strcmp(verb, "start")) {
697  ret = df_mpu6050_wrapper::start(rotation);
698  }
699 
700  else if (!strcmp(verb, "stop")) {
701  ret = df_mpu6050_wrapper::stop();
702  }
703 
704  else if (!strcmp(verb, "info")) {
705  ret = df_mpu6050_wrapper::info();
706  }
707 
708  else {
710  return 1;
711  }
712 
713  return ret;
714 }
perf_counter_t _accel_range_hit_counter
uint32_t integral_dt
Definition: sensor_accel.h:59
orb_advert_t _mavlink_log_pub
int start(enum Rotation rotation)
Accelerometer driver interface.
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
Definition: uORB.cpp:90
perf_counter_t _error_counter
uint64_t timestamp
Definition: sensor_accel.h:53
perf_counter_t _read_counter
Gyroscope driver interface.
measure the time elapsed performing an event
Definition: perf_counter.h:56
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
Definition: parameters.cpp:589
uint64_t _last_accel_range_hit_count
int16_t x_raw
Definition: sensor_gyro.h:65
void usage(const char *reason)
Print the correct usage.
Definition: Commander.cpp:4238
Definition: I2C.hpp:51
float x_integral
Definition: sensor_gyro.h:60
static void stop()
Definition: dataman.cpp:1491
int stop()
Stop the driver.
struct DfMPU6050Wrapper::gyro_calibration_s _gyro_calibration
void usage()
Prints info about the driver argument usage.
int16_t z_raw
Definition: sensor_gyro.h:67
int16_t y_raw
Definition: sensor_gyro.h:66
count the number of times an event occurs
Definition: perf_counter.h:55
static int32_t device_id[max_accel_sens]
Vector rotation library.
void info()
Print some debug info.
High-resolution timer with callouts and timekeeping.
matrix::Vector3f get_and_filtered(bool reset, uint32_t &integral_dt, matrix::Vector3f &filtered_val)
Get the current integral and reset the integrator if needed.
Definition: integrator.cpp:150
bool put(const uint64_t &timestamp, const matrix::Vector3f &val, matrix::Vector3f &integral, uint32_t &integral_dt)
Put an item into the integral.
Definition: integrator.cpp:54
Global flash based parameter store.
perf_counter_t _fifo_corruption_counter
int orb_subscribe(const struct orb_metadata *meta)
Definition: uORB.cpp:75
float z_integral
Definition: sensor_gyro.h:62
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
__EXPORT int df_mpu6050_wrapper_main(int argc, char *argv[])
Header common to all counters.
void perf_free(perf_counter_t handle)
Free a counter.
void init()
Activates/configures the hardware registers.
#define perf_alloc(a, b)
Definition: px4io.h:59
Rotation
Enum for board and external compass rotations.
Definition: rotation.h:51
uint8_t * data
Definition: dataman.cpp:149
perf_counter_t _fifo_overflow_counter
static hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
Compute the delta between a timestamp taken in the past and now.
Definition: drv_hrt.h:102
float y_integral
Definition: sensor_gyro.h:61
Simple error/warning functions, heavily inspired by the BSD functions of the same names...
void perf_end(perf_counter_t handle)
End a performance event.
uint64_t timestamp
Definition: sensor_gyro.h:53
hrt_abstime _last_accel_range_hit_time
struct DfMPU6050Wrapper::accel_calibration_s _accel_calibration
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
Definition: uORB.cpp:70
uint32_t device_id
Definition: sensor_gyro.h:55
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
Definition: parameters.cpp:370
static int start()
Definition: dataman.cpp:1452
A resettable integrator.
int start()
Start automatic measurement.
int stop()
Stop automatic measurement.
#define MPU6050_NEVER_AUTOPUBLISH_US
DfMPU6050Wrapper * g_dev
perf_counter_t _gyro_range_hit_counter
int info()
Print a little info about the driver.
int orb_check(int handle, bool *updated)
Definition: uORB.cpp:95
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
int _publish(struct imu_sensor_data &data)
uint32_t device_id
Definition: sensor_accel.h:55
__EXPORT matrix::Dcmf get_rot_matrix(enum Rotation rot)
Get the rotation matrix.
Definition: rotation.cpp:45
void perf_set_count(perf_counter_t handle, uint64_t count)
Set a counter.
#define OK
Definition: uavcan_main.cpp:71
perf_counter_t _publish_perf
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority)
Definition: uORB.cpp:53
matrix::Dcmf _rotation_matrix
void perf_begin(perf_counter_t handle)
Begin a performance event.
DfMPU6050Wrapper(enum Rotation rotation)
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
uint32_t integral_dt
Definition: sensor_gyro.h:59
Performance measuring tools.