PX4 Firmware
PX4 Autopilot Software http://px4.io
mpu9x50_main.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (C) 2015 Mark Charlebois. 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 #include <stdint.h>
35 #include <fcntl.h>
36 #include <unistd.h>
37 #include <signal.h>
38 
39 #include <px4_platform_common/tasks.h>
40 
41 #include <px4_platform_common/log.h>
42 #include <px4_platform_common/getopt.h>
43 
44 // TODO-JYW:
45 // Include references to the driver framework. This will produce a duplicate definition
46 // of qurt_log. Use a #define for the qurt_log function to avoid redefinition. This code
47 // change must still be made. Document the latter change to be clear.
48 // #include <df_stubs.h>
49 // #include <SPIDevObj.hpp>
50 
51 #include <uORB/uORB.h>
53 #include <drivers/drv_accel.h>
54 #include <drivers/drv_gyro.h>
55 #include <drivers/drv_mag.h>
56 #include <drivers/drv_hrt.h>
57 
58 #ifdef __cplusplus
59 extern "C" {
60 #endif
61 #include <semaphore.h>
62 #include <mpu9x50.h>
63 #ifdef __cplusplus
64 }
65 #endif
66 
67 /** driver 'main' command */
68 extern "C" { __EXPORT int mpu9x50_main(int argc, char *argv[]); }
69 
70 #define MAX_LEN_DEV_PATH 32
71 
72 // TODO - need to load this from parameter file
73 #define SPI_INT_GPIO 65 // GPIO pin for Data Ready Interrupt
74 namespace mpu9x50
75 {
76 
77 /** SPI device path that mpu9x50 is connected to */
79 
80 /** flag indicating if mpu9x50 app is running */
81 static bool _is_running = false;
82 
83 /** flag indicating if measurement thread should exit */
84 static bool _task_should_exit = false;
85 
86 /** handle to the task main thread */
87 static px4_task_t _task_handle = -1;
88 
89 /** IMU measurement data */
90 static struct mpu9x50_data _data;
91 
92 static orb_advert_t _gyro_pub = nullptr; /**< gyro data publication */
93 static int _gyro_orb_class_instance; /**< instance handle for gyro devices */
94 static orb_advert_t _accel_pub = nullptr; /**< accelerameter data publication */
95 static int _accel_orb_class_instance; /**< instance handle for accel devices */
96 static orb_advert_t _mag_pub = nullptr; /**< compass data publication */
97 static int _mag_orb_class_instance; /**< instance handle for mag devices */
98 static int _params_sub; /**< parameter update subscription */
99 static sensor_gyro_s _gyro; /**< gyro report */
100 static sensor_accel_s _accel; /**< accel report */
101 static struct mag_report _mag; /**< mag report */
102 static struct gyro_calibration_s _gyro_sc; /**< gyro scale */
103 static struct accel_calibration_s _accel_sc; /**< accel scale */
104 static struct mag_calibration_s _mag_sc; /**< mag scale */
105 static enum gyro_lpf_e _gyro_lpf = MPU9X50_GYRO_LPF_20HZ; /**< gyro lpf enum value */
106 static enum acc_lpf_e _accel_lpf = MPU9X50_ACC_LPF_20HZ; /**< accel lpf enum value */
107 static enum gyro_sample_rate_e _imu_sample_rate = MPU9x50_SAMPLE_RATE_500HZ; /**< IMU sample rate enum */
108 
109 struct {
131 } _params_handles; /**< parameter handles */
132 
133 bool exit_mreasurement = false;
134 
135 
136 /** Print out the usage information */
137 static void usage();
138 
139 /** mpu9x50 stop measurement */
140 static void stop();
141 
142 /** task main trampoline function */
143 static void task_main_trampoline(int argc, char *argv[]);
144 
145 /** mpu9x50 measurement thread primary entry point */
146 static void task_main(int argc, char *argv[]);
147 
148 /**
149  * create and advertise all publicatoins
150  * @return true on success, false otherwise
151  */
152 static bool create_pubs();
153 
154 /** update all sensor reports with the latest sensor reading */
155 static void update_reports();
156 
157 /** publish all sensor reports */
158 static void publish_reports();
159 
160 /** update all parameters */
161 static void parameters_update();
162 
163 /** initialize all parameter handles and load the initial parameter values */
164 static void parameters_init();
165 
166 /** poll parameter update */
167 static void parameter_update_poll();
168 
169 static int64_t _isr_data_ready_timestamp = 0;
170 
171 /**
172  * MPU9x50 data ready interrupt service routine
173  * @param[out] context address of the context data provided by user whill
174  * registering the interrupt servcie routine
175  */
176 static void *data_ready_isr(void *context);
177 
178 void *data_ready_isr(void *context)
179 {
180  if (exit_mreasurement) {
181  return NULL;
182  }
183 
184  _isr_data_ready_timestamp = hrt_absolute_time();
185  // send signal to measurement thread
186  px4_task_kill(_task_handle, SIGRTMIN);
187 
188  return NULL;
189 }
190 
192 {
193  bool updated;
194 
195  /* Check if parameters have changed */
196  orb_check(_params_sub, &updated);
197 
198  if (updated) {
199  struct parameter_update_s param_update;
200  orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
202  }
203 }
204 
206 {
207  PX4_DEBUG("mpu9x50_parameters_update");
208  float v;
209  int v_int;
210 
211  // accel params
212  if (param_get(_params_handles.accel_x_offset, &v) == 0) {
213  _accel_sc.x_offset = v;
214  PX4_DEBUG("mpu9x50_parameters_update accel_x_offset %f", v);
215  }
216 
217  if (param_get(_params_handles.accel_x_scale, &v) == 0) {
218  _accel_sc.x_scale = v;
219  PX4_DEBUG("mpu9x50_parameters_update accel_x_scale %f", v);
220  }
221 
222  if (param_get(_params_handles.accel_y_offset, &v) == 0) {
223  _accel_sc.y_offset = v;
224  PX4_DEBUG("mpu9x50_parameters_update accel_y_offset %f", v);
225  }
226 
227  if (param_get(_params_handles.accel_y_scale, &v) == 0) {
228  _accel_sc.y_scale = v;
229  PX4_DEBUG("mpu9x50_parameters_update accel_y_scale %f", v);
230  }
231 
232  if (param_get(_params_handles.accel_z_offset, &v) == 0) {
233  _accel_sc.z_offset = v;
234  PX4_DEBUG("mpu9x50_parameters_update accel_z_offset %f", v);
235  }
236 
237  if (param_get(_params_handles.accel_z_scale, &v) == 0) {
238  _accel_sc.z_scale = v;
239  PX4_DEBUG("mpu9x50_parameters_update accel_z_scale %f", v);
240  }
241 
242  // gyro params
243  if (param_get(_params_handles.gyro_x_offset, &v) == 0) {
244  _gyro_sc.x_offset = v;
245  PX4_DEBUG("mpu9x50_parameters_update gyro_x_offset %f", v);
246  }
247 
248  if (param_get(_params_handles.gyro_x_scale, &v) == 0) {
249  _gyro_sc.x_scale = v;
250  PX4_DEBUG("mpu9x50_parameters_update gyro_x_scale %f", v);
251  }
252 
253  if (param_get(_params_handles.gyro_y_offset, &v) == 0) {
254  _gyro_sc.y_offset = v;
255  PX4_DEBUG("mpu9x50_parameters_update gyro_y_offset %f", v);
256  }
257 
258  if (param_get(_params_handles.gyro_y_scale, &v) == 0) {
259  _gyro_sc.y_scale = v;
260  PX4_DEBUG("mpu9x50_parameters_update gyro_y_scale %f", v);
261  }
262 
263  if (param_get(_params_handles.gyro_z_offset, &v) == 0) {
264  _gyro_sc.z_offset = v;
265  PX4_DEBUG("mpu9x50_parameters_update gyro_z_offset %f", v);
266  }
267 
268  if (param_get(_params_handles.gyro_z_scale, &v) == 0) {
269  _gyro_sc.z_scale = v;
270  PX4_DEBUG("mpu9x50_parameters_update gyro_z_scale %f", v);
271  }
272 
273  // mag params
274  if (param_get(_params_handles.mag_x_offset, &v) == 0) {
275  _mag_sc.x_offset = v;
276  PX4_DEBUG("mpu9x50_parameters_update mag_x_offset %f", v);
277  }
278 
279  if (param_get(_params_handles.mag_x_scale, &v) == 0) {
280  _mag_sc.x_scale = v;
281  PX4_DEBUG("mpu9x50_parameters_update mag_x_scale %f", v);
282  }
283 
284  if (param_get(_params_handles.mag_y_offset, &v) == 0) {
285  _mag_sc.y_offset = v;
286  PX4_DEBUG("mpu9x50_parameters_update mag_y_offset %f", v);
287  }
288 
289  if (param_get(_params_handles.mag_y_scale, &v) == 0) {
290  _mag_sc.y_scale = v;
291  PX4_DEBUG("mpu9x50_parameters_update mag_y_scale %f", v);
292  }
293 
294  if (param_get(_params_handles.mag_z_offset, &v) == 0) {
295  _mag_sc.z_offset = v;
296  PX4_DEBUG("mpu9x50_parameters_update mag_z_offset %f", v);
297  }
298 
299  if (param_get(_params_handles.mag_z_scale, &v) == 0) {
300  _mag_sc.z_scale = v;
301  PX4_DEBUG("mpu9x50_parameters_update mag_z_scale %f", v);
302  }
303 
304  // LPF params
305  if (param_get(_params_handles.gyro_lpf_enum, &v_int) == 0) {
306  if (v_int >= NUM_MPU9X50_GYRO_LPF) {
307  PX4_WARN("invalid gyro_lpf_enum %d use default %d", v_int, _gyro_lpf);
308 
309  } else {
310  _gyro_lpf = (enum gyro_lpf_e)v_int;
311  PX4_DEBUG("mpu9x50_parameters_update gyro_lpf_enum %d", _gyro_lpf);
312  }
313  }
314 
315  if (param_get(_params_handles.accel_lpf_enum, &v_int) == 0) {
316  if (v_int >= NUM_MPU9X50_ACC_LPF) {
317  PX4_WARN("invalid accel_lpf_enum %d use default %d", v_int, _accel_lpf);
318 
319  } else {
320  _accel_lpf = (enum acc_lpf_e)v_int;
321  PX4_DEBUG("mpu9x50_parameters_update accel_lpf_enum %d", _accel_lpf);
322  }
323  }
324 
325  if (param_get(_params_handles.imu_sample_rate_enum, &v_int) == 0) {
326  if (v_int >= NUM_MPU9X50_SAMPLE_RATE) {
327  PX4_WARN("invalid imu_sample_rate %d use default %d", v_int, _imu_sample_rate);
328 
329  } else {
330  _imu_sample_rate = (enum gyro_sample_rate_e)v_int;
331  PX4_DEBUG("mpu9x50_parameters_update imu_sample_rate %d", _imu_sample_rate);
332  }
333  }
334 }
335 
337 {
338  _params_handles.accel_x_offset = param_find("CAL_ACC0_XOFF");
339  _params_handles.accel_x_scale = param_find("CAL_ACC0_XSCALE");
340  _params_handles.accel_y_offset = param_find("CAL_ACC0_YOFF");
341  _params_handles.accel_y_scale = param_find("CAL_ACC0_YSCALE");
342  _params_handles.accel_z_offset = param_find("CAL_ACC0_ZOFF");
343  _params_handles.accel_z_scale = param_find("CAL_ACC0_ZSCALE");
344 
345  _params_handles.gyro_x_offset = param_find("CAL_GYRO0_XOFF");
346  _params_handles.gyro_x_scale = param_find("CAL_GYRO0_XSCALE");
347  _params_handles.gyro_y_offset = param_find("CAL_GYRO0_YOFF");
348  _params_handles.gyro_y_scale = param_find("CAL_GYRO0_YSCALE");
349  _params_handles.gyro_z_offset = param_find("CAL_GYRO0_ZOFF");
350  _params_handles.gyro_z_scale = param_find("CAL_GYRO0_ZSCALE");
351 
352  _params_handles.mag_x_offset = param_find("CAL_MAG0_XOFF");
353  _params_handles.mag_x_scale = param_find("CAL_MAG0_XSCALE");
354  _params_handles.mag_y_offset = param_find("CAL_MAG0_YOFF");
355  _params_handles.mag_y_scale = param_find("CAL_MAG0_YSCALE");
356  _params_handles.mag_z_offset = param_find("CAL_MAG0_ZOFF");
357  _params_handles.mag_z_scale = param_find("CAL_MAG0_ZSCALE");
358 
359  _params_handles.gyro_lpf_enum = param_find("MPU_GYRO_LPF_ENM");
360  _params_handles.accel_lpf_enum = param_find("MPU_ACC_LPF_ENM");
361 
362  _params_handles.imu_sample_rate_enum = param_find("MPU_SAMPLE_R_ENM");
363 
365 }
366 
368 {
369  // initialize the reports
370  memset(&_gyro, 0, sizeof(sensor_gyro_s));
371  memset(&_accel, 0, sizeof(sensor_accel_s));
372  memset(&_mag, 0, sizeof(struct mag_report));
373 
374  _gyro_pub = orb_advertise_multi(ORB_ID(sensor_gyro), &_gyro,
375  &_gyro_orb_class_instance, ORB_PRIO_HIGH - 1);
376 
377  if (_gyro_pub == nullptr) {
378  PX4_ERR("sensor_gyro advert fail");
379  return false;
380  }
381 
382  _accel_pub = orb_advertise_multi(ORB_ID(sensor_accel), &_accel,
383  &_accel_orb_class_instance, ORB_PRIO_HIGH - 1);
384 
385  if (_accel_pub == nullptr) {
386  PX4_ERR("sensor_accel advert fail");
387  return false;
388  }
389 
390  _mag_pub = orb_advertise_multi(ORB_ID(sensor_mag), &_mag,
391  &_mag_orb_class_instance, ORB_PRIO_HIGH - 1);
392 
393  if (_mag_pub == nullptr) {
394  PX4_ERR("sensor_mag advert fail");
395  return false;
396  }
397 
398  return true;
399 }
400 
402 {
403  _gyro.timestamp = _data.timestamp;
404  _gyro.x = ((_data.gyro_raw[0] * _data.gyro_scaling) - _gyro_sc.x_offset) * _gyro_sc.x_scale;
405  _gyro.y = ((_data.gyro_raw[1] * _data.gyro_scaling) - _gyro_sc.y_offset) * _gyro_sc.y_scale;
406  _gyro.z = ((_data.gyro_raw[2] * _data.gyro_scaling) - _gyro_sc.z_offset) * _gyro_sc.z_scale;
407  _gyro.temperature = _data.temperature;
408  _gyro.scaling = _data.gyro_scaling;
409  _gyro.x_raw = _data.gyro_raw[0];
410  _gyro.y_raw = _data.gyro_raw[1];
411  _gyro.z_raw = _data.gyro_raw[2];
412 
413  _accel.timestamp = _data.timestamp;
414  _accel.x = ((_data.accel_raw[0] * _data.accel_scaling) - _accel_sc.x_offset) * _accel_sc.x_scale;
415  _accel.y = ((_data.accel_raw[1] * _data.accel_scaling) - _accel_sc.y_offset) * _accel_sc.y_scale;
416  _accel.z = ((_data.accel_raw[2] * _data.accel_scaling) - _accel_sc.z_offset) * _accel_sc.z_scale;
417  _accel.temperature = _data.temperature;
418  _accel.scaling = _data.accel_scaling;
419  _accel.x_raw = _data.accel_raw[0];
420  _accel.y_raw = _data.accel_raw[1];
421  _accel.z_raw = _data.accel_raw[2];
422 
423  if (_data.mag_data_ready) {
424  _mag.timestamp = _data.timestamp;
425  _mag.x = ((_data.mag_raw[0] * _data.mag_scaling) - _mag_sc.x_offset) * _mag_sc.x_scale;
426  _mag.y = ((_data.mag_raw[1] * _data.mag_scaling) - _mag_sc.y_offset) * _mag_sc.y_scale;
427  _mag.z = ((_data.mag_raw[2] * _data.mag_scaling) - _mag_sc.z_offset) * _mag_sc.z_scale;
428  _mag.scaling = _data.mag_scaling;
429  _mag.temperature = _data.temperature;
430  _mag.x_raw = _data.mag_raw[0];
431  _mag.y_raw = _data.mag_raw[1];
432  _mag.z_raw = _data.mag_raw[2];
433  }
434 }
435 
437 {
438  if (orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &_gyro) != OK) {
439  PX4_WARN("failed to publish gyro report");
440 
441  } else {
442  //PX4_DEBUG("MPU_GYRO_RAW: %d %d %d", _gyro.x_raw, _gyro.y_raw, _gyro.z_raw)
443  //PX4_DEBUG("MPU_GYRO: %f %f %f", _gyro.x, _gyro.y, _gyro.z)
444  }
445 
446  if (orb_publish(ORB_ID(sensor_accel), _accel_pub, &_accel) != OK) {
447  PX4_WARN("failed to publish accel report");
448 
449  } else {
450  //PX4_DEBUG("MPU_ACCEL_RAW: %d %d %d", _accel.x_raw, _accel.y_raw, _accel.z_raw)
451  //PX4_DEBUG("MPU_ACCEL: %f %f %f", _accel.x, _accel.y, _accel.z)
452  }
453 
454  if (_data.mag_data_ready) {
455  if (orb_publish(ORB_ID(sensor_mag), _mag_pub, &_mag) != OK) {
456  PX4_WARN("failed to publish mag report");
457 
458  } else {
459  //PX4_DEBUG("MPU_MAG_RAW: %d %d %d", _mag.x_raw, _mag.y_raw, _mag.z_raw)
460  //PX4_DEBUG("MPU_MAG: %f %f %f", _mag.x, _mag.y, _mag.z)
461  }
462  }
463 }
464 
465 void task_main(int argc, char *argv[])
466 {
467  PX4_WARN("enter mpu9x50 task_main");
468 
469  sigset_t set;
470  int sig = 0;
471  int rv;
472  exit_mreasurement = false;
473 
474  parameters_init();
475 
476  // create the mpu9x50 default configuration structure
477  struct mpu9x50_config config = {
478  .gyro_lpf = _gyro_lpf,
479  .acc_lpf = _accel_lpf,
480  .gyro_fsr = MPU9X50_GYRO_FSR_500DPS,
481  .acc_fsr = MPU9X50_ACC_FSR_4G,
482  .gyro_sample_rate = _imu_sample_rate,
483  .compass_enabled = true,
484  .compass_sample_rate = MPU9x50_COMPASS_SAMPLE_RATE_100HZ,
485  .spi_dev_path = _device,
486  };
487 
488  // TODO-JYW:
489  // manually register with the DriverFramework to allow this driver to
490  // be found by other modules
491 // DriverFramework::StubSensor<DriverFramework::SPIDevObj> stub_sensor("mpu9x50", "/dev/accel0", "/dev/accel");
492 
493  if (mpu9x50_initialize(&config) != 0) {
494  PX4_WARN("error initializing mpu9x50. Quit!");
495  goto exit;
496  }
497 
498  if (mpu9x50_register_interrupt(SPI_INT_GPIO, &mpu9x50::data_ready_isr, NULL)
499  //if (mpu9x50_register_interrupt(SPI_INT_GPIO, &mpu9x50::data_ready_isr, NULL)
500 
501  != 0) {
502  PX4_WARN("error registering data ready interrupt. Quit!");
503  goto exit;
504  }
505 
506  // create all uorb publications
507  if (!create_pubs()) {
508  goto exit;
509  }
510 
511  // subscribe to parameter_update topic
512  _params_sub = orb_subscribe(ORB_ID(parameter_update));
513 
514  // initialize signal
515  sigemptyset(&set);
516  sigaddset(&set, SIGRTMIN);
517 
518  while (!_task_should_exit) {
519  // wait on signal
520  rv = sigwait(&set, &sig);
521 
522  // check if we are waken up by the proper signal
523  if (rv != 0 || sig != SIGRTMIN) {
524  PX4_WARN("mpu9x50 sigwait failed rv %d sig %d", rv, sig);
525  continue;
526  }
527 
528  // read new IMU data and store the results in data
529  if (mpu9x50_get_data(&_data) != 0) {
530  PX4_WARN("mpu9x50_get_data() failed");
531  continue;
532  }
533 
534  // since the context switch takes long time, override the timestamp provided by mpu9x50_get_data()
535  // with the ts of isr.
536  // Note: This is ok for MPU sample rate of < 1000; Higer than 1000 Sample rates will be an issue
537  // as the data is not consistent.
538  _data.timestamp = _isr_data_ready_timestamp;
539 
540  // poll parameter update
542 
543  // data is ready
544  update_reports();
545 
546  // publish all sensor reports
547  publish_reports();
548 
549  }
550 
551  exit_mreasurement = true;
552 
553 exit:
554  PX4_WARN("closing mpu9x50");
555  mpu9x50_close();
556 }
557 
558 /** mpu9x50 main entrance */
559 void task_main_trampoline(int argc, char *argv[])
560 {
561  PX4_WARN("task_main_trampoline");
562  task_main(argc, argv);
563 }
564 
565 void start()
566 {
567  ASSERT(_task_handle == -1);
568 
569  /* start the task */
570  _task_handle = px4_task_spawn_cmd("mpu9x50_main",
571  SCHED_DEFAULT,
572  SCHED_PRIORITY_MAX,
573  1500,
574  (px4_main_t)&task_main_trampoline,
575  nullptr);
576 
577  if (_task_handle < 0) {
578  warn("mpu9x50_main task start failed");
579  return;
580  }
581 
582  _is_running = true;
583 }
584 
585 void stop()
586 {
587  // TODO - set thread exit signal to terminate the task main thread
588 
589  _is_running = false;
590  _task_handle = -1;
591 }
592 
593 void usage()
594 {
595  PX4_WARN("missing command: try 'start', 'stop', 'status'");
596  PX4_WARN("options:");
597  PX4_WARN(" -D device");
598 }
599 
600 }; // namespace mpu9x50
601 
602 
603 int mpu9x50_main(int argc, char *argv[])
604 {
605  const char *device = NULL;
606  int ch;
607  int myoptind = 1;
608  const char *myoptarg = NULL;
609 
610  while ((ch = px4_getopt(argc, argv, "D:", &myoptind, &myoptarg)) != EOF) {
611  switch (ch) {
612  case 'D':
613  device = myoptarg;
614  break;
615 
616  default:
617  mpu9x50::usage();
618  return 1;
619  }
620  }
621 
622  // Check on required arguments
623  if (device == NULL || strlen(device) == 0) {
624  mpu9x50::usage();
625  return 1;
626  }
627 
629  strncpy(mpu9x50::_device, device, strlen(device));
630 
631  const char *verb = argv[myoptind];
632 
633  /*
634  * Start/load the driver.
635  */
636  if (!strcmp(verb, "start")) {
637  if (mpu9x50::_is_running) {
638  PX4_WARN("mpu9x50 already running");
639  return 1;
640  }
641 
642  mpu9x50::start();
643 
644  } else if (!strcmp(verb, "stop")) {
645  if (mpu9x50::_is_running) {
646  PX4_WARN("mpu9x50 is not running");
647  return 1;
648  }
649 
650  mpu9x50::stop();
651 
652  } else if (!strcmp(verb, "status")) {
653  PX4_WARN("mpu9x50 is %s", mpu9x50::_is_running ? "running" : "stopped");
654  return 0;
655 
656  } else {
657  mpu9x50::usage();
658  return 1;
659  }
660 
661  return 0;
662 }
param_t mag_y_offset
Accelerometer driver interface.
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
Definition: uORB.cpp:90
uint64_t timestamp
Definition: sensor_accel.h:53
param_t gyro_z_offset
Gyroscope driver interface.
static char _device[MAX_LEN_DEV_PATH]
SPI device path that mpu9x50 is connected to.
static int _params_sub
parameter update subscription
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
Definition: parameters.cpp:589
API for the uORB lightweight object broker.
gyro scaling factors; Vout = (Vin * Vscale) + Voffset
Definition: drv_gyro.h:54
static void parameter_update_poll()
poll parameter update
int16_t x_raw
Definition: sensor_gyro.h:65
static int _mag_orb_class_instance
instance handle for mag devices
param_t gyro_z_scale
param_t gyro_y_scale
Definition: I2C.hpp:51
param_t gyro_x_offset
mag scaling factors; Vout = (Vin * Vscale) + Voffset
Definition: drv_mag.h:56
static int _gyro_orb_class_instance
instance handle for gyro devices
int16_t z_raw
Definition: sensor_gyro.h:67
int16_t y_raw
Definition: sensor_gyro.h:66
param_t accel_z_offset
Namespace encapsulating all device framework classes, functions and data.
Definition: CDev.cpp:47
param_t mag_y_scale
static orb_advert_t _gyro_pub
gyro data publication
High-resolution timer with callouts and timekeeping.
static bool _task_should_exit
flag indicating if measurement thread should exit
static int _accel_orb_class_instance
instance handle for accel devices
accel scaling factors; Vout = Vscale * (Vin + Voffset)
Definition: drv_accel.h:54
void start()
Attempt to start driver on all available I2C busses.
float x_offset
Definition: drv_mag.h:57
param_t accel_z_scale
static enum gyro_sample_rate_e _imu_sample_rate
IMU sample rate enum.
int orb_subscribe(const struct orb_metadata *meta)
Definition: uORB.cpp:75
static void stop()
mpu9x50 stop measurement
static void task_main(int argc, char *argv[])
mpu9x50 measurement thread primary entry point
#define SPI_INT_GPIO
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
static bool create_pubs()
create and advertise all publicatoins
param_t gyro_x_scale
static void parameters_update()
update all parameters
static struct gyro_calibration_s _gyro_sc
gyro scale
static sensor_accel_s _accel
accel report
#define MAX_LEN_DEV_PATH
static void task_main_trampoline(int argc, char *argv[])
task main trampoline function
static struct mag_report _mag
mag report
param_t gyro_y_offset
float y_offset
Definition: drv_mag.h:59
param_t mag_x_scale
static void * data_ready_isr(void *context)
MPU9x50 data ready interrupt service routine.
static sensor_gyro_s _gyro
gyro report
static enum gyro_lpf_e _gyro_lpf
gyro lpf enum value
static int64_t _isr_data_ready_timestamp
param_t gyro_lpf_enum
uint64_t timestamp
Definition: sensor_gyro.h:53
static void parameters_init()
initialize all parameter handles and load the initial parameter values
param_t accel_y_scale
param_t mag_z_offset
__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
float z_offset
Definition: drv_mag.h:61
static void usage()
Print out the usage information.
param_t accel_lpf_enum
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
Definition: parameters.cpp:370
static bool _is_running
flag indicating if mpu9x50 app is running
int orb_check(int handle, bool *updated)
Definition: uORB.cpp:95
float temperature
Definition: sensor_gyro.h:63
param_t mag_x_offset
static void update_reports()
update all sensor reports with the latest sensor reading
param_t accel_x_offset
param_t accel_x_scale
static struct mag_calibration_s _mag_sc
mag scale
param_t accel_y_offset
param_t imu_sample_rate_enum
#define OK
Definition: uavcan_main.cpp:71
struct mpu9x50::@16 _params_handles
parameter handles
bool exit_mreasurement
static px4_task_t _task_handle
handle to the task main thread
static orb_advert_t _mag_pub
compass data publication
static struct mpu9x50_data _data
IMU measurement data.
__EXPORT int mpu9x50_main(int argc, char *argv[])
driver &#39;main&#39; command
param_t mag_z_scale
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority)
Definition: uORB.cpp:53
static enum acc_lpf_e _accel_lpf
accel lpf enum value
#define warn(...)
Definition: err.h:94
static orb_advert_t _accel_pub
accelerameter data publication
__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
static struct accel_calibration_s _accel_sc
accel scale
static void publish_reports()
publish all sensor reports
#define mag_report
Definition: drv_mag.h:53