PX4 Firmware
PX4 Autopilot Software http://px4.io
l3gd20.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2012-2019 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 l3gd20.cpp
36  * Driver for the ST L3GD20 MEMS and L3GD20H mems gyros connected via SPI.
37  *
38  * Note: With the exception of the self-test feature, the ST L3G4200D is
39  * also supported by this driver.
40  */
41 
42 #include <drivers/device/spi.h>
45 #include <perf/perf_counter.h>
46 #include <px4_platform_common/getopt.h>
47 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
48 
49 #define L3GD20_DEVICE_PATH "/dev/l3gd20"
50 
51 /* Orientation on board */
52 #define SENSOR_BOARD_ROTATION_000_DEG 0
53 #define SENSOR_BOARD_ROTATION_090_DEG 1
54 #define SENSOR_BOARD_ROTATION_180_DEG 2
55 #define SENSOR_BOARD_ROTATION_270_DEG 3
56 
57 /* SPI protocol address bits */
58 #define DIR_READ (1<<7)
59 #define DIR_WRITE (0<<7)
60 #define ADDR_INCREMENT (1<<6)
61 
62 /* register addresses */
63 #define ADDR_WHO_AM_I 0x0F
64 #define WHO_I_AM_H 0xD7
65 #define WHO_I_AM 0xD4
66 #define WHO_I_AM_L3G4200D 0xD3 /* for L3G4200D */
67 
68 #define ADDR_CTRL_REG1 0x20
69 #define REG1_RATE_LP_MASK 0xF0 /* Mask to guard partial register update */
70 
71 /* keep lowpass low to avoid noise issues */
72 #define RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4))
73 #define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (0<<5) | (1<<4))
74 #define RATE_190HZ_LP_50HZ ((0<<7) | (1<<6) | (1<<5) | (0<<4))
75 #define RATE_190HZ_LP_70HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4))
76 #define RATE_380HZ_LP_20HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4))
77 #define RATE_380HZ_LP_25HZ ((1<<7) | (0<<6) | (0<<5) | (1<<4))
78 #define RATE_380HZ_LP_50HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4))
79 #define RATE_380HZ_LP_100HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4))
80 #define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (0<<5) | (0<<4))
81 #define RATE_760HZ_LP_35HZ ((1<<7) | (1<<6) | (0<<5) | (1<<4))
82 #define RATE_760HZ_LP_50HZ ((1<<7) | (1<<6) | (1<<5) | (0<<4))
83 #define RATE_760HZ_LP_100HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4))
84 
85 #define ADDR_CTRL_REG2 0x21
86 #define ADDR_CTRL_REG3 0x22
87 #define ADDR_CTRL_REG4 0x23
88 #define REG4_RANGE_MASK 0x30 /* Mask to guard partial register update */
89 #define RANGE_250DPS (0<<4)
90 #define RANGE_500DPS (1<<4)
91 #define RANGE_2000DPS (3<<4)
92 
93 #define ADDR_CTRL_REG5 0x24
94 #define ADDR_REFERENCE 0x25
95 #define ADDR_OUT_TEMP 0x26
96 #define ADDR_STATUS_REG 0x27
97 #define ADDR_OUT_X_L 0x28
98 #define ADDR_OUT_X_H 0x29
99 #define ADDR_OUT_Y_L 0x2A
100 #define ADDR_OUT_Y_H 0x2B
101 #define ADDR_OUT_Z_L 0x2C
102 #define ADDR_OUT_Z_H 0x2D
103 #define ADDR_FIFO_CTRL_REG 0x2E
104 #define ADDR_FIFO_SRC_REG 0x2F
105 #define ADDR_INT1_CFG 0x30
106 #define ADDR_INT1_SRC 0x31
107 #define ADDR_INT1_TSH_XH 0x32
108 #define ADDR_INT1_TSH_XL 0x33
109 #define ADDR_INT1_TSH_YH 0x34
110 #define ADDR_INT1_TSH_YL 0x35
111 #define ADDR_INT1_TSH_ZH 0x36
112 #define ADDR_INT1_TSH_ZL 0x37
113 #define ADDR_INT1_DURATION 0x38
114 #define ADDR_LOW_ODR 0x39
115 
116 
117 /* Internal configuration values */
118 #define REG1_POWER_NORMAL (1<<3)
119 #define REG1_Z_ENABLE (1<<2)
120 #define REG1_Y_ENABLE (1<<1)
121 #define REG1_X_ENABLE (1<<0)
122 
123 #define REG4_BDU (1<<7)
124 #define REG4_BLE (1<<6)
125 //#define REG4_SPI_3WIRE (1<<0)
126 
127 #define REG5_FIFO_ENABLE (1<<6)
128 #define REG5_REBOOT_MEMORY (1<<7)
129 
130 #define STATUS_ZYXOR (1<<7)
131 #define STATUS_ZOR (1<<6)
132 #define STATUS_YOR (1<<5)
133 #define STATUS_XOR (1<<4)
134 #define STATUS_ZYXDA (1<<3)
135 #define STATUS_ZDA (1<<2)
136 #define STATUS_YDA (1<<1)
137 #define STATUS_XDA (1<<0)
138 
139 #define FIFO_CTRL_BYPASS_MODE (0<<5)
140 #define FIFO_CTRL_FIFO_MODE (1<<5)
141 #define FIFO_CTRL_STREAM_MODE (1<<6)
142 #define FIFO_CTRL_STREAM_TO_FIFO_MODE (3<<5)
143 #define FIFO_CTRL_BYPASS_TO_STREAM_MODE (1<<7)
144 
145 #define L3GD20_DEFAULT_RATE 760
146 #define L3G4200D_DEFAULT_RATE 800
147 #define L3GD20_MAX_OUTPUT_RATE 280
148 #define L3GD20_DEFAULT_RANGE_DPS 2000
149 #define L3GD20_DEFAULT_FILTER_FREQ 30
150 #define L3GD20_TEMP_OFFSET_CELSIUS 40
151 
152 #define L3GD20_MAX_OFFSET 0.45f /**< max offset: 25 degrees/s */
153 
154 #ifndef SENSOR_BOARD_ROTATION_DEFAULT
155 #define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG
156 #endif
157 
158 /*
159  we set the timer interrupt to run a bit faster than the desired
160  sample rate and then throw away duplicates using the data ready bit.
161  This time reduction is enough to cope with worst case timing jitter
162  due to other timers
163  */
164 #define L3GD20_TIMER_REDUCTION 600
165 
166 extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); }
167 
168 class L3GD20 : public device::SPI, public px4::ScheduledWorkItem
169 {
170 public:
171  L3GD20(int bus, const char *path, uint32_t device, enum Rotation rotation);
172  virtual ~L3GD20();
173 
174  virtual int init();
175 
176  /**
177  * Diagnostics - print some basic information about the driver.
178  */
179  void print_info();
180 
181  // print register dump
182  void print_registers();
183 
184  // trigger an error
185  void test_error();
186 
187 protected:
188  virtual int probe();
189 
190 private:
191 
193 
194  unsigned _current_rate{0};
196 
197  unsigned _read{0};
198 
203 
204  uint8_t _register_wait{0};
205 
206  /* true if an L3G4200D is detected */
207  bool _is_l3g4200d{false};
208 
210 
211  // this is used to support runtime checking of key
212  // configuration registers to detect SPI bus errors and sensor
213  // reset
214  static constexpr int L3GD20_NUM_CHECKED_REGISTERS{8};
215  static constexpr uint8_t _checked_registers[] = {
224  };
225 
227  uint8_t _checked_next{0};
228 
229  /**
230  * Start automatic measurement.
231  */
232  void start();
233 
234  /**
235  * Stop automatic measurement.
236  */
237  void stop();
238 
239  /**
240  * Reset the driver
241  */
242  void reset();
243 
244  /**
245  * disable I2C on the chip
246  */
247  void disable_i2c();
248 
249  void Run() override;
250 
251  /**
252  * check key registers for correct values
253  */
254  void check_registers(void);
255 
256  /**
257  * Fetch measurements from the sensor and update the report ring.
258  */
259  void measure();
260 
261  /**
262  * Read a register from the L3GD20
263  *
264  * @param The register to read.
265  * @return The value that was read.
266  */
267  uint8_t read_reg(unsigned reg);
268 
269  /**
270  * Write a register in the L3GD20
271  *
272  * @param reg The register to write.
273  * @param value The new value to write.
274  */
275  void write_reg(unsigned reg, uint8_t value);
276 
277  /**
278  * Modify a register in the L3GD20
279  *
280  * Bits are cleared before bits are set.
281  *
282  * @param reg The register to modify.
283  * @param clearbits Bits in the register to clear.
284  * @param setbits Bits in the register to set.
285  */
286  void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
287 
288  /**
289  * Write a register in the L3GD20, updating _checked_values
290  *
291  * @param reg The register to write.
292  * @param value The new value to write.
293  */
294  void write_checked_reg(unsigned reg, uint8_t value);
295 
296  /**
297  * Set the L3GD20 measurement range.
298  *
299  * @param max_dps The measurement range is set to permit reading at least
300  * this rate in degrees per second.
301  * Zero selects the maximum supported range.
302  * @return OK if the value can be supported, -ERANGE otherwise.
303  */
304  int set_range(unsigned max_dps);
305 
306  /**
307  * Set the L3GD20 internal sampling frequency.
308  *
309  * @param frequency The internal sampling frequency is set to not less than
310  * this value.
311  * Zero selects the maximum rate supported.
312  * @return OK if the value can be supported.
313  */
314  int set_samplerate(unsigned frequency);
315 
316  /* this class does not allow copying */
317  L3GD20(const L3GD20 &);
318  L3GD20 operator=(const L3GD20 &);
319 };
320 
321 constexpr uint8_t L3GD20::_checked_registers[];
322 
323 L3GD20::L3GD20(int bus, const char *path, uint32_t device, enum Rotation rotation) :
324  SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11 * 1000 * 1000),
325  ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(this->get_device_id())),
326  _px4_gyro(get_device_id(), ORB_PRIO_DEFAULT, rotation),
327  _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")),
328  _errors(perf_alloc(PC_COUNT, "l3gd20_err")),
329  _bad_registers(perf_alloc(PC_COUNT, "l3gd20_bad_reg")),
330  _duplicates(perf_alloc(PC_COUNT, "l3gd20_dupe")),
331  _rotation(rotation)
332 {
334 }
335 
337 {
338  /* make sure we are truly inactive */
339  stop();
340 
341  /* delete the perf counter */
346 }
347 
348 int
350 {
351  /* do SPI init (and probe) first */
352  if (SPI::init() != OK) {
353  return PX4_ERROR;
354  }
355 
356  reset();
357 
358  start();
359 
360  return PX4_OK;
361 }
362 
363 int
365 {
366  /* read dummy value to void to clear SPI statemachine on sensor */
367  (void)read_reg(ADDR_WHO_AM_I);
368 
369  bool success = false;
370  uint8_t v = 0;
371 
372  /* verify that the device is attached and functioning, accept
373  * L3GD20, L3GD20H and L3G4200D */
374  if ((v = read_reg(ADDR_WHO_AM_I)) == WHO_I_AM) {
376  success = true;
377 
378  } else if ((v = read_reg(ADDR_WHO_AM_I)) == WHO_I_AM_H) {
380  success = true;
381 
382  } else if ((v = read_reg(ADDR_WHO_AM_I)) == WHO_I_AM_L3G4200D) {
383  /* Detect the L3G4200D used on AeroCore */
384  _is_l3g4200d = true;
386  success = true;
387  }
388 
389  if (success) {
390  _checked_values[0] = v;
391  return OK;
392  }
393 
394  return -EIO;
395 }
396 
397 uint8_t
398 L3GD20::read_reg(unsigned reg)
399 {
400  uint8_t cmd[2] {};
401 
402  cmd[0] = reg | DIR_READ;
403  cmd[1] = 0;
404 
405  transfer(cmd, cmd, sizeof(cmd));
406 
407  return cmd[1];
408 }
409 
410 void
411 L3GD20::write_reg(unsigned reg, uint8_t value)
412 {
413  uint8_t cmd[2] {};
414 
415  cmd[0] = reg | DIR_WRITE;
416  cmd[1] = value;
417 
418  transfer(cmd, nullptr, sizeof(cmd));
419 }
420 
421 void
422 L3GD20::write_checked_reg(unsigned reg, uint8_t value)
423 {
424  write_reg(reg, value);
425 
426  for (uint8_t i = 0; i < L3GD20_NUM_CHECKED_REGISTERS; i++) {
427  if (reg == _checked_registers[i]) {
428  _checked_values[i] = value;
429  }
430  }
431 }
432 
433 
434 void
435 L3GD20::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
436 {
437  uint8_t val = read_reg(reg);
438  val &= ~clearbits;
439  val |= setbits;
440  write_checked_reg(reg, val);
441 }
442 
443 int
444 L3GD20::set_range(unsigned max_dps)
445 {
446  uint8_t bits = REG4_BDU;
447  float new_range_scale_dps_digit;
448 
449  if (max_dps == 0) {
450  max_dps = 2000;
451  }
452 
453  if (max_dps <= 250) {
454  //new_range = 250;
455  bits |= RANGE_250DPS;
456  new_range_scale_dps_digit = 8.75e-3f;
457 
458  } else if (max_dps <= 500) {
459  //new_range = 500;
460  bits |= RANGE_500DPS;
461  new_range_scale_dps_digit = 17.5e-3f;
462 
463  } else if (max_dps <= 2000) {
464  //new_range = 2000;
465  bits |= RANGE_2000DPS;
466  new_range_scale_dps_digit = 70e-3f;
467 
468  } else {
469  return -EINVAL;
470  }
471 
472  _px4_gyro.set_scale(new_range_scale_dps_digit / 180.0f * M_PI_F);
473 
475 
476  return OK;
477 }
478 
479 int
480 L3GD20::set_samplerate(unsigned frequency)
481 {
483 
484  if (frequency == 0) {
485  frequency = _is_l3g4200d ? 800 : 760;
486  }
487 
488  /*
489  * Use limits good for H or non-H models. Rates are slightly different
490  * for L3G4200D part but register settings are the same.
491  */
492  if (frequency <= 100) {
493  _current_rate = _is_l3g4200d ? 100 : 95;
494  bits |= RATE_95HZ_LP_25HZ;
495 
496  } else if (frequency <= 200) {
497  _current_rate = _is_l3g4200d ? 200 : 190;
498  bits |= RATE_190HZ_LP_50HZ;
499 
500  } else if (frequency <= 400) {
501  _current_rate = _is_l3g4200d ? 400 : 380;
502  bits |= RATE_380HZ_LP_50HZ;
503 
504  } else if (frequency <= 800) {
505  _current_rate = _is_l3g4200d ? 800 : 760;
506  bits |= RATE_760HZ_LP_50HZ;
507 
508  } else {
509  return -EINVAL;
510  }
511 
513 
514  return OK;
515 }
516 
517 void
519 {
520  /* make sure we are stopped first */
521  stop();
522 
523  /* start polling at the specified rate */
524  uint64_t interval = 1000000 / L3G4200D_DEFAULT_RATE;
525  ScheduleOnInterval(interval - L3GD20_TIMER_REDUCTION, 10000);
526 }
527 
528 void
530 {
531  ScheduleClear();
532 }
533 
534 void
536 {
537  uint8_t retries = 10;
538 
539  while (retries--) {
540  // add retries
541  uint8_t a = read_reg(0x05);
542  write_reg(0x05, (0x20 | a));
543 
544  if (read_reg(0x05) == (a | 0x20)) {
545  // this sets the I2C_DIS bit on the
546  // L3GD20H. The l3gd20 datasheet doesn't
547  // mention this register, but it does seem to
548  // accept it.
550  return;
551  }
552  }
553 
554  DEVICE_DEBUG("FAILED TO DISABLE I2C");
555 }
556 
557 void
559 {
560  // ensure the chip doesn't interpret any other bus traffic as I2C
561  disable_i2c();
562 
563  /* set default configuration */
565  write_checked_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */
566  write_checked_reg(ADDR_CTRL_REG3, 0x08); /* DRDY enable */
569  write_checked_reg(ADDR_CTRL_REG5, REG5_FIFO_ENABLE); /* disable wake-on-interrupt */
570 
571  /* disable FIFO. This makes things simpler and ensures we
572  * aren't getting stale data. It means we must run the hrt
573  * callback fast enough to not miss data. */
575 
576  set_samplerate(0); // 760Hz or 800Hz
578 
579  _read = 0;
580 }
581 
582 void
584 {
585  /* make another measurement */
586  measure();
587 }
588 
589 void
591 {
592  uint8_t v;
593 
594  if ((v = read_reg(_checked_registers[_checked_next])) != _checked_values[_checked_next]) {
595  /*
596  if we get the wrong value then we know the SPI bus
597  or sensor is very sick. We set _register_wait to 20
598  and wait until we have seen 20 good values in a row
599  before we consider the sensor to be OK again.
600  */
602 
603  /*
604  try to fix the bad register value. We only try to
605  fix one per loop to prevent a bad sensor hogging the
606  bus. We skip zero as that is the WHO_AM_I, which
607  is not writeable
608  */
609  if (_checked_next != 0) {
610  write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]);
611  }
612 
613  _register_wait = 20;
614  }
615 
616  _checked_next = (_checked_next + 1) % L3GD20_NUM_CHECKED_REGISTERS;
617 }
618 
619 void
621 {
622  /* status register and data as read back from the device */
623 #pragma pack(push, 1)
624  struct {
625  uint8_t cmd;
626  int8_t temp;
627  uint8_t status;
628  int16_t x;
629  int16_t y;
630  int16_t z;
631  } raw_report{};
632 #pragma pack(pop)
633 
634  /* start the performance counter */
636 
637  check_registers();
638 
639  /* fetch data from the sensor */
640  const hrt_abstime timestamp_sample = hrt_absolute_time();
641  raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT;
642  transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report));
643 
644  if (!(raw_report.status & STATUS_ZYXDA)) {
647  return;
648  }
649 
650  /*
651  * 1) Scale raw value to SI units using scaling from datasheet.
652  * 2) Subtract static offset (in SI units)
653  * 3) Scale the statically calibrated values with a linear
654  * dynamically obtained factor
655  *
656  * Note: the static sensor offset is the number the sensor outputs
657  * at a nominally 'zero' input. Therefore the offset has to
658  * be subtracted.
659  *
660  * Example: A gyro outputs a value of 74 at zero angular rate
661  * the offset is 74 from the origin and subtracting
662  * 74 from all measurements centers them around zero.
663  */
665 
667 
668  switch (_orientation) {
670  /* swap x and y */
671  _px4_gyro.update(timestamp_sample, raw_report.y, raw_report.x, raw_report.z);
672  break;
673 
675  /* swap x and y and negate both */
676  int16_t x = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
677  int16_t y = ((raw_report.y == -32768) ? 32767 : -raw_report.y);
678  _px4_gyro.update(timestamp_sample, x, y, raw_report.z);
679  }
680 
681  break;
682 
684  /* swap x and y and negate y */
685  int16_t x = raw_report.y;
686  int16_t y = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
687  _px4_gyro.update(timestamp_sample, x, y, raw_report.z);
688  }
689  break;
690 
692 
693  // FALLTHROUGH
694  default:
695  // keep axes in place
696  _px4_gyro.update(timestamp_sample, raw_report.x, raw_report.y, raw_report.z);
697  }
698 
699  _read++;
700 
701  /* stop the perf counter */
703 }
704 
705 void
707 {
708  printf("gyro reads: %u\n", _read);
713 
714  ::printf("checked_next: %u\n", _checked_next);
715 
716  for (uint8_t i = 0; i < L3GD20_NUM_CHECKED_REGISTERS; i++) {
717  uint8_t v = read_reg(_checked_registers[i]);
718 
719  if (v != _checked_values[i]) {
720  ::printf("reg %02x:%02x should be %02x\n",
721  (unsigned)_checked_registers[i],
722  (unsigned)v,
723  (unsigned)_checked_values[i]);
724  }
725  }
726 
728 }
729 
730 void
732 {
733  printf("L3GD20 registers\n");
734 
735  for (uint8_t reg = 0; reg <= 0x40; reg++) {
736  uint8_t v = read_reg(reg);
737  printf("%02x:%02x ", (unsigned)reg, (unsigned)v);
738 
739  if ((reg + 1) % 16 == 0) {
740  printf("\n");
741  }
742  }
743 
744  printf("\n");
745 }
746 
747 void
749 {
750  // trigger a deliberate error
752 }
753 
754 /**
755  * Local functions in support of the shell command.
756  */
757 namespace l3gd20
758 {
759 
761 
762 void usage();
763 void start(bool external_bus, enum Rotation rotation);
764 void info();
765 void regdump();
766 void test_error();
767 
768 /**
769  * Start the driver.
770  *
771  * This function call only returns once the driver
772  * started or failed to detect the sensor.
773  */
774 void
775 start(bool external_bus, enum Rotation rotation)
776 {
777  if (g_dev != nullptr) {
778  errx(0, "already started");
779  }
780 
781  /* create the driver */
782  if (external_bus) {
783 #if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_GYRO)
784  g_dev = new L3GD20(PX4_SPI_BUS_EXT, L3GD20_DEVICE_PATH, PX4_SPIDEV_EXT_GYRO, rotation);
785 #else
786  errx(0, "External SPI not available");
787 #endif
788 
789  } else {
790  g_dev = new L3GD20(PX4_SPI_BUS_SENSORS, L3GD20_DEVICE_PATH, PX4_SPIDEV_GYRO, rotation);
791  }
792 
793  if (g_dev == nullptr) {
794  goto fail;
795  }
796 
797  if (OK != g_dev->init()) {
798  goto fail;
799  }
800 
801  exit(0);
802 fail:
803 
804  if (g_dev != nullptr) {
805  delete g_dev;
806  g_dev = nullptr;
807  }
808 
809  errx(1, "driver start failed");
810 }
811 
812 /**
813  * Print a little info about the driver.
814  */
815 void
817 {
818  if (g_dev == nullptr) {
819  errx(1, "driver not running\n");
820  }
821 
822  printf("state @ %p\n", g_dev);
823  g_dev->print_info();
824 
825  exit(0);
826 }
827 
828 /**
829  * Dump the register information
830  */
831 void
832 regdump(void)
833 {
834  if (g_dev == nullptr) {
835  errx(1, "driver not running");
836  }
837 
838  printf("regdump @ %p\n", g_dev);
839  g_dev->print_registers();
840 
841  exit(0);
842 }
843 
844 /**
845  * trigger an error
846  */
847 void
849 {
850  if (g_dev == nullptr) {
851  errx(1, "driver not running");
852  }
853 
854  printf("regdump @ %p\n", g_dev);
855  g_dev->test_error();
856 
857  exit(0);
858 }
859 
860 void
862 {
863  warnx("missing command: try 'start', 'info', 'testerror' or 'regdump'");
864  warnx("options:");
865  warnx(" -X (external bus)");
866  warnx(" -R rotation");
867 }
868 
869 } // namespace
870 
871 int
872 l3gd20_main(int argc, char *argv[])
873 {
874  int myoptind = 1;
875  int ch;
876  const char *myoptarg = nullptr;
877  bool external_bus = false;
878  enum Rotation rotation = ROTATION_NONE;
879 
880  while ((ch = px4_getopt(argc, argv, "XR:", &myoptind, &myoptarg)) != EOF) {
881  switch (ch) {
882  case 'X':
883  external_bus = true;
884  break;
885 
886  case 'R':
887  rotation = (enum Rotation)atoi(myoptarg);
888  break;
889 
890  default:
891  l3gd20::usage();
892  return 0;
893  }
894  }
895 
896  if (myoptind >= argc) {
897  l3gd20::usage();
898  return -1;
899  }
900 
901  const char *verb = argv[myoptind];
902 
903  /*
904  * Start/load the driver.
905 
906  */
907  if (!strcmp(verb, "start")) {
908  l3gd20::start(external_bus, rotation);
909  }
910 
911  /*
912  * Print driver information.
913  */
914  if (!strcmp(verb, "info")) {
915  l3gd20::info();
916  }
917 
918  /*
919  * Print register information.
920  */
921  if (!strcmp(verb, "regdump")) {
922  l3gd20::regdump();
923  }
924 
925  /*
926  * trigger an error
927  */
928  if (!strcmp(verb, "testerror")) {
930  }
931 
932  PX4_ERR("unrecognized command, try 'start', 'test', 'reset', 'info', 'testerror' or 'regdump'");
933  return -1;
934 }
#define RANGE_250DPS
Definition: l3gd20.cpp:89
L3GD20 operator=(const L3GD20 &)
#define ADDR_FIFO_CTRL_REG
Definition: l3gd20.cpp:103
#define L3GD20_TIMER_REDUCTION
Definition: l3gd20.cpp:164
perf_counter_t _duplicates
Definition: l3gd20.cpp:202
void Run() override
Definition: l3gd20.cpp:583
static struct vehicle_status_s status
Definition: Commander.cpp:138
unsigned _read
Definition: l3gd20.cpp:197
static constexpr int L3GD20_NUM_CHECKED_REGISTERS
Definition: l3gd20.cpp:214
measure the time elapsed performing an event
Definition: perf_counter.h:56
#define FIFO_CTRL_BYPASS_MODE
Definition: l3gd20.cpp:139
void print_status()
#define WHO_I_AM
Definition: l3gd20.cpp:65
#define ADDR_INCREMENT
Definition: l3gd20.cpp:60
#define SENSOR_BOARD_ROTATION_DEFAULT
Definition: l3gd20.cpp:155
virtual ~L3GD20()
Definition: l3gd20.cpp:336
void regdump()
Dump the register information.
Definition: l3gd20.cpp:832
void usage(const char *reason)
Print the correct usage.
Definition: Commander.cpp:4238
__EXPORT int l3gd20_main(int argc, char *argv[])
Definition: l3gd20.cpp:872
#define REG1_Z_ENABLE
Definition: l3gd20.cpp:119
uint8_t read_reg(unsigned reg)
Read a register from the L3GD20.
Definition: l3gd20.cpp:398
#define SENSOR_BOARD_ROTATION_180_DEG
Definition: l3gd20.cpp:54
Definition: I2C.hpp:51
#define RATE_95HZ_LP_25HZ
Definition: l3gd20.cpp:72
#define RATE_380HZ_LP_50HZ
Definition: l3gd20.cpp:78
perf_counter_t _bad_registers
Definition: l3gd20.cpp:201
enum Rotation _rotation
Definition: l3gd20.cpp:209
void measure()
Fetch measurements from the sensor and update the report ring.
Definition: l3gd20.cpp:620
L3GD20 * g_dev
Definition: l3gd20.cpp:760
virtual int init()
Definition: l3gd20.cpp:349
PX4Gyroscope _px4_gyro
Definition: l3gd20.cpp:192
Namespace encapsulating all device framework classes, functions and data.
Definition: CDev.cpp:47
count the number of times an event occurs
Definition: perf_counter.h:55
Vector rotation library.
void set_device_type(uint8_t devtype)
void disable_i2c()
disable I2C on the chip
Definition: l3gd20.cpp:535
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
Modify a register in the L3GD20.
Definition: l3gd20.cpp:435
#define ADDR_CTRL_REG5
Definition: l3gd20.cpp:93
#define RATE_760HZ_LP_50HZ
Definition: l3gd20.cpp:82
#define REG4_BDU
Definition: l3gd20.cpp:123
#define WHO_I_AM_H
Definition: l3gd20.cpp:64
void update(hrt_abstime timestamp, float x, float y, float z)
void perf_count(perf_counter_t handle)
Count a performance event.
#define RANGE_2000DPS
Definition: l3gd20.cpp:91
#define DRV_GYR_DEVTYPE_L3GD20
Definition: drv_sensor.h:72
#define REG5_FIFO_ENABLE
Definition: l3gd20.cpp:127
void start()
Start automatic measurement.
Definition: l3gd20.cpp:518
#define RATE_190HZ_LP_50HZ
Definition: l3gd20.cpp:74
Header common to all counters.
void print_registers()
Definition: l3gd20.cpp:731
void perf_free(perf_counter_t handle)
Free a counter.
#define REG1_X_ENABLE
Definition: l3gd20.cpp:121
void init()
Activates/configures the hardware registers.
void usage()
Prints info about the driver argument usage.
Definition: l3gd20.cpp:861
#define SENSOR_BOARD_ROTATION_090_DEG
Definition: l3gd20.cpp:53
#define perf_alloc(a, b)
Definition: px4io.h:59
void test_error()
Definition: l3gd20.cpp:748
Rotation
Enum for board and external compass rotations.
Definition: rotation.h:51
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
#define ADDR_CTRL_REG4
Definition: l3gd20.cpp:87
#define warnx(...)
Definition: err.h:95
void check_registers(void)
check key registers for correct values
Definition: l3gd20.cpp:590
#define L3G4200D_DEFAULT_RATE
Definition: l3gd20.cpp:146
Local functions in support of the shell command.
Definition: l3gd20.cpp:757
unsigned _current_rate
Definition: l3gd20.cpp:194
void set_error_count(uint64_t error_count)
void perf_end(perf_counter_t handle)
End a performance event.
bool _is_l3g4200d
Definition: l3gd20.cpp:207
uint8_t _checked_next
Definition: l3gd20.cpp:227
void info()
Print a little info about the driver.
Definition: l3gd20.cpp:816
unsigned _orientation
Definition: l3gd20.cpp:195
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
#define SENSOR_BOARD_ROTATION_000_DEG
Definition: l3gd20.cpp:52
#define DIR_READ
Definition: l3gd20.cpp:58
#define L3GD20_DEFAULT_RANGE_DPS
Definition: l3gd20.cpp:148
#define ADDR_WHO_AM_I
Definition: l3gd20.cpp:63
void print_info()
Diagnostics - print some basic information about the driver.
Definition: l3gd20.cpp:706
#define SENSOR_BOARD_ROTATION_270_DEG
Definition: l3gd20.cpp:55
uint8_t _checked_values[L3GD20_NUM_CHECKED_REGISTERS]
Definition: l3gd20.cpp:226
L3GD20(int bus, const char *path, uint32_t device, enum Rotation rotation)
Definition: l3gd20.cpp:323
void write_checked_reg(unsigned reg, uint8_t value)
Write a register in the L3GD20, updating _checked_values.
Definition: l3gd20.cpp:422
#define L3GD20_TEMP_OFFSET_CELSIUS
Definition: l3gd20.cpp:150
perf_counter_t _errors
Definition: l3gd20.cpp:200
void set_temperature(float temperature)
#define ADDR_CTRL_REG1
Definition: l3gd20.cpp:68
#define REG1_POWER_NORMAL
Definition: l3gd20.cpp:118
#define L3GD20_DEVICE_PATH
Definition: l3gd20.cpp:49
#define ADDR_CTRL_REG2
Definition: l3gd20.cpp:85
#define DIR_WRITE
Definition: l3gd20.cpp:59
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
uint64_t perf_event_count(perf_counter_t handle)
Return current event_count.
void write_reg(unsigned reg, uint8_t value)
Write a register in the L3GD20.
Definition: l3gd20.cpp:411
#define errx(eval,...)
Definition: err.h:89
#define REG1_Y_ENABLE
Definition: l3gd20.cpp:120
Definition: bst.cpp:62
int set_range(unsigned max_dps)
Set the L3GD20 measurement range.
Definition: l3gd20.cpp:444
static constexpr uint8_t _checked_registers[]
Definition: l3gd20.cpp:215
void stop()
Stop automatic measurement.
Definition: l3gd20.cpp:529
void test_error()
trigger an error
Definition: l3gd20.cpp:848
#define OK
Definition: uavcan_main.cpp:71
#define M_PI_F
Definition: ashtech.cpp:44
void start(bool external_bus, enum Rotation rotation)
Start the driver.
Definition: l3gd20.cpp:775
#define ADDR_CTRL_REG3
Definition: l3gd20.cpp:86
#define RANGE_500DPS
Definition: l3gd20.cpp:90
void set_scale(float scale)
void reset()
Reset the driver.
Definition: l3gd20.cpp:558
int set_samplerate(unsigned frequency)
Set the L3GD20 internal sampling frequency.
Definition: l3gd20.cpp:480
virtual int probe()
Definition: l3gd20.cpp:364
uint8_t _register_wait
Definition: l3gd20.cpp:204
#define ADDR_OUT_TEMP
Definition: l3gd20.cpp:95
perf_counter_t _sample_perf
Definition: l3gd20.cpp:199
void perf_begin(perf_counter_t handle)
Begin a performance event.
#define WHO_I_AM_L3G4200D
Definition: l3gd20.cpp:66
#define DEVICE_DEBUG(FMT,...)
Definition: Device.hpp:52
#define ADDR_LOW_ODR
Definition: l3gd20.cpp:114
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
Performance measuring tools.
#define STATUS_ZYXDA
Definition: l3gd20.cpp:134