PX4 Firmware
PX4 Autopilot Software http://px4.io
BMI088_gyro.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 "BMI088_gyro.hpp"
35 #include "BMI088_accel.hpp"
36 
37 
38 /*
39  * Global variable of the accelerometer temperature reading, to read it in the bmi055_gyro driver.
40  * This is a ugly HACK! The driver should potentially be rewritten with the gyro as subdriver.
41  */
43 
44 /*
45  list of registers that will be checked in check_registers(). Note
46  that ADDR_WHO_AM_I must be first in the list.
47  */
48 
56  };
57 
58 BMI088_gyro::BMI088_gyro(int bus, const char *path_gyro, uint32_t device, enum Rotation rotation) :
59  BMI088("BMI088_GYRO", path_gyro, bus, device, SPIDEV_MODE3, BMI088_BUS_SPEED, rotation),
60  ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
61  _px4_gyro(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation),
62  _sample_perf(perf_alloc(PC_ELAPSED, "bmi088_gyro_read")),
63  _bad_transfers(perf_alloc(PC_COUNT, "bmi088_gyro_bad_transfers")),
64  _bad_registers(perf_alloc(PC_COUNT, "bmi088_gyro_bad_registers"))
65 {
67 }
68 
70 {
71  /* make sure we are truly inactive */
72  stop();
73 
74  /* delete the perf counter */
78 }
79 
80 int
82 {
83  /* do SPI init (and probe) first */
84  int ret = SPI::init();
85 
86  /* if probe/setup failed, bail now */
87  if (ret != OK) {
88  DEVICE_DEBUG("SPI setup failed");
89  return ret;
90  }
91 
92  return reset();
93 }
94 
96 {
98  usleep(5000);
99  write_checked_reg(BMI088_GYR_BW, 0); // Write Gyro Bandwidth (will be overwritten in gyro_set_sample_rate())
100  write_checked_reg(BMI088_GYR_RANGE, 0);// Write Gyro range
102  write_checked_reg(BMI088_GYR_INT_MAP_1, BMI088_GYR_DRDY_INT1); //Map DRDY interrupt on pin INT1
103 
105  gyro_set_sample_rate(BMI088_GYRO_DEFAULT_RATE);// set Gyro ODR & Filter Bandwidth
106 
107  //Enable Gyroscope in normal mode
109  up_udelay(1000);
110 
111  uint8_t retries = 10;
112 
113  while (retries--) {
114  bool all_ok = true;
115 
116  for (uint8_t i = 0; i < BMI088_GYRO_NUM_CHECKED_REGISTERS; i++) {
119  all_ok = false;
120  }
121  }
122 
123  if (all_ok) {
124  break;
125  }
126  }
127 
128  return OK;
129 }
130 
131 int
133 {
134  /* look for device ID */
136 
137  // verify product revision
138  switch (_whoami) {
139  case BMI088_GYR_WHO_AM_I:
140  memset(_checked_values, 0, sizeof(_checked_values));
141  memset(_checked_bad, 0, sizeof(_checked_bad));
143  _checked_bad[0] = _whoami;
144  return OK;
145  }
146 
147  printf("unexpected whoami 0x%02x\n", _whoami);
148  DEVICE_DEBUG("unexpected whoami 0x%02x", _whoami);
149  return -EIO;
150 }
151 
152 int
154 {
155  uint8_t setbits = 0;
156  uint8_t clearbits = BMI088_GYRO_BW_MASK;
157 
158  if (frequency <= 100) {
159  setbits |= BMI088_GYRO_RATE_100; /* 32 Hz cutoff */
160  //_gyro_sample_rate = 100;
161 
162  } else if (frequency <= 250) {
163  setbits |= BMI088_GYRO_RATE_400; /* 47 Hz cutoff */
164  //_gyro_sample_rate = 400;
165 
166  } else if (frequency <= 1000) {
167  setbits |= BMI088_GYRO_RATE_1000; /* 116 Hz cutoff */
168  //_gyro_sample_rate = 1000;
169 
170  } else if (frequency > 1000) {
171  setbits |= BMI088_GYRO_RATE_2000; /* 230 Hz cutoff */
172  //_gyro_sample_rate = 2000;
173 
174  } else {
175  return -EINVAL;
176  }
177 
178  modify_reg(BMI088_GYR_BW, clearbits, setbits);
179 
180  return OK;
181 }
182 
183 /*
184  deliberately trigger an error in the sensor to trigger recovery
185  */
186 void
188 {
190  ::printf("error triggered\n");
191  print_registers();
192 }
193 
194 void
195 BMI088_gyro::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
196 {
197  uint8_t val = read_reg(reg);
198  val &= ~clearbits;
199  val |= setbits;
200  write_checked_reg(reg, val);
201 }
202 
203 void
204 BMI088_gyro::write_checked_reg(unsigned reg, uint8_t value)
205 {
206  write_reg(reg, value);
207 
208  for (uint8_t i = 0; i < BMI088_GYRO_NUM_CHECKED_REGISTERS; i++) {
209  if (reg == _checked_registers[i]) {
210  _checked_values[i] = value;
211  _checked_bad[i] = value;
212  }
213  }
214 }
215 
216 int
218 {
219  uint8_t setbits = 0;
221  float lsb_per_dps;
222 
223  if (max_dps == 0) {
224  max_dps = 2000;
225  }
226 
227  if (max_dps <= 125) {
228  //max_gyro_dps = 125;
229  lsb_per_dps = 262.4;
230  setbits |= BMI088_GYRO_RANGE_125_DPS;
231 
232  } else if (max_dps <= 250) {
233  //max_gyro_dps = 250;
234  lsb_per_dps = 131.2;
235  setbits |= BMI088_GYRO_RANGE_250_DPS;
236 
237  } else if (max_dps <= 500) {
238  //max_gyro_dps = 500;
239  lsb_per_dps = 65.6;
240  setbits |= BMI088_GYRO_RANGE_500_DPS;
241 
242  } else if (max_dps <= 1000) {
243  //max_gyro_dps = 1000;
244  lsb_per_dps = 32.8;
245  setbits |= BMI088_GYRO_RANGE_1000_DPS;
246 
247  } else if (max_dps <= 2000) {
248  //max_gyro_dps = 2000;
249  lsb_per_dps = 16.4;
250  setbits |= BMI088_GYRO_RANGE_2000_DPS;
251 
252  } else {
253  return -EINVAL;
254  }
255 
256  _px4_gyro.set_scale(M_PI_F / (180.0f * lsb_per_dps));
257 
258  modify_reg(BMI088_GYR_RANGE, clearbits, setbits);
259 
260  return OK;
261 }
262 
263 void
265 {
266  /* make sure we are stopped first */
267  stop();
268 
269  /* start polling at the specified rate */
270  ScheduleOnInterval(BMI088_GYRO_DEFAULT_RATE - BMI088_TIMER_REDUCTION, 1000);
271 }
272 
273 void
275 {
276  ScheduleClear();
277 }
278 
279 void
281 {
282  /* make another measurement */
283  measure();
284 }
285 
286 void
288 {
289  BMI088_gyro *dev = static_cast<BMI088_gyro *>(arg);
290 
291  /* make another measurement */
292  dev->measure();
293 }
294 
295 void
297 {
298  uint8_t v;
299 
301  _checked_values[_checked_next]) {
303 
304  /*
305  if we get the wrong value then we know the SPI bus
306  or sensor is very sick. We set _register_wait to 20
307  and wait until we have seen 20 good values in a row
308  before we consider the sensor to be OK again.
309  */
311 
312  /*
313  try to fix the bad register value. We only try to
314  fix one per loop to prevent a bad sensor hogging the
315  bus.
316  */
317  if (_register_wait == 0 || _checked_next == 0) {
318  // if the product_id is wrong then reset the
319  // sensor completely
321  _reset_wait = hrt_absolute_time() + 10000;
322  _checked_next = 0;
323 
324  } else {
325  write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]);
326  // waiting 3ms between register writes seems
327  // to raise the chance of the sensor
328  // recovering considerably
329  _reset_wait = hrt_absolute_time() + 3000;
330  }
331 
332  _register_wait = 20;
333  }
334 
335  _checked_next = (_checked_next + 1) % BMI088_GYRO_NUM_CHECKED_REGISTERS;
336 }
337 
338 void
340 {
341  if (hrt_absolute_time() < _reset_wait) {
342  // we're waiting for a reset to complete
343  return;
344  }
345 
346  struct BMI_GyroReport bmi_gyroreport;
347 
348  struct Report {
349  int16_t gyro_x;
350  int16_t gyro_y;
351  int16_t gyro_z;
352  } report;
353 
354  /* start measuring */
356 
357  /*
358  * Fetch the full set of measurements from the BMI088 gyro in one pass.
359  */
360  bmi_gyroreport.cmd = BMI088_GYR_X_L | DIR_READ;
361 
362  const hrt_abstime timestamp_sample = hrt_absolute_time();
363 
364  if (OK != transfer((uint8_t *)&bmi_gyroreport, ((uint8_t *)&bmi_gyroreport), sizeof(bmi_gyroreport))) {
365  return;
366  }
367 
368  check_registers();
369 
370 
371  // Get the last temperature from the accelerometer (the Gyro does not have its own temperature measurement)
373 
374  report.gyro_x = bmi_gyroreport.gyro_x;
375  report.gyro_y = bmi_gyroreport.gyro_y;
376  report.gyro_z = bmi_gyroreport.gyro_z;
377 
378  if (report.gyro_x == 0 &&
379  report.gyro_y == 0 &&
380  report.gyro_z == 0) {
381  // all zero data - probably an SPI bus error
384  // note that we don't call reset() here as a reset()
385  // costs 20ms with interrupts disabled. That means if
386  // the bmi088 does go bad it would cause a FMU failure,
387  // regardless of whether another sensor is available,
388  return;
389  }
390 
391  if (_register_wait != 0) {
392  // we are waiting for some good transfers before using
393  // the sensor again, but don't return any data yet
394  _register_wait--;
395  return;
396  }
397 
398  // report the error count as the sum of the number of bad
399  // transfers and bad register reads. This allows the higher
400  // level code to decide if it should use this sensor based on
401  // whether it has had failures
402  const uint64_t error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);
403  _px4_gyro.set_error_count(error_count);
404 
405  // Get the temperature from the accelerometer part of the BMI088, because the gyro part does not have a temperature register
407 
408  /*
409  * 1) Scale raw value to SI units using scaling from datasheet.
410  * 2) Subtract static offset (in SI units)
411  * 3) Scale the statically calibrated values with a linear
412  * dynamically obtained factor
413  *
414  * Note: the static sensor offset is the number the sensor outputs
415  * at a nominally 'zero' input. Therefore the offset has to
416  * be subtracted.
417  *
418  * Example: A gyro outputs a value of 74 at zero angular rate
419  * the offset is 74 from the origin and subtracting
420  * 74 from all measurements centers them around zero.
421  */
422  _px4_gyro.update(timestamp_sample, report.gyro_x, report.gyro_y, report.gyro_z);
423 
424  /* stop measuring */
426 }
427 
428 void
430 {
431  PX4_INFO("Gyro");
432 
436 
437  ::printf("checked_next: %u\n", _checked_next);
438 
439  for (uint8_t i = 0; i < BMI088_GYRO_NUM_CHECKED_REGISTERS; i++) {
440  uint8_t v = read_reg(_checked_registers[i]);
441 
442  if (v != _checked_values[i]) {
443  ::printf("reg %02x:%02x should be %02x\n",
444  (unsigned)_checked_registers[i],
445  (unsigned)v,
446  (unsigned)_checked_values[i]);
447  }
448 
449  if (v != _checked_bad[i]) {
450  ::printf("reg %02x:%02x was bad %02x\n",
451  (unsigned)_checked_registers[i],
452  (unsigned)v,
453  (unsigned)_checked_bad[i]);
454  }
455  }
456 
458 }
459 
460 void
462 {
463  uint8_t index = 0;
464  printf("BMI088 gyro registers\n");
465 
466  uint8_t reg = _checked_registers[index++];
467  uint8_t v = read_reg(reg);
468  printf("Gyro Chip Id: %02x:%02x ", (unsigned)reg, (unsigned)v);
469  printf("\n");
470 
471  reg = _checked_registers[index++];
472  v = read_reg(reg);
473  printf("Gyro Power: %02x:%02x ", (unsigned)reg, (unsigned)v);
474  printf("\n");
475 
476  reg = _checked_registers[index++];
477  v = read_reg(reg);
478  printf("Gyro Bw: %02x:%02x ", (unsigned)reg, (unsigned)v);
479  printf("\n");
480 
481  reg = _checked_registers[index++];
482  v = read_reg(reg);
483  printf("Gyro Range: %02x:%02x ", (unsigned)reg, (unsigned)v);
484  printf("\n");
485 
486  reg = _checked_registers[index++];
487  v = read_reg(reg);
488  printf("Gyro Int-en-0: %02x:%02x ", (unsigned)reg, (unsigned)v);
489  printf("\n");
490 
491  reg = _checked_registers[index++];
492  v = read_reg(reg);
493  printf("Gyro Int-en-1: %02x:%02x ", (unsigned)reg, (unsigned)v);
494  printf("\n");
495 
496  reg = _checked_registers[index++];
497  v = read_reg(reg);
498  printf("Gyro Int-Map-1: %02x:%02x ", (unsigned)reg, (unsigned)v);
499 
500  printf("\n");
501 }
#define BMI088_GYRO_RANGE_2000_DPS
uint8_t _checked_values[BMI088_GYRO_NUM_CHECKED_REGISTERS]
#define BMI088_GYRO_RANGE_250_DPS
measure the time elapsed performing an event
Definition: perf_counter.h:56
perf_counter_t _sample_perf
void stop()
Stop automatic measurement.
virtual ~BMI088_gyro()
Definition: BMI088_gyro.cpp:69
void print_status()
#define BMI088_GYRO_RATE_2000
virtual uint8_t read_reg(unsigned reg)
Read a register from the BMI088.
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
Modify a register in the BMI088_gyro.
uint8_t _register_wait
whoami result
Definition: BMI088.hpp:60
Definition: I2C.hpp:51
#define BMI088_GYRO_NORMAL
int reset()
Reset chip.
Definition: BMI088_gyro.cpp:95
void print_registers()
uint8_t _checked_bad[BMI088_GYRO_NUM_CHECKED_REGISTERS]
void check_registers(void)
#define BMI088_GYRO_RANGE_1000_DPS
#define BMI088_GYR_CHIP_ID
Definition: BMI088_gyro.hpp:44
virtual int init()
Definition: BMI088_gyro.cpp:81
static const uint8_t _checked_registers[BMI088_GYRO_NUM_CHECKED_REGISTERS]
__EXPORT float _accel_last_temperature_copy
Definition: BMI088_gyro.cpp:42
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
void set_device_type(uint8_t devtype)
#define BMI088_GYR_INT_MAP_1
Definition: BMI088_gyro.hpp:65
#define BMI088_GYRO_NUM_CHECKED_REGISTERS
#define BMI088_GYR_LPM1
Definition: BMI088_gyro.hpp:58
#define DIR_READ
Definition: bmp388_spi.cpp:46
perf_counter_t _bad_transfers
PX4Gyroscope _px4_gyro
#define BMI088_GYR_BW
Definition: BMI088_gyro.hpp:57
#define BMI088_GYR_X_L
Definition: BMI088_gyro.hpp:45
void update(hrt_abstime timestamp, float x, float y, float z)
void perf_count(perf_counter_t handle)
Count a performance event.
void measure()
Fetch measurements from the sensor and update the report buffers.
#define BMI088_GYRO_RATE_100
Definition: BMI088_gyro.hpp:97
static void measure_trampoline(void *arg)
Static trampoline from the hrt_call context; because we don&#39;t have a generic hrt wrapper yet...
void perf_free(perf_counter_t handle)
Free a counter.
void init()
Activates/configures the hardware registers.
#define perf_alloc(a, b)
Definition: px4io.h:59
void test_error()
Rotation
Enum for board and external compass rotations.
Definition: rotation.h:51
#define BMI088_GYR_DRDY_INT_EN
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
void set_error_count(uint64_t error_count)
void perf_end(perf_counter_t handle)
End a performance event.
#define BMI088_GYR_WHO_AM_I
Definition: BMI088_gyro.hpp:94
void print_info()
Diagnostics - print some basic information about the driver.
#define BMI088_SOFT_RESET
Definition: BMI088.hpp:47
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
#define BMI088_GYRO_DEFAULT_RANGE_DPS
#define BMI088_GYR_INT_EN_0
Definition: BMI088_gyro.hpp:62
perf_counter_t _bad_registers
#define BMI088_GYRO_BW_MASK
Report conversation within the BMI088_gyro, including command byte and interrupt status.
void set_temperature(float temperature)
#define BMI088_GYRO_RANGE_125_DPS
#define BMI088_GYRO_RATE_400
Definition: BMI088_gyro.hpp:99
#define BMI088_GYR_DRDY_INT1
void write_checked_reg(unsigned reg, uint8_t value)
Write a register in the BMI088_gyro, updating _checked_values.
#define BMI088_GYRO_DEFAULT_RATE
float _last_temperature
uint8_t _whoami
Definition: BMI088.hpp:58
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.
uint64_t _reset_wait
Definition: BMI088.hpp:61
void Run() override
Definition: bst.cpp:62
#define BMI088_GYR_SOFTRESET
Definition: BMI088_gyro.hpp:61
#define BMI088_GYRO_RANGE_500_DPS
#define BMI088_BUS_SPEED
Definition: BMI088.hpp:49
#define BMI088_GYR_INT_EN_1
Definition: BMI088_gyro.hpp:63
#define BMI088_TIMER_REDUCTION
Definition: BMI088.hpp:51
#define OK
Definition: uavcan_main.cpp:71
#define M_PI_F
Definition: ashtech.cpp:44
uint8_t _checked_next
Definition: BMI088.hpp:65
int gyro_set_sample_rate(float desired_sample_rate_hz)
void set_scale(float scale)
#define BMI088_GYRO_RATE_1000
#define DRV_DEVTYPE_BMI088
Definition: drv_sensor.h:117
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).
virtual int probe()
int set_gyro_range(unsigned max_dps)
Set the BMI088_gyro measurement range.
#define BMI088_GYR_RANGE
Definition: BMI088_gyro.hpp:56
BMI088_gyro(int bus, const char *path_gyro, uint32_t device, enum Rotation rotation)
Definition: BMI088_gyro.cpp:58
void write_reg(unsigned reg, uint8_t value)
Write a register in the BMI088.