PX4 Firmware
PX4 Autopilot Software http://px4.io
bmi160.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2018 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 #include "bmi160.hpp"
35 
36 /*
37  list of registers that will be checked in check_registers(). Note
38  that ADDR_WHO_AM_I must be first in the list.
39  */
40 const uint8_t BMI160::_checked_registers[BMI160_NUM_CHECKED_REGISTERS] = { BMIREG_CHIP_ID,
50  };
51 
52 BMI160::BMI160(int bus, uint32_t device, enum Rotation rotation) :
53  SPI("BMI160", nullptr, bus, device, SPIDEV_MODE3, BMI160_BUS_SPEED),
54  ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(this->get_device_id())),
55  _px4_accel(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation),
56  _px4_gyro(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation),
57  _accel_reads(perf_alloc(PC_COUNT, "bmi160_accel_read")),
58  _gyro_reads(perf_alloc(PC_COUNT, "bmi160_gyro_read")),
59  _sample_perf(perf_alloc(PC_ELAPSED, "bmi160_read")),
60  _bad_transfers(perf_alloc(PC_COUNT, "bmi160_bad_transfers")),
61  _bad_registers(perf_alloc(PC_COUNT, "bmi160_bad_registers")),
62  _good_transfers(perf_alloc(PC_COUNT, "bmi160_good_transfers")),
63  _reset_retries(perf_alloc(PC_COUNT, "bmi160_reset_retries")),
64  _duplicates(perf_alloc(PC_COUNT, "bmi160_duplicates"))
65 {
68 
71 }
72 
74 {
75  /* make sure we are truly inactive */
76  stop();
77 
78  /* delete the perf counter */
87 }
88 
89 int
91 {
92  /* do SPI init (and probe) first */
93  int ret = SPI::init();
94 
95  /* if probe/setup failed, bail now */
96  if (ret != OK) {
97  DEVICE_DEBUG("SPI setup failed");
98  return ret;
99  }
100 
101  ret = reset();
102 
103  if (ret != PX4_OK) {
104  return ret;
105  }
106 
107  start();
108 
109  return ret;
110 }
111 
113 {
114  write_reg(BMIREG_CONF, (1 << 1)); //Enable NVM programming
115 
116  write_checked_reg(BMIREG_ACC_CONF, BMI_ACCEL_US | BMI_ACCEL_BWP_NORMAL); //Normal operation, no decimation
118  write_checked_reg(BMIREG_GYR_CONF, BMI_GYRO_BWP_NORMAL); //Normal operation, no decimation
120  write_checked_reg(BMIREG_INT_EN_1, BMI_DRDY_INT_EN); //Enable DRDY interrupt
121  write_checked_reg(BMIREG_INT_OUT_CTRL, BMI_INT1_EN); //Enable interrupts on pin INT1
122  write_checked_reg(BMIREG_INT_MAP_1, BMI_DRDY_INT1); //DRDY interrupt on pin INT1
124  BMI_AUTO_DIS_SEC); //Disable secondary interface; Work in SPI 4-wire mode
125  write_checked_reg(BMIREG_NV_CONF, BMI_SPI); //Disable I2C interface
126 
129 
132 
133 
134  //_set_dlpf_filter(BMI160_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); //NOT CONSIDERING FILTERING YET
135 
136  //Enable Accelerometer in normal mode
138  up_udelay(4100);
139  //usleep(4100);
140 
141  //Enable Gyroscope in normal mode
143  up_udelay(80300);
144  //usleep(80300);
145 
146  uint8_t retries = 10;
147 
148  while (retries--) {
149  bool all_ok = true;
150 
151  for (uint8_t i = 0; i < BMI160_NUM_CHECKED_REGISTERS; i++) {
154  all_ok = false;
155  }
156  }
157 
158  if (all_ok) {
159  break;
160  }
161  }
162 
163  _accel_reads = 0;
164  _gyro_reads = 0;
165 
166  return OK;
167 }
168 
169 int
171 {
172  /* look for device ID */
174 
175  // verify product revision
176  switch (_whoami) {
177  case BMI160_WHO_AM_I:
178  memset(_checked_values, 0, sizeof(_checked_values));
179  memset(_checked_bad, 0, sizeof(_checked_bad));
181  _checked_bad[0] = _whoami;
182  return OK;
183  }
184 
185  DEVICE_DEBUG("unexpected whoami 0x%02x", _whoami);
186  return -EIO;
187 }
188 
189 int
191 {
192  uint8_t setbits = 0;
193  uint8_t clearbits = (BMI_ACCEL_RATE_25_8 | BMI_ACCEL_RATE_1600);
194 
195  if ((int)frequency == 0) {
196  frequency = 1600;
197  }
198 
199  if (frequency <= 25 / 32) {
200  setbits |= BMI_ACCEL_RATE_25_32;
201  _accel_sample_rate = 25 / 32;
202 
203  } else if (frequency <= 25 / 16) {
204  setbits |= BMI_ACCEL_RATE_25_16;
205  _accel_sample_rate = 25 / 16;
206 
207  } else if (frequency <= 25 / 8) {
208  setbits |= BMI_ACCEL_RATE_25_8;
209  _accel_sample_rate = 25 / 8;
210 
211  } else if (frequency <= 25 / 4) {
212  setbits |= BMI_ACCEL_RATE_25_4;
213  _accel_sample_rate = 25 / 4;
214 
215  } else if (frequency <= 25 / 2) {
216  setbits |= BMI_ACCEL_RATE_25_2;
217  _accel_sample_rate = 25 / 2;
218 
219  } else if (frequency <= 25) {
220  setbits |= BMI_ACCEL_RATE_25;
221  _accel_sample_rate = 25;
222 
223  } else if (frequency <= 50) {
224  setbits |= BMI_ACCEL_RATE_50;
225  _accel_sample_rate = 50;
226 
227  } else if (frequency <= 100) {
228  setbits |= BMI_ACCEL_RATE_100;
229  _accel_sample_rate = 100;
230 
231  } else if (frequency <= 200) {
232  setbits |= BMI_ACCEL_RATE_200;
233  _accel_sample_rate = 200;
234 
235  } else if (frequency <= 400) {
236  setbits |= BMI_ACCEL_RATE_400;
237  _accel_sample_rate = 400;
238 
239  } else if (frequency <= 800) {
240  setbits |= BMI_ACCEL_RATE_800;
241  _accel_sample_rate = 800;
242 
243  } else if (frequency > 800) {
244  setbits |= BMI_ACCEL_RATE_1600;
245  _accel_sample_rate = 1600;
246 
247  } else {
248  return -EINVAL;
249  }
250 
251  modify_reg(BMIREG_ACC_CONF, clearbits, setbits);
252 
253  return OK;
254 }
255 
256 int
258 {
259  uint8_t setbits = 0;
260  uint8_t clearbits = (BMI_GYRO_RATE_200 | BMI_GYRO_RATE_25);
261 
262  if ((int)frequency == 0) {
263  frequency = 3200;
264  }
265 
266  if (frequency <= 25) {
267  setbits |= BMI_GYRO_RATE_25;
268  _gyro_sample_rate = 25;
269 
270  } else if (frequency <= 50) {
271  setbits |= BMI_GYRO_RATE_50;
272  _gyro_sample_rate = 50;
273 
274  } else if (frequency <= 100) {
275  setbits |= BMI_GYRO_RATE_100;
276  _gyro_sample_rate = 100;
277 
278  } else if (frequency <= 200) {
279  setbits |= BMI_GYRO_RATE_200;
280  _gyro_sample_rate = 200;
281 
282  } else if (frequency <= 400) {
283  setbits |= BMI_GYRO_RATE_400;
284  _gyro_sample_rate = 400;
285 
286  } else if (frequency <= 800) {
287  setbits |= BMI_GYRO_RATE_800;
288  _gyro_sample_rate = 800;
289 
290  } else if (frequency <= 1600) {
291  setbits |= BMI_GYRO_RATE_1600;
292  _gyro_sample_rate = 1600;
293 
294  } else if (frequency > 1600) {
295  setbits |= BMI_GYRO_RATE_3200;
296  _gyro_sample_rate = 3200;
297 
298  } else {
299  return -EINVAL;
300  }
301 
302  modify_reg(BMIREG_GYR_CONF, clearbits, setbits);
303 
304  return OK;
305 }
306 
307 void
308 BMI160::_set_dlpf_filter(uint16_t bandwidth)
309 {
310  _dlpf_freq = 0;
311  bandwidth = bandwidth; //TO BE IMPLEMENTED
312  /*uint8_t setbits = BW_SCAL_ODR_BW_XL;
313  uint8_t clearbits = BW_XL_50_HZ;
314 
315  if (bandwidth == 0) {
316  _dlpf_freq = 408;
317  clearbits = BW_SCAL_ODR_BW_XL | BW_XL_50_HZ;
318  setbits = 0;
319  }
320 
321  if (bandwidth <= 50) {
322  setbits |= BW_XL_50_HZ;
323  _dlpf_freq = 50;
324 
325  } else if (bandwidth <= 105) {
326  setbits |= BW_XL_105_HZ;
327  _dlpf_freq = 105;
328 
329  } else if (bandwidth <= 211) {
330  setbits |= BW_XL_211_HZ;
331  _dlpf_freq = 211;
332 
333  } else if (bandwidth <= 408) {
334  setbits |= BW_XL_408_HZ;
335  _dlpf_freq = 408;
336 
337  }
338  modify_reg(CTRL_REG6_XL, clearbits, setbits);*/
339 }
340 
341 /*
342  deliberately trigger an error in the sensor to trigger recovery
343  */
344 void
346 {
348  ::printf("error triggered\n");
349  print_registers();
350 }
351 
352 uint8_t
353 BMI160::read_reg(unsigned reg)
354 {
355  uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0};
356 
357  transfer(cmd, cmd, sizeof(cmd));
358 
359  return cmd[1];
360 }
361 
362 uint16_t
363 BMI160::read_reg16(unsigned reg)
364 {
365  uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 };
366 
367  transfer(cmd, cmd, sizeof(cmd));
368 
369  return (uint16_t)(cmd[1] << 8) | cmd[2];
370 }
371 
372 void
373 BMI160::write_reg(unsigned reg, uint8_t value)
374 {
375  uint8_t cmd[2];
376 
377  cmd[0] = reg | DIR_WRITE;
378  cmd[1] = value;
379 
380  transfer(cmd, nullptr, sizeof(cmd));
381 }
382 
383 void
384 BMI160::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
385 {
386  uint8_t val;
387 
388  val = read_reg(reg);
389  val &= ~clearbits;
390  val |= setbits;
391  write_checked_reg(reg, val);
392 }
393 
394 void
395 BMI160::write_checked_reg(unsigned reg, uint8_t value)
396 {
397  write_reg(reg, value);
398 
399  for (uint8_t i = 0; i < BMI160_NUM_CHECKED_REGISTERS; i++) {
400  if (reg == _checked_registers[i]) {
401  _checked_values[i] = value;
402  _checked_bad[i] = value;
403  }
404  }
405 }
406 
407 int
408 BMI160::set_accel_range(unsigned max_g)
409 {
410  uint8_t setbits = 0;
411  uint8_t clearbits = BMI_ACCEL_RANGE_2_G | BMI_ACCEL_RANGE_16_G;
412  float lsb_per_g;
413 
414  if (max_g == 0) {
415  max_g = 16;
416  }
417 
418  if (max_g <= 2) {
419  //max_accel_g = 2;
420  setbits |= BMI_ACCEL_RANGE_2_G;
421  lsb_per_g = 16384;
422 
423  } else if (max_g <= 4) {
424  //max_accel_g = 4;
425  setbits |= BMI_ACCEL_RANGE_4_G;
426  lsb_per_g = 8192;
427 
428  } else if (max_g <= 8) {
429  //max_accel_g = 8;
430  setbits |= BMI_ACCEL_RANGE_8_G;
431  lsb_per_g = 4096;
432 
433  } else if (max_g <= 16) {
434  //max_accel_g = 16;
435  setbits |= BMI_ACCEL_RANGE_16_G;
436  lsb_per_g = 2048;
437 
438  } else {
439  return -EINVAL;
440  }
441 
442  _px4_accel.set_scale(CONSTANTS_ONE_G / lsb_per_g);
443 
444  modify_reg(BMIREG_ACC_RANGE, clearbits, setbits);
445 
446  return OK;
447 }
448 
449 int
450 BMI160::set_gyro_range(unsigned max_dps)
451 {
452  uint8_t setbits = 0;
453  uint8_t clearbits = BMI_GYRO_RANGE_125_DPS | BMI_GYRO_RANGE_250_DPS;
454  float lsb_per_dps;
455  //float max_gyro_dps;
456 
457  if (max_dps == 0) {
458  max_dps = 2000;
459  }
460 
461  if (max_dps <= 125) {
462  //max_gyro_dps = 125;
463  lsb_per_dps = 262.4;
464  setbits |= BMI_GYRO_RANGE_125_DPS;
465 
466  } else if (max_dps <= 250) {
467  //max_gyro_dps = 250;
468  lsb_per_dps = 131.2;
469  setbits |= BMI_GYRO_RANGE_250_DPS;
470 
471  } else if (max_dps <= 500) {
472  //max_gyro_dps = 500;
473  lsb_per_dps = 65.6;
474  setbits |= BMI_GYRO_RANGE_500_DPS;
475 
476  } else if (max_dps <= 1000) {
477  //max_gyro_dps = 1000;
478  lsb_per_dps = 32.8;
479  setbits |= BMI_GYRO_RANGE_1000_DPS;
480 
481  } else if (max_dps <= 2000) {
482  //max_gyro_dps = 2000;
483  lsb_per_dps = 16.4;
484  setbits |= BMI_GYRO_RANGE_2000_DPS;
485 
486  } else {
487  return -EINVAL;
488  }
489 
490  _px4_gyro.set_scale(M_PI_F / (180.0f * lsb_per_dps));
491 
492  modify_reg(BMIREG_GYR_RANGE, clearbits, setbits);
493 
494  return OK;
495 }
496 
497 void
499 {
500  /* make sure we are stopped first */
501  stop();
502 
503  /* start polling at the specified rate */
504  ScheduleOnInterval((1_s / BMI160_GYRO_DEFAULT_RATE) - BMI160_TIMER_REDUCTION, 1000);
505 
506  reset();
507 }
508 
509 void
511 {
512  ScheduleClear();
513 }
514 
515 void
517 {
518  /* make another measurement */
519  measure();
520 }
521 
522 void
524 {
525  uint8_t v;
526 
528  _checked_values[_checked_next]) {
530 
531  /*
532  if we get the wrong value then we know the SPI bus
533  or sensor is very sick. We set _register_wait to 20
534  and wait until we have seen 20 good values in a row
535  before we consider the sensor to be OK again.
536  */
538 
539  /*
540  try to fix the bad register value. We only try to
541  fix one per loop to prevent a bad sensor hogging the
542  bus.
543  */
544  if (_register_wait == 0 || _checked_next == 0) {
545  // if the product_id is wrong then reset the
546  // sensor completely
548  _reset_wait = hrt_absolute_time() + 10000;
549  _checked_next = 0;
550 
551  } else {
552  write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]);
553  // waiting 3ms between register writes seems
554  // to raise the chance of the sensor
555  // recovering considerably
556  _reset_wait = hrt_absolute_time() + 3000;
557  }
558 
559  _register_wait = 20;
560  }
561 
562  _checked_next = (_checked_next + 1) % BMI160_NUM_CHECKED_REGISTERS;
563 }
564 
565 void
567 {
568  if (hrt_absolute_time() < _reset_wait) {
569  // we're waiting for a reset to complete
570  return;
571  }
572 
573  struct BMIReport bmi_report {};
574 
575  struct Report {
576  int16_t accel_x;
577  int16_t accel_y;
578  int16_t accel_z;
579  int16_t temp;
580  int16_t gyro_x;
581  int16_t gyro_y;
582  int16_t gyro_z;
583  } report;
584 
585  /* start measuring */
587 
588  /*
589  * Fetch the full set of measurements from the BMI160 in one pass.
590  */
591  bmi_report.cmd = BMIREG_GYR_X_L | DIR_READ;
592 
593  uint8_t status = read_reg(BMIREG_STATUS);
594 
595  const hrt_abstime timestamp_sample = hrt_absolute_time();
596 
597  if (OK != transfer((uint8_t *)&bmi_report, ((uint8_t *)&bmi_report), sizeof(bmi_report))) {
598  return;
599  }
600 
601  check_registers();
602 
603  if ((!(status & (0x80))) && (!(status & (0x04)))) {
606  _got_duplicate = true;
607  return;
608  }
609 
610  _last_accel[0] = bmi_report.accel_x;
611  _last_accel[1] = bmi_report.accel_y;
612  _last_accel[2] = bmi_report.accel_z;
613  _got_duplicate = false;
614 
615  uint8_t temp_l = read_reg(BMIREG_TEMP_0);
616  uint8_t temp_h = read_reg(BMIREG_TEMP_1);
617 
618  report.temp = ((temp_h << 8) + temp_l);
619 
620 
621 
622  report.accel_x = bmi_report.accel_x;
623  report.accel_y = bmi_report.accel_y;
624  report.accel_z = bmi_report.accel_z;
625 
626  report.gyro_x = bmi_report.gyro_x;
627  report.gyro_y = bmi_report.gyro_y;
628  report.gyro_z = bmi_report.gyro_z;
629 
630  if (report.accel_x == 0 &&
631  report.accel_y == 0 &&
632  report.accel_z == 0 &&
633  report.temp == 0 &&
634  report.gyro_x == 0 &&
635  report.gyro_y == 0 &&
636  report.gyro_z == 0) {
637 
638  // all zero data - probably a SPI bus error
641  // note that we don't call reset() here as a reset()
642  // costs 20ms with interrupts disabled. That means if
643  // the bmi160 does go bad it would cause a FMU failure,
644  // regardless of whether another sensor is available,
645  return;
646  }
647 
649 
650  if (_register_wait != 0) {
651  // we are waiting for some good transfers before using
652  // the sensor again. We still increment
653  // _good_transfers, but don't return any data yet
654  _register_wait--;
655  return;
656  }
657 
658  // report the error count as the sum of the number of bad
659  // transfers and bad register reads. This allows the higher
660  // level code to decide if it should use this sensor based on
661  // whether it has had failures
662  const uint64_t error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);
663  _px4_accel.set_error_count(error_count);
664  _px4_gyro.set_error_count(error_count);
665 
666  const float temperature = 23.0f + report.temp * 1.0f / 512.0f;
667  _px4_accel.set_temperature(temperature);
668  _px4_gyro.set_temperature(temperature);
669 
670  /*
671  * 1) Scale raw value to SI units using scaling from datasheet.
672  * 2) Subtract static offset (in SI units)
673  * 3) Scale the statically calibrated values with a linear
674  * dynamically obtained factor
675  *
676  * Note: the static sensor offset is the number the sensor outputs
677  * at a nominally 'zero' input. Therefore the offset has to
678  * be subtracted.
679  *
680  * Example: A gyro outputs a value of 74 at zero angular rate
681  * the offset is 74 from the origin and subtracting
682  * 74 from all measurements centers them around zero.
683  */
684 
685 
686  /* NOTE: Axes have been swapped to match the board a few lines above. */
687 
688  _px4_accel.update(timestamp_sample, bmi_report.accel_x, bmi_report.accel_y, bmi_report.accel_z);
689  _px4_gyro.update(timestamp_sample, bmi_report.gyro_x, bmi_report.gyro_y, bmi_report.gyro_z);
690 
691  /* stop measuring */
693 }
694 
695 void
697 {
706 
707  ::printf("checked_next: %u\n", _checked_next);
708 
709  for (uint8_t i = 0; i < BMI160_NUM_CHECKED_REGISTERS; i++) {
710  uint8_t v = read_reg(_checked_registers[i]);
711 
712  if (v != _checked_values[i]) {
713  ::printf("reg %02x:%02x should be %02x\n",
714  (unsigned)_checked_registers[i],
715  (unsigned)v,
716  (unsigned)_checked_values[i]);
717  }
718 
719  if (v != _checked_bad[i]) {
720  ::printf("reg %02x:%02x was bad %02x\n",
721  (unsigned)_checked_registers[i],
722  (unsigned)v,
723  (unsigned)_checked_bad[i]);
724  }
725  }
726 
729 }
730 
731 void
733 {
734  printf("BMI160 registers\n");
735 
736  for (uint8_t reg = 0x40; reg <= 0x47; reg++) {
737  uint8_t v = read_reg(reg);
738  printf("%02x:%02x ", (unsigned)reg, (unsigned)v);
739 
740  if (reg % 13 == 0) {
741  printf("\n");
742  }
743  }
744 
745  printf("\n");
746 }
#define BMI_GYRO_RANGE_2000_DPS
Definition: bmi160.hpp:195
#define BMI_SPI_4_WIRE
Definition: bmi160.hpp:212
void print_registers()
Definition: bmi160.cpp:732
#define BMIREG_IF_CONF
Definition: bmi160.hpp:127
uint64_t _reset_wait
Definition: bmi160.hpp:289
#define BMI_GYRO_RANGE_500_DPS
Definition: bmi160.hpp:197
#define BMI160_ACCEL_DEFAULT_RANGE_G
Definition: bmi160.hpp:225
uint16_t read_reg16(unsigned reg)
Definition: bmi160.cpp:363
void start()
Start automatic measurement.
Definition: bmi160.cpp:498
#define BMI_GYRO_RATE_200
Definition: bmi160.hpp:185
static struct vehicle_status_s status
Definition: Commander.cpp:138
uint8_t _checked_bad[BMI160_NUM_CHECKED_REGISTERS]
Definition: bmi160.hpp:297
#define BMI_ACCEL_RATE_25_16
Definition: bmi160.hpp:159
#define BMIREG_ACC_CONF
Definition: bmi160.hpp:87
#define BMI160_BUS_SPEED
Definition: bmi160.hpp:240
measure the time elapsed performing an event
Definition: perf_counter.h:56
void set_sample_rate(unsigned rate)
int accel_set_sample_rate(float desired_sample_rate_hz)
Definition: bmi160.cpp:190
#define BMIREG_GYR_RANGE
Definition: bmi160.hpp:90
#define BMI160_GYRO_DEFAULT_RATE
Definition: bmi160.hpp:230
void print_status()
void write_reg(unsigned reg, uint8_t value)
Write a register in the BMI160.
Definition: bmi160.cpp:373
uint8_t _checked_values[BMI160_NUM_CHECKED_REGISTERS]
Definition: bmi160.hpp:296
#define BMI_ACCEL_RATE_25_8
Definition: bmi160.hpp:160
void update(hrt_abstime timestamp, float x, float y, float z)
#define BMI160_WHO_AM_I
Definition: bmi160.hpp:146
#define BMIREG_GYR_CONF
Definition: bmi160.hpp:89
#define BMI_GYRO_RATE_3200
Definition: bmi160.hpp:189
#define BMIREG_GYR_X_L
Definition: bmi160.hpp:62
int gyro_set_sample_rate(float desired_sample_rate_hz)
Definition: bmi160.cpp:257
#define BMI_ACCEL_RATE_25
Definition: bmi160.hpp:163
int reset()
Reset chip.
Definition: bmi160.cpp:112
#define BMI_ACCEL_RANGE_2_G
Definition: bmi160.hpp:176
void measure()
Fetch measurements from the sensor and update the report buffers.
Definition: bmi160.cpp:566
PX4Gyroscope _px4_gyro
Definition: bmi160.hpp:270
uint8_t _register_wait
Definition: bmi160.hpp:288
#define BMI_ACCEL_RANGE_16_G
Definition: bmi160.hpp:179
#define BMIREG_CMD
Definition: bmi160.hpp:142
void _set_dlpf_filter(uint16_t frequency_hz)
Definition: bmi160.cpp:308
int16_t gyro_y
Definition: bmi160.hpp:406
#define BMI_AUTO_DIS_SEC
Definition: bmi160.hpp:213
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
#define BMIREG_CONF
Definition: bmi160.hpp:126
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
Modify a register in the BMI160.
Definition: bmi160.cpp:384
#define BMI_DRDY_INT_EN
Definition: bmi160.hpp:202
float _gyro_sample_rate
Definition: bmi160.hpp:277
#define BMI_ACCEL_US
Definition: bmi160.hpp:170
#define BMIREG_INT_EN_1
Definition: bmi160.hpp:101
virtual int init()
Definition: bmi160.cpp:90
void set_device_type(uint8_t devtype)
void stop()
Stop automatic measurement.
Definition: bmi160.cpp:510
#define BMI_ACCEL_RANGE_4_G
Definition: bmi160.hpp:177
static constexpr float CONSTANTS_ONE_G
Definition: geo.h:51
void set_error_count(uint64_t error_count)
#define BMI_GYRO_RATE_50
Definition: bmi160.hpp:183
float _accel_sample_rate
Definition: bmi160.hpp:276
#define BMI_GYRO_NORMAL_MODE
Definition: bmi160.hpp:222
uint16_t _last_accel[3]
Definition: bmi160.hpp:301
#define BMI_ACCEL_RATE_1600
Definition: bmi160.hpp:169
PX4Accelerometer _px4_accel
Definition: bmi160.hpp:269
#define BMI_SPI
Definition: bmi160.hpp:218
#define BMI_ACCEL_RANGE_8_G
Definition: bmi160.hpp:178
#define DIR_READ
Definition: bmp388_spi.cpp:46
virtual int probe()
Definition: bmi160.cpp:170
#define BMI_GYRO_RANGE_1000_DPS
Definition: bmi160.hpp:196
#define BMI_ACCEL_RATE_25_2
Definition: bmi160.hpp:162
perf_counter_t _sample_perf
Definition: bmi160.hpp:281
perf_counter_t _accel_reads
Definition: bmi160.hpp:279
#define BMI_GYRO_RATE_25
Definition: bmi160.hpp:182
void update(hrt_abstime timestamp, float x, float y, float z)
void perf_count(perf_counter_t handle)
Count a performance event.
#define BMI160_GYRO_DEFAULT_RANGE_DPS
Definition: bmi160.hpp:226
Report conversation within the BMI160, including command byte and interrupt status.
Definition: bmi160.hpp:403
perf_counter_t _good_transfers
Definition: bmi160.hpp:284
#define BMI_GYRO_BWP_NORMAL
Definition: bmi160.hpp:190
void perf_free(perf_counter_t handle)
Free a counter.
#define BMI_INT1_EN
Definition: bmi160.hpp:205
void init()
Activates/configures the hardware registers.
void set_temperature(float temperature)
#define BMI160_TIMER_REDUCTION
Definition: bmi160.hpp:242
#define perf_alloc(a, b)
Definition: px4io.h:59
int16_t accel_y
Definition: bmi160.hpp:409
#define BMIREG_CHIP_ID
Definition: bmi160.hpp:51
uint8_t _checked_next
Definition: bmi160.hpp:298
Rotation
Enum for board and external compass rotations.
Definition: rotation.h:51
#define BMIREG_TEMP_1
Definition: bmi160.hpp:83
#define BMIREG_INT_OUT_CTRL
Definition: bmi160.hpp:103
int16_t gyro_z
Definition: bmi160.hpp:407
void write_checked_reg(unsigned reg, uint8_t value)
Write a register in the BMI160, updating _checked_values.
Definition: bmi160.cpp:395
BMI160(int bus, uint32_t device, enum Rotation rotation)
Definition: bmi160.cpp:52
#define BMIREG_INT_MAP_1
Definition: bmi160.hpp:106
perf_counter_t _duplicates
Definition: bmi160.hpp:286
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
#define BMI_ACCEL_RATE_100
Definition: bmi160.hpp:165
perf_counter_t _bad_registers
Definition: bmi160.hpp:283
perf_counter_t _gyro_reads
Definition: bmi160.hpp:280
unsigned _dlpf_freq
whoami result
Definition: bmi160.hpp:274
void set_error_count(uint64_t error_count)
void perf_end(perf_counter_t handle)
End a performance event.
#define BMI_ACCEL_NORMAL_MODE
Definition: bmi160.hpp:221
#define BMI_GYRO_RATE_800
Definition: bmi160.hpp:187
#define BMI_ACCEL_RATE_25_32
Definition: bmi160.hpp:158
perf_counter_t _reset_retries
Definition: bmi160.hpp:285
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
#define DRV_GYR_DEVTYPE_BMI160
Definition: drv_sensor.h:75
static constexpr int BMI160_NUM_CHECKED_REGISTERS
Definition: bmi160.hpp:294
int16_t gyro_x
Definition: bmi160.hpp:405
void set_scale(float scale)
#define BMI_GYRO_RATE_1600
Definition: bmi160.hpp:188
virtual ~BMI160()
Definition: bmi160.cpp:73
#define DIR_WRITE
Definition: bmp388_spi.cpp:47
#define BMI_ACCEL_RATE_25_4
Definition: bmi160.hpp:161
void check_registers(void)
Definition: bmi160.cpp:523
void set_temperature(float temperature)
#define BMIREG_ACC_RANGE
Definition: bmi160.hpp:88
#define BMI_GYRO_RATE_100
Definition: bmi160.hpp:184
#define BMI_ACCEL_RATE_200
Definition: bmi160.hpp:166
#define BMIREG_STATUS
Definition: bmi160.hpp:77
void test_error()
Definition: bmi160.cpp:345
#define BMI_ACCEL_RATE_800
Definition: bmi160.hpp:168
uint8_t read_reg(unsigned reg)
Read a register from the BMI160.
Definition: bmi160.cpp:353
void print_info()
Diagnostics - print some basic information about the driver.
Definition: bmi160.cpp:696
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.
int16_t accel_z
Definition: bmi160.hpp:410
#define BMIREG_TEMP_0
Definition: bmi160.hpp:82
Definition: bst.cpp:62
#define DRV_ACC_DEVTYPE_BMI160
Definition: drv_sensor.h:70
#define BMI_GYRO_RATE_400
Definition: bmi160.hpp:186
int16_t accel_x
Definition: bmi160.hpp:408
#define BMI_GYRO_RANGE_250_DPS
Definition: bmi160.hpp:198
#define BMIREG_NV_CONF
Definition: bmi160.hpp:130
#define OK
Definition: uavcan_main.cpp:71
#define M_PI_F
Definition: ashtech.cpp:44
uint8_t _whoami
Definition: bmi160.hpp:272
#define BMI_ACCEL_BWP_NORMAL
Definition: bmi160.hpp:171
int set_gyro_range(unsigned max_dps)
Definition: bmi160.cpp:450
#define BMI_GYRO_RANGE_125_DPS
Definition: bmi160.hpp:199
#define BMI_DRDY_INT1
Definition: bmi160.hpp:208
void set_scale(float scale)
perf_counter_t _bad_transfers
Definition: bmi160.hpp:282
#define BMI160_ACCEL_DEFAULT_RATE
Definition: bmi160.hpp:227
#define BMI_ACCEL_RATE_50
Definition: bmi160.hpp:164
#define BMI_ACCEL_RATE_400
Definition: bmi160.hpp:167
void set_device_type(uint8_t devtype)
static const uint8_t _checked_registers[BMI160_NUM_CHECKED_REGISTERS]
Definition: bmi160.hpp:295
int set_accel_range(unsigned max_g)
Set the BMI160 measurement range.
Definition: bmi160.cpp:408
void Run() override
Definition: bmi160.cpp:516
bool _got_duplicate
Definition: bmi160.hpp:302
void perf_begin(perf_counter_t handle)
Begin a performance event.
#define DEVICE_DEBUG(FMT,...)
Definition: Device.hpp:52
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
#define BMI160_SOFT_RESET
Definition: bmi160.hpp:223