PX4 Firmware
PX4 Autopilot Software http://px4.io
df_lsm9ds1_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_lsm9ds1_wrapper.cpp
36  * Lightweight driver to access the MPU9250 of the DriverFramework.
37  *
38  * @author Miguel Arroyo <miguel@arroyo.me>
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 
54 #include <systemlib/err.h>
55 #include <lib/parameters/param.h>
56 #include <perf/perf_counter.h>
57 #include <systemlib/mavlink_log.h>
58 
59 #include <drivers/drv_hrt.h>
60 #include <drivers/drv_accel.h>
61 #include <drivers/drv_gyro.h>
62 #include <drivers/drv_mag.h>
64 
66 
68 
69 #include <lsm9ds1/LSM9DS1.hpp>
70 #include <DevMgr.hpp>
71 
72 // We don't want to auto publish, therefore set this to 0.
73 #define LSM9DS1_NEVER_AUTOPUBLISH_US 0
74 
75 
76 extern "C" { __EXPORT int df_lsm9ds1_wrapper_main(int argc, char *argv[]); }
77 
78 using namespace DriverFramework;
79 
80 
81 class DfLsm9ds1Wrapper : public LSM9DS1
82 {
83 public:
84  DfLsm9ds1Wrapper(bool mag_enabled, enum Rotation rotation);
86 
87 
88  /**
89  * Start automatic measurement.
90  *
91  * @return 0 on success
92  */
93  int start();
94 
95  /**
96  * Stop automatic measurement.
97  *
98  * @return 0 on success
99  */
100  int stop();
101 
102  /**
103  * Print some debug info.
104  */
105  void info();
106 
107 private:
108  int _publish(struct imu_sensor_data &data);
109 
110  void _update_accel_calibration();
111  void _update_gyro_calibration();
112  void _update_mag_calibration();
113 
118 
120 
122  float x_offset;
123  float x_scale;
124  float y_offset;
125  float y_scale;
126  float z_offset;
127  float z_scale;
128  } _accel_calibration;
129 
131  float x_offset;
132  float x_scale;
133  float y_offset;
134  float y_scale;
135  float z_offset;
136  float z_scale;
137  } _gyro_calibration;
138 
140  float x_offset;
141  float x_scale;
142  float y_offset;
143  float y_scale;
144  float z_offset;
145  float z_scale;
146  } _mag_calibration;
147 
152 
155 
156  unsigned _publish_count;
157 
165 
168 
170 };
171 
172 DfLsm9ds1Wrapper::DfLsm9ds1Wrapper(bool mag_enabled, enum Rotation rotation) :
173  LSM9DS1(IMU_DEVICE_ACC_GYRO, IMU_DEVICE_MAG),
174  _accel_topic(nullptr),
175  _gyro_topic(nullptr),
176  _mag_topic(nullptr),
177  _mavlink_log_pub(nullptr),
178  _param_update_sub(-1),
179  _accel_calibration{},
187  _publish_count(0),
188  _read_counter(perf_alloc(PC_COUNT, "lsm9ds1_reads")),
189  _error_counter(perf_alloc(PC_COUNT, "lsm9ds1_errors")),
190  _fifo_overflow_counter(perf_alloc(PC_COUNT, "lsm9ds1_fifo_overflows")),
191  _fifo_corruption_counter(perf_alloc(PC_COUNT, "lsm9ds1_fifo_corruptions")),
192  _gyro_range_hit_counter(perf_alloc(PC_COUNT, "lsm9ds1_gyro_range_hits")),
193  _accel_range_hit_counter(perf_alloc(PC_COUNT, "lsm9ds1_accel_range_hits")),
194  _publish_perf(perf_alloc(PC_ELAPSED, "lsm9ds1_publish")),
197  _mag_enabled(mag_enabled)
198 {
199  // Set sane default calibration values
206 
207  _gyro_calibration.x_scale = 1.0f;
208  _gyro_calibration.y_scale = 1.0f;
209  _gyro_calibration.z_scale = 1.0f;
213 
214  if (_mag_enabled) {
215  _mag_calibration.x_scale = 1.0f;
216  _mag_calibration.y_scale = 1.0f;
217  _mag_calibration.z_scale = 1.0f;
218  _mag_calibration.x_offset = 0.0f;
219  _mag_calibration.y_offset = 0.0f;
220  _mag_calibration.z_offset = 0.0f;
221  }
222 
223  // Get sensor rotation matrix
224  _rotation_matrix = get_rot_matrix(rotation);
225 }
226 
228 {
235 
237 }
238 
240 {
241  // TODO: don't publish garbage here
242  sensor_accel_s accel_report = {};
243  _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &accel_report,
245 
246  if (_accel_topic == nullptr) {
247  PX4_ERR("sensor_accel advert fail");
248  return -1;
249  }
250 
251  // TODO: don't publish garbage here
252  sensor_gyro_s gyro_report = {};
253  _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &gyro_report,
255 
256  if (_gyro_topic == nullptr) {
257  PX4_ERR("sensor_gyro advert fail");
258  return -1;
259  }
260 
261  if (_mag_enabled) {
262  mag_report mag_report = {};
263  _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report,
265 
266  if (_mag_topic == nullptr) {
267  PX4_ERR("sensor_mag advert fail");
268  return -1;
269  }
270  }
271 
272  /* Subscribe to param update topic. */
273  if (_param_update_sub < 0) {
274  _param_update_sub = orb_subscribe(ORB_ID(parameter_update));
275  }
276 
277  /* Init device and start sensor. */
278  int ret = init();
279 
280  if (ret != 0) {
281  PX4_ERR("LSM9DS1 init fail: %d", ret);
282  return ret;
283  }
284 
285  ret = LSM9DS1::start();
286 
287  if (ret != 0) {
288  PX4_ERR("LSM9DS1 start fail: %d", ret);
289  return ret;
290  }
291 
292  PX4_DEBUG("LSM9DS1 device id is: %d", m_id.dev_id);
293 
294  /* Force getting the calibration values. */
298 
299  return 0;
300 }
301 
303 {
304  /* Stop sensor. */
305  int ret = LSM9DS1::stop();
306 
307  if (ret != 0) {
308  PX4_ERR("LSM9DS1 stop fail: %d", ret);
309  return ret;
310  }
311 
312  return 0;
313 }
314 
316 {
323 
325 }
326 
328 {
329  // TODO: replace magic number
330  for (unsigned i = 0; i < 3; ++i) {
331 
332  // TODO: remove printfs and add error counter
333 
334  char str[30];
335  (void)sprintf(str, "CAL_GYRO%u_ID", i);
336  int32_t device_id;
337  int res = param_get(param_find(str), &device_id);
338 
339  if (res != OK) {
340  PX4_ERR("Could not access param %s", str);
341  continue;
342  }
343 
344  if ((uint32_t)device_id != m_id.dev_id) {
345  continue;
346  }
347 
348  (void)sprintf(str, "CAL_GYRO%u_XSCALE", i);
350 
351  if (res != OK) {
352  PX4_ERR("Could not access param %s", str);
353  }
354 
355  (void)sprintf(str, "CAL_GYRO%u_YSCALE", i);
357 
358  if (res != OK) {
359  PX4_ERR("Could not access param %s", str);
360  }
361 
362  (void)sprintf(str, "CAL_GYRO%u_ZSCALE", i);
364 
365  if (res != OK) {
366  PX4_ERR("Could not access param %s", str);
367  }
368 
369  (void)sprintf(str, "CAL_GYRO%u_XOFF", i);
371 
372  if (res != OK) {
373  PX4_ERR("Could not access param %s", str);
374  }
375 
376  (void)sprintf(str, "CAL_GYRO%u_YOFF", i);
378 
379  if (res != OK) {
380  PX4_ERR("Could not access param %s", str);
381  }
382 
383  (void)sprintf(str, "CAL_GYRO%u_ZOFF", i);
385 
386  if (res != OK) {
387  PX4_ERR("Could not access param %s", str);
388  }
389 
390  // We got calibration values, let's exit.
391  return;
392  }
393 
394  _gyro_calibration.x_scale = 1.0f;
395  _gyro_calibration.y_scale = 1.0f;
396  _gyro_calibration.z_scale = 1.0f;
400 }
401 
403 {
404  // TODO: replace magic number
405  for (unsigned i = 0; i < 3; ++i) {
406 
407  // TODO: remove printfs and add error counter
408 
409  char str[30];
410  (void)sprintf(str, "CAL_ACC%u_ID", i);
411  int32_t device_id;
412  int res = param_get(param_find(str), &device_id);
413 
414  if (res != OK) {
415  PX4_ERR("Could not access param %s", str);
416  continue;
417  }
418 
419  if ((uint32_t)device_id != m_id.dev_id) {
420  continue;
421  }
422 
423  (void)sprintf(str, "CAL_ACC%u_XSCALE", i);
425 
426  if (res != OK) {
427  PX4_ERR("Could not access param %s", str);
428  }
429 
430  (void)sprintf(str, "CAL_ACC%u_YSCALE", i);
432 
433  if (res != OK) {
434  PX4_ERR("Could not access param %s", str);
435  }
436 
437  (void)sprintf(str, "CAL_ACC%u_ZSCALE", i);
439 
440  if (res != OK) {
441  PX4_ERR("Could not access param %s", str);
442  }
443 
444  (void)sprintf(str, "CAL_ACC%u_XOFF", i);
446 
447  if (res != OK) {
448  PX4_ERR("Could not access param %s", str);
449  }
450 
451  (void)sprintf(str, "CAL_ACC%u_YOFF", i);
453 
454  if (res != OK) {
455  PX4_ERR("Could not access param %s", str);
456  }
457 
458  (void)sprintf(str, "CAL_ACC%u_ZOFF", i);
460 
461  if (res != OK) {
462  PX4_ERR("Could not access param %s", str);
463  }
464 
465  // We got calibration values, let's exit.
466  return;
467  }
468 
469  // Set sane default calibration values
476 }
477 
479 {
480  if (!_mag_enabled) {
481  return;
482  }
483 
484  // TODO: replace magic number
485  for (unsigned i = 0; i < 3; ++i) {
486 
487  // TODO: remove printfs and add error counter
488 
489  char str[30];
490  (void)sprintf(str, "CAL_MAG%u_ID", i);
491  int32_t device_id;
492  int res = param_get(param_find(str), &device_id);
493 
494  if (res != OK) {
495  PX4_ERR("Could not access param %s", str);
496  continue;
497  }
498 
499  if ((uint32_t)device_id != m_id.dev_id) {
500  continue;
501  }
502 
503  (void)sprintf(str, "CAL_MAG%u_XSCALE", i);
505 
506  if (res != OK) {
507  PX4_ERR("Could not access param %s", str);
508  }
509 
510  (void)sprintf(str, "CAL_MAG%u_YSCALE", i);
512 
513  if (res != OK) {
514  PX4_ERR("Could not access param %s", str);
515  }
516 
517  (void)sprintf(str, "CAL_MAG%u_ZSCALE", i);
519 
520  if (res != OK) {
521  PX4_ERR("Could not access param %s", str);
522  }
523 
524  (void)sprintf(str, "CAL_MAG%u_XOFF", i);
526 
527  if (res != OK) {
528  PX4_ERR("Could not access param %s", str);
529  }
530 
531  (void)sprintf(str, "CAL_MAG%u_YOFF", i);
533 
534  if (res != OK) {
535  PX4_ERR("Could not access param %s", str);
536  }
537 
538  (void)sprintf(str, "CAL_MAG%u_ZOFF", i);
540 
541  if (res != OK) {
542  PX4_ERR("Could not access param %s", str);
543  }
544 
545  // We got calibration values, let's exit.
546  return;
547  }
548 
549  // Set sane default calibration values
550  _mag_calibration.x_scale = 1.0f;
551  _mag_calibration.y_scale = 1.0f;
552  _mag_calibration.z_scale = 1.0f;
553  _mag_calibration.x_offset = 0.0f;
554  _mag_calibration.y_offset = 0.0f;
555  _mag_calibration.z_offset = 0.0f;
556 }
557 
558 int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data)
559 {
560  /* Check if calibration values are still up-to-date. */
561  bool updated;
562  orb_check(_param_update_sub, &updated);
563 
564  if (updated) {
565  parameter_update_s parameter_update;
566  orb_copy(ORB_ID(parameter_update), _param_update_sub, &parameter_update);
567 
570  }
571 
572  matrix::Vector3f vec_integrated_unused;
573  uint32_t integral_dt_unused;
574  matrix::Vector3f accel_val(data.accel_m_s2_x,
575  data.accel_m_s2_y,
576  data.accel_m_s2_z);
577  // apply sensor rotation on the accel measurement
578  accel_val = _rotation_matrix * accel_val;
579  // Apply calibration after rotation
580  accel_val(0) = (accel_val(0) - _accel_calibration.x_offset) * _accel_calibration.x_scale;
581  accel_val(1) = (accel_val(1) - _accel_calibration.y_offset) * _accel_calibration.y_scale;
582  accel_val(2) = (accel_val(2) - _accel_calibration.z_offset) * _accel_calibration.z_scale;
583  _accel_int.put_with_interval(data.fifo_sample_interval_us,
584  accel_val,
585  vec_integrated_unused,
586  integral_dt_unused);
587  matrix::Vector3f gyro_val(data.gyro_rad_s_x,
588  data.gyro_rad_s_y,
589  data.gyro_rad_s_z);
590  // apply sensor rotation on the gyro measurement
591  gyro_val = _rotation_matrix * gyro_val;
592  // Apply calibration after rotation
593  gyro_val(0) = (gyro_val(0) - _gyro_calibration.x_offset) * _gyro_calibration.x_scale;
594  gyro_val(1) = (gyro_val(1) - _gyro_calibration.y_offset) * _gyro_calibration.y_scale;
595  gyro_val(2) = (gyro_val(2) - _gyro_calibration.z_offset) * _gyro_calibration.z_scale;
596  _gyro_int.put_with_interval(data.fifo_sample_interval_us,
597  gyro_val,
598  vec_integrated_unused,
599  integral_dt_unused);
600 
601  // The driver empties the FIFO buffer at 1kHz, however we only need to publish at 250Hz.
602  // Therefore, only publish every forth time.
603  ++_publish_count;
604 
605  if (_publish_count < 4) {
606  return 0;
607  }
608 
609  _publish_count = 0;
610 
611  // Update all the counters.
612  perf_set_count(_read_counter, data.read_counter);
613  perf_set_count(_error_counter, data.error_counter);
614  perf_set_count(_fifo_overflow_counter, data.fifo_overflow_counter);
615  perf_set_count(_fifo_corruption_counter, data.fifo_overflow_counter);
616  perf_set_count(_gyro_range_hit_counter, data.gyro_range_hit_counter);
617  perf_set_count(_accel_range_hit_counter, data.accel_range_hit_counter);
618 
620 
621  sensor_accel_s accel_report = {};
622  sensor_gyro_s gyro_report = {};
623  mag_report mag_report = {};
624 
625  accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time();
626  mag_report.timestamp = accel_report.timestamp;
627  mag_report.is_external = false;
628 
629  // TODO: get these right
630  gyro_report.scaling = -1.0f;
631  gyro_report.device_id = m_id.dev_id;
632 
633  accel_report.scaling = -1.0f;
634  accel_report.device_id = m_id.dev_id;
635 
636  if (_mag_enabled) {
637  mag_report.scaling = -1.0f;
638  mag_report.device_id = m_id.dev_id;
639  }
640 
641  // TODO: remove these (or get the values)
642  gyro_report.x_raw = 0;
643  gyro_report.y_raw = 0;
644  gyro_report.z_raw = 0;
645 
646  accel_report.x_raw = 0;
647  accel_report.y_raw = 0;
648  accel_report.z_raw = 0;
649 
650  if (_mag_enabled) {
651  mag_report.x_raw = 0;
652  mag_report.y_raw = 0;
653  mag_report.z_raw = 0;
654  }
655 
656  matrix::Vector3f gyro_val_filt;
657  matrix::Vector3f accel_val_filt;
658 
659  // Read and reset.
660  matrix::Vector3f gyro_val_integ = _gyro_int.get_and_filtered(true, gyro_report.integral_dt, gyro_val_filt);
661  matrix::Vector3f accel_val_integ = _accel_int.get_and_filtered(true, accel_report.integral_dt, accel_val_filt);
662 
663  // Use the filtered (by integration) values to get smoother / less noisy data.
664  gyro_report.x = gyro_val_filt(0);
665  gyro_report.y = gyro_val_filt(1);
666  gyro_report.z = gyro_val_filt(2);
667 
668  accel_report.x = accel_val_filt(0);
669  accel_report.y = accel_val_filt(1);
670  accel_report.z = accel_val_filt(2);
671 
672  if (_mag_enabled) {
673  matrix::Vector3f mag_val(data.mag_ga_x,
674  data.mag_ga_y,
675  data.mag_ga_z);
676  mag_val = _rotation_matrix * mag_val;
677  mag_val(0) = (mag_val(0) - _mag_calibration.x_offset) * _mag_calibration.x_scale;
678  mag_val(1) = (mag_val(1) - _mag_calibration.y_offset) * _mag_calibration.y_scale;
679  mag_val(2) = (mag_val(2) - _mag_calibration.z_offset) * _mag_calibration.z_scale;
680 
681  mag_report.x = mag_val(0);
682  mag_report.y = mag_val(1);
683  mag_report.z = mag_val(2);
684  }
685 
686  gyro_report.x_integral = gyro_val_integ(0);
687  gyro_report.y_integral = gyro_val_integ(1);
688  gyro_report.z_integral = gyro_val_integ(2);
689 
690  accel_report.x_integral = accel_val_integ(0);
691  accel_report.y_integral = accel_val_integ(1);
692  accel_report.z_integral = accel_val_integ(2);
693 
694  // TODO: when is this ever blocked?
695  if (!(m_pub_blocked)) {
696 
697 
698  if (_gyro_topic != nullptr) {
699  orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report);
700  }
701 
702  if (_accel_topic != nullptr) {
703  orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
704  }
705 
706  if (_mag_topic != nullptr) {
707  orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
708  }
709 
710  // Report if there are high vibrations, every 10 times it happens.
711  const bool threshold_reached = (data.accel_range_hit_counter - _last_accel_range_hit_count > 10);
712 
713  // Report every 5s.
714  const bool due_to_report = (hrt_elapsed_time(&_last_accel_range_hit_time) > 5000000);
715 
716  if (threshold_reached && due_to_report) {
718  "High accelerations, range exceeded %llu times",
719  data.accel_range_hit_counter);
720 
722  _last_accel_range_hit_count = data.accel_range_hit_counter;
723  }
724  }
725 
727 
728  // TODO: check the return codes of this function
729  return 0;
730 };
731 
732 
734 {
735 
737 
738 int start(bool mag_enabled, enum Rotation rotation);
739 int stop();
740 int info();
741 void usage();
742 
743 int start(bool mag_enabled, enum Rotation rotation)
744 {
745  g_dev = new DfLsm9ds1Wrapper(mag_enabled, rotation);
746 
747  if (g_dev == nullptr) {
748  PX4_ERR("failed instantiating DfLsm9ds1Wrapper object");
749  return -1;
750  }
751 
752  int ret = g_dev->start();
753 
754  if (ret != 0) {
755  PX4_ERR("DfLsm9ds1Wrapper start failed");
756  return ret;
757  }
758 
759  // Open the IMU sensor
760  DevHandle h;
761  DevMgr::getHandle(IMU_DEVICE_ACC_GYRO, h);
762 
763  if (!h.isValid()) {
764  DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
765  IMU_DEVICE_ACC_GYRO, h.getError());
766  return -1;
767  }
768 
769  DevMgr::releaseHandle(h);
770 
771  DevMgr::getHandle(IMU_DEVICE_MAG, h);
772 
773  if (!h.isValid()) {
774  DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
775  IMU_DEVICE_MAG, h.getError());
776  return -1;
777  }
778 
779  DevMgr::releaseHandle(h);
780  return 0;
781 }
782 
783 int stop()
784 {
785  if (g_dev == nullptr) {
786  PX4_ERR("driver not running");
787  return 1;
788  }
789 
790  int ret = g_dev->stop();
791 
792  if (ret != 0) {
793  PX4_ERR("driver could not be stopped");
794  return ret;
795  }
796 
797  delete g_dev;
798  g_dev = nullptr;
799  return 0;
800 }
801 
802 /**
803  * Print a little info about the driver.
804  */
805 int
807 {
808  if (g_dev == nullptr) {
809  PX4_ERR("driver not running");
810  return 1;
811  }
812 
813  PX4_INFO("state @ %p", g_dev);
814  g_dev->info();
815 
816  return 0;
817 }
818 
819 void
821 {
822  PX4_INFO("Usage: df_lsm9ds1_wrapper 'start', 'info', 'stop', 'start_without_mag'");
823  PX4_INFO("options:");
824  PX4_INFO(" -R rotation");
825 }
826 
827 } // namespace df_lsm9ds1_wrapper
828 
829 
830 int
831 df_lsm9ds1_wrapper_main(int argc, char *argv[])
832 {
833  int ch;
834  enum Rotation rotation = ROTATION_NONE;
835  int ret = 0;
836  int myoptind = 1;
837  const char *myoptarg = NULL;
838 
839  /* jump over start/off/etc and look at options first */
840  while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
841  switch (ch) {
842  case 'R':
843  rotation = (enum Rotation)atoi(myoptarg);
844  break;
845 
846  default:
848  return 0;
849  }
850  }
851 
852  if (argc <= 1) {
854  return 1;
855  }
856 
857  const char *verb = argv[myoptind];
858 
859  if (!strcmp(verb, "start_without_mag")) {
860  ret = df_lsm9ds1_wrapper::start(false, rotation);
861  }
862 
863  else if (!strcmp(verb, "start")) {
864  ret = df_lsm9ds1_wrapper::start(true, rotation);
865  }
866 
867  else if (!strcmp(verb, "stop")) {
868  ret = df_lsm9ds1_wrapper::stop();
869  }
870 
871  else if (!strcmp(verb, "info")) {
872  ret = df_lsm9ds1_wrapper::info();
873  }
874 
875  else {
877  return 1;
878  }
879 
880  return ret;
881 }
uint64_t _last_accel_range_hit_count
uint32_t integral_dt
Definition: sensor_accel.h:59
int stop()
Stop the driver.
Accelerometer driver interface.
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
Definition: uORB.cpp:90
matrix::Dcmf _rotation_matrix
uint64_t timestamp
Definition: sensor_accel.h:53
Gyroscope driver interface.
int info()
Print a little info about the driver.
measure the time elapsed performing an event
Definition: perf_counter.h:56
struct DfLsm9ds1Wrapper::mag_calibration_s _mag_calibration
perf_counter_t _fifo_overflow_counter
__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
void usage(const char *reason)
Print the correct usage.
Definition: Commander.cpp:4238
Definition: I2C.hpp:51
perf_counter_t _error_counter
float x_integral
Definition: sensor_gyro.h:60
static void stop()
Definition: dataman.cpp:1491
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.
High-resolution timer with callouts and timekeeping.
perf_counter_t _fifo_corruption_counter
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
Global flash based parameter store.
int orb_subscribe(const struct orb_metadata *meta)
Definition: uORB.cpp:75
float z_integral
Definition: sensor_gyro.h:62
perf_counter_t _gyro_range_hit_counter
#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.
void init()
Activates/configures the hardware registers.
int stop()
Stop automatic measurement.
#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
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
int _publish(struct imu_sensor_data &data)
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
void usage()
Prints info about the driver argument usage.
bool put_with_interval(unsigned interval_us, matrix::Vector3f &val, matrix::Vector3f &integral, uint32_t &integral_dt)
Put an item into the integral but provide an interval instead of a timestamp.
Definition: integrator.cpp:118
__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.
struct DfLsm9ds1Wrapper::gyro_calibration_s _gyro_calibration
struct DfLsm9ds1Wrapper::accel_calibration_s _accel_calibration
perf_counter_t _publish_perf
DfLsm9ds1Wrapper(bool mag_enabled, enum Rotation rotation)
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.
orb_advert_t _mavlink_log_pub
uint32_t device_id
Definition: sensor_accel.h:55
void info()
Print some debug info.
__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
hrt_abstime _last_accel_range_hit_time
__EXPORT int df_lsm9ds1_wrapper_main(int argc, char *argv[])
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority)
Definition: uORB.cpp:53
#define LSM9DS1_NEVER_AUTOPUBLISH_US
perf_counter_t _read_counter
void perf_begin(perf_counter_t handle)
Begin a performance event.
DfLsm9ds1Wrapper * g_dev
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
perf_counter_t _accel_range_hit_counter
int start()
Start automatic measurement.
int start(bool mag_enabled, enum Rotation rotation)
uint32_t integral_dt
Definition: sensor_gyro.h:59
Performance measuring tools.
#define mag_report
Definition: drv_mag.h:53