PX4 Firmware
PX4 Autopilot Software http://px4.io
df_mpu9250_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_mpu9250_wrapper.cpp
36  * Lightweight driver to access the MPU9250 of the DriverFramework.
37  *
38  * @author Julian Oes <julian@oes.ch>
39  */
40 
41 #include <px4_platform_common/px4_config.h>
42 
43 #include <sys/types.h>
44 #include <sys/stat.h>
45 #include <stdint.h>
46 #include <stddef.h>
47 #include <stdlib.h>
48 #include <string.h>
49 #include <math.h>
50 #include <unistd.h>
51 #include <px4_platform_common/getopt.h>
52 #include <errno.h>
53 #include <lib/parameters/param.h>
54 #include <systemlib/err.h>
55 #include <perf/perf_counter.h>
56 #include <systemlib/mavlink_log.h>
57 
58 #include <drivers/drv_hrt.h>
59 #include <drivers/drv_accel.h>
60 #include <drivers/drv_gyro.h>
61 #include <drivers/drv_mag.h>
64 
66 
68 
69 #include <mpu9250/MPU9250.hpp>
70 #include <DevMgr.hpp>
71 
72 #define MPU9250_ACCEL_DEFAULT_RATE 1000
73 #define MPU9250_GYRO_DEFAULT_RATE 1000
74 
75 #define MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30
76 #define MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30
77 
78 #define MPU9250_PUB_RATE 280
79 
80 
81 extern "C" { __EXPORT int df_mpu9250_wrapper_main(int argc, char *argv[]); }
82 
83 using namespace DriverFramework;
84 
85 
86 class DfMpu9250Wrapper : public MPU9250
87 {
88 public:
89  DfMpu9250Wrapper(bool mag_enabled, enum Rotation rotation);
91 
92 
93  /**
94  * Start automatic measurement.
95  *
96  * @return 0 on success
97  */
98  int start();
99 
100  /**
101  * Stop automatic measurement.
102  *
103  * @return 0 on success
104  */
105  int stop();
106 
107  /**
108  * Print some debug info.
109  */
110  void info();
111 
112 private:
113  int _publish(struct imu_sensor_data &data);
114 
115  void _update_accel_calibration();
116  void _update_gyro_calibration();
117  void _update_mag_calibration();
118 
122 
124 
126 
128  float x_offset;
129  float x_scale;
130  float y_offset;
131  float y_scale;
132  float z_offset;
133  float z_scale;
134  } _accel_calibration;
135 
137  float x_offset;
138  float x_scale;
139  float y_offset;
140  float y_scale;
141  float z_offset;
142  float z_scale;
143  } _gyro_calibration;
144 
146  float x_offset;
147  float x_scale;
148  float y_offset;
149  float y_scale;
150  float z_offset;
151  float z_scale;
152  } _mag_calibration;
153 
157 
160 
167 
176 
179 
181 
182  enum Rotation _rotation;
183 };
184 
185 DfMpu9250Wrapper::DfMpu9250Wrapper(bool mag_enabled, enum Rotation rotation) :
186  MPU9250(IMU_DEVICE_PATH, mag_enabled),
187  _accel_topic(nullptr),
188  _gyro_topic(nullptr),
189  _mag_topic(nullptr),
190  _mavlink_log_pub(nullptr),
191  _param_update_sub(-1),
192  _accel_calibration{},
198  _accel_int(1000000 / MPU9250_PUB_RATE, false),
199  _gyro_int(1000000 / MPU9250_PUB_RATE, true),
206  _read_counter(perf_alloc(PC_COUNT, "mpu9250_reads")),
207  _error_counter(perf_alloc(PC_COUNT, "mpu9250_errors")),
208  _fifo_overflow_counter(perf_alloc(PC_COUNT, "mpu9250_fifo_overflows")),
209  _fifo_corruption_counter(perf_alloc(PC_COUNT, "mpu9250_fifo_corruptions")),
210  _gyro_range_hit_counter(perf_alloc(PC_COUNT, "mpu9250_gyro_range_hits")),
211  _accel_range_hit_counter(perf_alloc(PC_COUNT, "mpu9250_accel_range_hits")),
212  _mag_fifo_overflow_counter(perf_alloc(PC_COUNT, "mpu9250_mag_fifo_overflows")),
213  _publish_perf(perf_alloc(PC_ELAPSED, "mpu9250_publish")),
216  _mag_enabled(mag_enabled),
217  _rotation(rotation)
218 {
219  // Set sane default calibration values
226 
227  _gyro_calibration.x_scale = 1.0f;
228  _gyro_calibration.y_scale = 1.0f;
229  _gyro_calibration.z_scale = 1.0f;
233 
234  if (_mag_enabled) {
235  _mag_calibration.x_scale = 1.0f;
236  _mag_calibration.y_scale = 1.0f;
237  _mag_calibration.z_scale = 1.0f;
238  _mag_calibration.x_offset = 0.0f;
239  _mag_calibration.y_offset = 0.0f;
240  _mag_calibration.z_offset = 0.0f;
241  }
242 
243  // set software low pass filter for controllers
244  param_t param_handle = param_find("IMU_ACCEL_CUTOFF");
246 
247  if (param_handle != PARAM_INVALID && (param_get(param_handle, &param_val) == PX4_OK)) {
251 
252  } else {
253  PX4_ERR("IMU_ACCEL_CUTOFF param invalid");
254  }
255 
256  param_handle = param_find("IMU_GYRO_CUTOFF");
258 
259  if (param_handle != PARAM_INVALID && (param_get(param_handle, &param_val) == PX4_OK)) {
263 
264  } else {
265  PX4_ERR("IMU_GYRO_CUTOFF param invalid");
266  }
267 }
268 
270 {
277 
278  if (_mag_enabled) {
280  }
281 
283 }
284 
286 {
287  // TODO: don't publish garbage here
288  sensor_accel_s accel_report = {};
289  _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &accel_report,
291 
292  if (_accel_topic == nullptr) {
293  PX4_ERR("sensor_accel advert fail");
294  return -1;
295  }
296 
297  // TODO: don't publish garbage here
298  sensor_gyro_s gyro_report = {};
299  _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &gyro_report,
301 
302  if (_gyro_topic == nullptr) {
303  PX4_ERR("sensor_gyro advert fail");
304  return -1;
305  }
306 
307  if (_mag_enabled) {
308  }
309 
310  /* Subscribe to param update topic. */
311  if (_param_update_sub < 0) {
312  _param_update_sub = orb_subscribe(ORB_ID(parameter_update));
313  }
314 
315  /* Init device and start sensor. */
316  int ret = init();
317 
318  if (ret != 0) {
319  PX4_ERR("MPU9250 init fail: %d", ret);
320  return ret;
321  }
322 
323  ret = MPU9250::start();
324 
325  if (ret != 0) {
326  PX4_ERR("MPU9250 start fail: %d", ret);
327  return ret;
328  }
329 
330  PX4_DEBUG("MPU9250 device id is: %d", m_id.dev_id);
331 
332  /* Force getting the calibration values. */
336 
337  return 0;
338 }
339 
341 {
342  /* Stop sensor. */
343  int ret = MPU9250::stop();
344 
345  if (ret != 0) {
346  PX4_ERR("MPU9250 stop fail: %d", ret);
347  return ret;
348  }
349 
350  return 0;
351 }
352 
354 {
361 
362  if (_mag_enabled) {
364  }
365 
367 }
368 
370 {
371  // TODO: replace magic number
372  for (unsigned i = 0; i < 3; ++i) {
373 
374  // TODO: remove printfs and add error counter
375 
376  char str[30];
377  (void)sprintf(str, "CAL_GYRO%u_ID", i);
378  int32_t device_id;
379  int res = param_get(param_find(str), &device_id);
380 
381  if (res != OK) {
382  PX4_ERR("Could not access param %s", str);
383  continue;
384  }
385 
386  if ((uint32_t)device_id != m_id.dev_id) {
387  continue;
388  }
389 
390  (void)sprintf(str, "CAL_GYRO%u_XSCALE", i);
392 
393  if (res != OK) {
394  PX4_ERR("Could not access param %s", str);
395  }
396 
397  (void)sprintf(str, "CAL_GYRO%u_YSCALE", i);
399 
400  if (res != OK) {
401  PX4_ERR("Could not access param %s", str);
402  }
403 
404  (void)sprintf(str, "CAL_GYRO%u_ZSCALE", i);
406 
407  if (res != OK) {
408  PX4_ERR("Could not access param %s", str);
409  }
410 
411  (void)sprintf(str, "CAL_GYRO%u_XOFF", i);
413 
414  if (res != OK) {
415  PX4_ERR("Could not access param %s", str);
416  }
417 
418  (void)sprintf(str, "CAL_GYRO%u_YOFF", i);
420 
421  if (res != OK) {
422  PX4_ERR("Could not access param %s", str);
423  }
424 
425  (void)sprintf(str, "CAL_GYRO%u_ZOFF", i);
427 
428  if (res != OK) {
429  PX4_ERR("Could not access param %s", str);
430  }
431 
432  // We got calibration values, let's exit.
433  return;
434  }
435 
436  _gyro_calibration.x_scale = 1.0f;
437  _gyro_calibration.y_scale = 1.0f;
438  _gyro_calibration.z_scale = 1.0f;
442 }
443 
445 {
446  // TODO: replace magic number
447  for (unsigned i = 0; i < 3; ++i) {
448 
449  // TODO: remove printfs and add error counter
450 
451  char str[30];
452  (void)sprintf(str, "CAL_ACC%u_ID", i);
453  int32_t device_id;
454  int res = param_get(param_find(str), &device_id);
455 
456  if (res != OK) {
457  PX4_ERR("Could not access param %s", str);
458  continue;
459  }
460 
461  if ((uint32_t)device_id != m_id.dev_id) {
462  continue;
463  }
464 
465  (void)sprintf(str, "CAL_ACC%u_XSCALE", i);
467 
468  if (res != OK) {
469  PX4_ERR("Could not access param %s", str);
470  }
471 
472  (void)sprintf(str, "CAL_ACC%u_YSCALE", i);
474 
475  if (res != OK) {
476  PX4_ERR("Could not access param %s", str);
477  }
478 
479  (void)sprintf(str, "CAL_ACC%u_ZSCALE", i);
481 
482  if (res != OK) {
483  PX4_ERR("Could not access param %s", str);
484  }
485 
486  (void)sprintf(str, "CAL_ACC%u_XOFF", i);
488 
489  if (res != OK) {
490  PX4_ERR("Could not access param %s", str);
491  }
492 
493  (void)sprintf(str, "CAL_ACC%u_YOFF", i);
495 
496  if (res != OK) {
497  PX4_ERR("Could not access param %s", str);
498  }
499 
500  (void)sprintf(str, "CAL_ACC%u_ZOFF", i);
502 
503  if (res != OK) {
504  PX4_ERR("Could not access param %s", str);
505  }
506 
507  // We got calibration values, let's exit.
508  return;
509  }
510 
511  // Set sane default calibration values
518 }
519 
521 {
522  if (!_mag_enabled) {
523  return;
524  }
525 
526  // TODO: replace magic number
527  for (unsigned i = 0; i < 3; ++i) {
528 
529  // TODO: remove printfs and add error counter
530 
531  char str[30];
532  (void)sprintf(str, "CAL_MAG%u_ID", i);
533  int32_t device_id;
534  int res = param_get(param_find(str), &device_id);
535 
536  if (res != OK) {
537  PX4_ERR("Could not access param %s", str);
538  continue;
539  }
540 
541  if ((uint32_t)device_id != m_id.dev_id) {
542  continue;
543  }
544 
545  (void)sprintf(str, "CAL_MAG%u_XSCALE", i);
547 
548  if (res != OK) {
549  PX4_ERR("Could not access param %s", str);
550  }
551 
552  (void)sprintf(str, "CAL_MAG%u_YSCALE", i);
554 
555  if (res != OK) {
556  PX4_ERR("Could not access param %s", str);
557  }
558 
559  (void)sprintf(str, "CAL_MAG%u_ZSCALE", i);
561 
562  if (res != OK) {
563  PX4_ERR("Could not access param %s", str);
564  }
565 
566  (void)sprintf(str, "CAL_MAG%u_XOFF", i);
568 
569  if (res != OK) {
570  PX4_ERR("Could not access param %s", str);
571  }
572 
573  (void)sprintf(str, "CAL_MAG%u_YOFF", i);
575 
576  if (res != OK) {
577  PX4_ERR("Could not access param %s", str);
578  }
579 
580  (void)sprintf(str, "CAL_MAG%u_ZOFF", i);
582 
583  if (res != OK) {
584  PX4_ERR("Could not access param %s", str);
585  }
586 
587  // We got calibration values, let's exit.
588  return;
589  }
590 
591  // Set sane default calibration values
592  _mag_calibration.x_scale = 1.0f;
593  _mag_calibration.y_scale = 1.0f;
594  _mag_calibration.z_scale = 1.0f;
595  _mag_calibration.x_offset = 0.0f;
596  _mag_calibration.y_offset = 0.0f;
597  _mag_calibration.z_offset = 0.0f;
598 }
599 
600 int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
601 {
602  /* Check if calibration values are still up-to-date. */
603  bool updated;
604  orb_check(_param_update_sub, &updated);
605 
606  if (updated) {
607  parameter_update_s parameter_update;
608  orb_copy(ORB_ID(parameter_update), _param_update_sub, &parameter_update);
609 
613  }
614 
615  sensor_accel_s accel_report = {};
616  sensor_gyro_s gyro_report = {};
617  mag_report mag_report = {};
618 
619  accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time();
620 
621  accel_report.error_count = gyro_report.error_count = mag_report.error_count = data.error_counter;
622 
623  // ACCEL
624 
625  float xraw_f = data.accel_m_s2_x;
626  float yraw_f = data.accel_m_s2_y;
627  float zraw_f = data.accel_m_s2_z;
628 
629  // apply user specified rotation
630  rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
631 
632  // MPU9250 driver from DriverFramework does not provide any raw values
633  // TEMP We misuse the raw values on the Snapdragon to publish unfiltered data for VISLAM
634  accel_report.x_raw = (int16_t)(xraw_f * 1000); // (int16) [m / s^2 * 1000];
635  accel_report.y_raw = (int16_t)(yraw_f * 1000); // (int16) [m / s^2 * 1000];
636  accel_report.z_raw = (int16_t)(zraw_f * 1000); // (int16) [m / s^2 * 1000];
637 
638  // adjust values according to the calibration
639  float x_in_new = (xraw_f - _accel_calibration.x_offset) * _accel_calibration.x_scale;
640  float y_in_new = (yraw_f - _accel_calibration.y_offset) * _accel_calibration.y_scale;
641  float z_in_new = (zraw_f - _accel_calibration.z_offset) * _accel_calibration.z_scale;
642 
643  accel_report.x = _accel_filter_x.apply(x_in_new);
644  accel_report.y = _accel_filter_y.apply(y_in_new);
645  accel_report.z = _accel_filter_z.apply(z_in_new);
646 
647  matrix::Vector3f aval(x_in_new, y_in_new, z_in_new);
648  matrix::Vector3f aval_integrated;
649 
650  _accel_int.put(accel_report.timestamp, aval, aval_integrated, accel_report.integral_dt);
651  accel_report.x_integral = aval_integrated(0);
652  accel_report.y_integral = aval_integrated(1);
653  accel_report.z_integral = aval_integrated(2);
654 
655  // GYRO
656 
657  xraw_f = data.gyro_rad_s_x;
658  yraw_f = data.gyro_rad_s_y;
659  zraw_f = data.gyro_rad_s_z;
660 
661  // apply user specified rotation
662  rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
663 
664  // MPU9250 driver from DriverFramework does not provide any raw values
665  // TEMP We misuse the raw values on the Snapdragon to publish unfiltered data for VISLAM
666  gyro_report.x_raw = (int16_t)(xraw_f * 1000); // (int16) [rad / s * 1000];
667  gyro_report.y_raw = (int16_t)(yraw_f * 1000); // (int16) [rad / s * 1000];
668  gyro_report.z_raw = (int16_t)(zraw_f * 1000); // (int16) [rad / s * 1000];
669 
670  // adjust values according to the calibration
671  float x_gyro_in_new = (xraw_f - _gyro_calibration.x_offset) * _gyro_calibration.x_scale;
672  float y_gyro_in_new = (yraw_f - _gyro_calibration.y_offset) * _gyro_calibration.y_scale;
673  float z_gyro_in_new = (zraw_f - _gyro_calibration.z_offset) * _gyro_calibration.z_scale;
674 
675  gyro_report.x = _gyro_filter_x.apply(x_gyro_in_new);
676  gyro_report.y = _gyro_filter_y.apply(y_gyro_in_new);
677  gyro_report.z = _gyro_filter_z.apply(z_gyro_in_new);
678 
679  matrix::Vector3f gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
680  matrix::Vector3f gval_integrated;
681 
682  bool sensor_notify = _gyro_int.put(gyro_report.timestamp, gval, gval_integrated, gyro_report.integral_dt);
683  gyro_report.x_integral = gval_integrated(0);
684  gyro_report.y_integral = gval_integrated(1);
685  gyro_report.z_integral = gval_integrated(2);
686 
687 
688  // if gyro integrator did not return a sample we can return here
689  // Note: the accel integrator receives the same timestamp as the gyro integrator
690  // so we do not need to handle it seperately
691  if (!sensor_notify) {
692  return 0;
693  }
694 
695  // Update all the counters.
696  perf_set_count(_read_counter, data.read_counter);
697  perf_set_count(_error_counter, data.error_counter);
698  perf_set_count(_fifo_overflow_counter, data.fifo_overflow_counter);
699  perf_set_count(_fifo_corruption_counter, data.fifo_overflow_counter);
700  perf_set_count(_gyro_range_hit_counter, data.gyro_range_hit_counter);
701  perf_set_count(_accel_range_hit_counter, data.accel_range_hit_counter);
702 
703  if (_mag_enabled) {
704  perf_set_count(_mag_fifo_overflow_counter, data.mag_fifo_overflow_counter);
705  }
706 
708 
709  // TODO: get these right
710  gyro_report.scaling = -1.0f;
711  gyro_report.device_id = m_id.dev_id;
712 
713  accel_report.scaling = -1.0f;
714  accel_report.device_id = m_id.dev_id;
715 
716  if (_mag_enabled) {
717  mag_report.timestamp = accel_report.timestamp;
718  mag_report.is_external = false;
719 
720  mag_report.scaling = -1.0f;
721  mag_report.device_id = m_id.dev_id;
722 
723  xraw_f = data.mag_ga_x;
724  yraw_f = data.mag_ga_y;
725  zraw_f = data.mag_ga_z;
726 
727  rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
728 
729  // MPU9250 driver from DriverFramework does not provide any raw values
730  // TEMP We misuse the raw values on the Snapdragon to publish unfiltered data for VISLAM
731  mag_report.x_raw = xraw_f * 1000; // (int16) [Gs * 1000]
732  mag_report.y_raw = yraw_f * 1000; // (int16) [Gs * 1000]
733  mag_report.z_raw = zraw_f * 1000; // (int16) [Gs * 1000]
734 
735  mag_report.x = (xraw_f - _mag_calibration.x_offset) * _mag_calibration.x_scale;
736  mag_report.y = (yraw_f - _mag_calibration.y_offset) * _mag_calibration.y_scale;
737  mag_report.z = (zraw_f - _mag_calibration.z_offset) * _mag_calibration.z_scale;
738  }
739 
740  // TODO: when is this ever blocked?
741  if (!(m_pub_blocked) && sensor_notify) {
742 
743  if (_gyro_topic != nullptr) {
744  orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report);
745  }
746 
747  if (_accel_topic != nullptr) {
748  orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
749  }
750 
751  if (_mag_enabled) {
752 
753  if (_mag_topic == nullptr) {
754  _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report,
756 
757  } else {
758  orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
759  }
760  }
761 
762  // Report if there are high vibrations, every 10 times it happens.
763  const bool threshold_reached = (data.accel_range_hit_counter - _last_accel_range_hit_count > 10);
764 
765  // Report every 5s.
766  const bool due_to_report = (hrt_elapsed_time(&_last_accel_range_hit_time) > 5000000);
767 
768  if (threshold_reached && due_to_report) {
770  "High accelerations, range exceeded %llu times",
771  data.accel_range_hit_counter);
772 
774  _last_accel_range_hit_count = data.accel_range_hit_counter;
775  }
776  }
777 
779 
780  // TODO: check the return codes of this function
781  return 0;
782 };
783 
784 
786 {
787 
789 
790 int start(bool mag_enabled, enum Rotation rotation);
791 int stop();
792 int info();
793 void usage();
794 
795 int start(bool mag_enabled, enum Rotation rotation)
796 {
797  g_dev = new DfMpu9250Wrapper(mag_enabled, rotation);
798 
799  if (g_dev == nullptr) {
800  PX4_ERR("failed instantiating DfMpu9250Wrapper object");
801  return -1;
802  }
803 
804  int ret = g_dev->start();
805 
806  if (ret != 0) {
807  PX4_ERR("DfMpu9250Wrapper start failed");
808  return ret;
809  }
810 
811  // Open the IMU sensor
812  DevHandle h;
813  DevMgr::getHandle(IMU_DEVICE_PATH, h);
814 
815  if (!h.isValid()) {
816  DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
817  IMU_DEVICE_PATH, h.getError());
818  return -1;
819  }
820 
821  DevMgr::releaseHandle(h);
822 
823  return 0;
824 }
825 
826 int stop()
827 {
828  if (g_dev == nullptr) {
829  PX4_ERR("driver not running");
830  return 1;
831  }
832 
833  int ret = g_dev->stop();
834 
835  if (ret != 0) {
836  PX4_ERR("driver could not be stopped");
837  return ret;
838  }
839 
840  delete g_dev;
841  g_dev = nullptr;
842  return 0;
843 }
844 
845 /**
846  * Print a little info about the driver.
847  */
848 int
850 {
851  if (g_dev == nullptr) {
852  PX4_ERR("driver not running");
853  return 1;
854  }
855 
856  PX4_INFO("state @ %p", g_dev);
857  g_dev->info();
858 
859  return 0;
860 }
861 
862 void
864 {
865  PX4_INFO("Usage: df_mpu9250_wrapper 'start', 'start_without_mag', 'info', 'stop'");
866  PX4_INFO("options:");
867  PX4_INFO(" -R rotation");
868 }
869 
870 } // namespace df_mpu9250_wrapper
871 
872 
873 int
874 df_mpu9250_wrapper_main(int argc, char *argv[])
875 {
876  int ch;
877  enum Rotation rotation = ROTATION_NONE;
878  int ret = 0;
879  int myoptind = 1;
880  const char *myoptarg = NULL;
881 
882  /* jump over start/off/etc and look at options first */
883  while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
884  switch (ch) {
885  case 'R':
886  rotation = (enum Rotation)atoi(myoptarg);
887  break;
888 
889  default:
891  return 0;
892  }
893  }
894 
895  if (argc <= 1) {
897  return 1;
898  }
899 
900  const char *verb = argv[myoptind];
901 
902  if (!strcmp(verb, "start_without_mag")) {
903  ret = df_mpu9250_wrapper::start(false, rotation);
904  }
905 
906  else if (!strcmp(verb, "start")) {
907  ret = df_mpu9250_wrapper::start(true, rotation);
908  }
909 
910  else if (!strcmp(verb, "stop")) {
911  ret = df_mpu9250_wrapper::stop();
912  }
913 
914  else if (!strcmp(verb, "info")) {
915  ret = df_mpu9250_wrapper::info();
916  }
917 
918  else {
920  return 1;
921  }
922 
923  return ret;
924 }
#define PARAM_INVALID
Handle returned when a parameter cannot be found.
Definition: param.h:103
uint32_t integral_dt
Definition: sensor_accel.h:59
math::LowPassFilter2p _gyro_filter_y
Accelerometer driver interface.
hrt_abstime _last_accel_range_hit_time
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
Definition: uORB.cpp:90
uint64_t timestamp
Definition: sensor_accel.h:53
Gyroscope driver interface.
perf_counter_t _fifo_corruption_counter
void info()
Print some debug info.
measure the time elapsed performing an event
Definition: perf_counter.h:56
struct DfMpu9250Wrapper::accel_calibration_s _accel_calibration
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
Definition: parameters.cpp:589
int16_t x_raw
Definition: sensor_gyro.h:65
__EXPORT void rotate_3f(enum Rotation rot, float &x, float &y, float &z)
rotate a 3 element float vector in-place
Definition: rotation.cpp:63
void usage(const char *reason)
Print the correct usage.
Definition: Commander.cpp:4238
Definition: I2C.hpp:51
uint64_t error_count
Definition: sensor_gyro.h:54
float x_integral
Definition: sensor_gyro.h:60
#define MPU9250_PUB_RATE
void set_cutoff_frequency(float sample_freq, float cutoff_freq)
static void stop()
Definition: dataman.cpp:1491
math::LowPassFilter2p _accel_filter_z
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
#define MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ
perf_counter_t _fifo_overflow_counter
DfMpu9250Wrapper * g_dev
static int32_t device_id[max_accel_sens]
Vector rotation library.
High-resolution timer with callouts and timekeeping.
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.
virtual int init()
Definition: mpu9250.cpp:104
math::LowPassFilter2p _gyro_filter_x
struct DfMpu9250Wrapper::mag_calibration_s _mag_calibration
perf_counter_t _publish_perf
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
Header common to all counters.
void perf_free(perf_counter_t handle)
Free a counter.
struct DfMpu9250Wrapper::gyro_calibration_s _gyro_calibration
#define MPU9250_GYRO_DEFAULT_RATE
#define perf_alloc(a, b)
Definition: px4io.h:59
uint64_t _last_accel_range_hit_count
perf_counter_t _error_counter
Rotation
Enum for board and external compass rotations.
Definition: rotation.h:51
uint8_t * data
Definition: dataman.cpp:149
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
int stop()
Stop automatic measurement.
int stop()
Stop the driver.
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
math::LowPassFilter2p _accel_filter_y
uint64_t error_count
Definition: sensor_accel.h:54
__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.
perf_counter_t _mag_fifo_overflow_counter
float apply(float sample)
Add a new raw value to the filter.
int info()
Print a little info about the driver.
int start()
Start automatic measurement.
int _publish(struct imu_sensor_data &data)
int orb_check(int handle, bool *updated)
Definition: uORB.cpp:95
void usage()
Prints info about the driver argument usage.
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
perf_counter_t _read_counter
DfMpu9250Wrapper(bool mag_enabled, enum Rotation rotation)
#define MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ
uint32_t device_id
Definition: sensor_accel.h:55
#define MPU9250_ACCEL_DEFAULT_RATE
orb_advert_t _mavlink_log_pub
__EXPORT int df_mpu9250_wrapper_main(int argc, char *argv[])
void perf_set_count(perf_counter_t handle, uint64_t count)
Set a counter.
#define OK
Definition: uavcan_main.cpp:71
math::LowPassFilter2p _accel_filter_x
void stop()
Definition: mpu9250.cpp:501
perf_counter_t _accel_range_hit_counter
perf_counter_t _gyro_range_hit_counter
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority)
Definition: uORB.cpp:53
void perf_begin(perf_counter_t handle)
Begin a performance event.
void start()
Definition: mpu9250.cpp:492
math::LowPassFilter2p _gyro_filter_z
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
uint32_t param_t
Parameter handle.
Definition: param.h:98
uint32_t integral_dt
Definition: sensor_gyro.h:59
int start(bool mag_enabled, enum Rotation rotation)
Performance measuring tools.
#define mag_report
Definition: drv_mag.h:53