PX4 Firmware
PX4 Autopilot Software http://px4.io
BMI055_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 "BMI055_gyro.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 
48  };
49 
50 BMI055_gyro::BMI055_gyro(int bus, const char *path_gyro, uint32_t device, enum Rotation rotation) :
51  BMI055("BMI055_GYRO", path_gyro, bus, device, SPIDEV_MODE3, BMI055_BUS_SPEED, rotation),
52  ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
53  _px4_gyro(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation),
54  _sample_perf(perf_alloc(PC_ELAPSED, "bmi055_gyro_read")),
55  _bad_transfers(perf_alloc(PC_COUNT, "bmi055_gyro_bad_transfers")),
56  _bad_registers(perf_alloc(PC_COUNT, "bmi055_gyro_bad_registers"))
57 {
59 }
60 
62 {
63  /* make sure we are truly inactive */
64  stop();
65 
66  /* delete the perf counter */
70 }
71 
72 int
74 {
75  /* do SPI init (and probe) first */
76  int ret = SPI::init();
77 
78  /* if probe/setup failed, bail now */
79  if (ret != OK) {
80  DEVICE_DEBUG("SPI setup failed");
81  return ret;
82  }
83 
84  return reset();
85 }
86 
88 {
90  usleep(5000);
91  write_checked_reg(BMI055_GYR_BW, 0); // Write Gyro Bandwidth (will be overwritten in gyro_set_sample_rate())
92  write_checked_reg(BMI055_GYR_RANGE, 0);// Write Gyro range
94  write_checked_reg(BMI055_GYR_INT_MAP_1, BMI055_GYR_DRDY_INT1); //Map DRDY interrupt on pin INT1
95 
97  gyro_set_sample_rate(BMI055_GYRO_DEFAULT_RATE);// set Gyro ODR & Filter Bandwidth
98 
99  //Enable Gyroscope in normal mode
101  up_udelay(1000);
102 
103  uint8_t retries = 10;
104 
105  while (retries--) {
106  bool all_ok = true;
107 
108  for (uint8_t i = 0; i < BMI055_GYRO_NUM_CHECKED_REGISTERS; i++) {
111  all_ok = false;
112  }
113  }
114 
115  if (all_ok) {
116  break;
117  }
118  }
119 
120  return OK;
121 }
122 
123 int
125 {
126  /* look for device ID */
128 
129  // verify product revision
130  switch (_whoami) {
131  case BMI055_GYR_WHO_AM_I:
132  memset(_checked_values, 0, sizeof(_checked_values));
133  memset(_checked_bad, 0, sizeof(_checked_bad));
135  _checked_bad[0] = _whoami;
136  return OK;
137  }
138 
139  DEVICE_DEBUG("unexpected whoami 0x%02x", _whoami);
140  return -EIO;
141 }
142 
143 int
145 {
146  uint8_t setbits = 0;
147  uint8_t clearbits = BMI055_GYRO_BW_MASK;
148 
149  if (frequency <= 100) {
150  setbits |= BMI055_GYRO_RATE_100; /* 32 Hz cutoff */
151  //_gyro_sample_rate = 100;
152 
153  } else if (frequency <= 250) {
154  setbits |= BMI055_GYRO_RATE_400; /* 47 Hz cutoff */
155  //_gyro_sample_rate = 400;
156 
157  } else if (frequency <= 1000) {
158  setbits |= BMI055_GYRO_RATE_1000; /* 116 Hz cutoff */
159  //_gyro_sample_rate = 1000;
160 
161  } else if (frequency > 1000) {
162  setbits |= BMI055_GYRO_RATE_2000; /* 230 Hz cutoff */
163  //_gyro_sample_rate = 2000;
164 
165  } else {
166  return -EINVAL;
167  }
168 
169  modify_reg(BMI055_GYR_BW, clearbits, setbits);
170 
171  return OK;
172 }
173 
174 /*
175  deliberately trigger an error in the sensor to trigger recovery
176  */
177 void
179 {
181  ::printf("error triggered\n");
182  print_registers();
183 }
184 
185 void
186 BMI055_gyro::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
187 {
188  uint8_t val = read_reg(reg);
189  val &= ~clearbits;
190  val |= setbits;
191  write_checked_reg(reg, val);
192 }
193 
194 void
195 BMI055_gyro::write_checked_reg(unsigned reg, uint8_t value)
196 {
197  write_reg(reg, value);
198 
199  for (uint8_t i = 0; i < BMI055_GYRO_NUM_CHECKED_REGISTERS; i++) {
200  if (reg == _checked_registers[i]) {
201  _checked_values[i] = value;
202  _checked_bad[i] = value;
203  }
204  }
205 }
206 
207 int
209 {
210  uint8_t setbits = 0;
212  float lsb_per_dps;
213 
214  if (max_dps == 0) {
215  max_dps = 2000;
216  }
217 
218  if (max_dps <= 125) {
219  //max_gyro_dps = 125;
220  lsb_per_dps = 262.4;
221  setbits |= BMI055_GYRO_RANGE_125_DPS;
222 
223  } else if (max_dps <= 250) {
224  //max_gyro_dps = 250;
225  lsb_per_dps = 131.2;
226  setbits |= BMI055_GYRO_RANGE_250_DPS;
227 
228  } else if (max_dps <= 500) {
229  //max_gyro_dps = 500;
230  lsb_per_dps = 65.6;
231  setbits |= BMI055_GYRO_RANGE_500_DPS;
232 
233  } else if (max_dps <= 1000) {
234  //max_gyro_dps = 1000;
235  lsb_per_dps = 32.8;
236  setbits |= BMI055_GYRO_RANGE_1000_DPS;
237 
238  } else if (max_dps <= 2000) {
239  //max_gyro_dps = 2000;
240  lsb_per_dps = 16.4;
241  setbits |= BMI055_GYRO_RANGE_2000_DPS;
242 
243  } else {
244  return -EINVAL;
245  }
246 
247  _px4_gyro.set_scale(M_PI_F / (180.0f * lsb_per_dps));
248 
249  modify_reg(BMI055_GYR_RANGE, clearbits, setbits);
250 
251  return OK;
252 }
253 
254 void
256 {
257  /* make sure we are stopped first */
258  stop();
259 
260  /* start polling at the specified rate */
261  ScheduleOnInterval(BMI055_GYRO_DEFAULT_RATE - BMI055_TIMER_REDUCTION, 1000);
262 }
263 
264 void
266 {
267  ScheduleClear();
268 }
269 
270 void
272 {
273  /* make another measurement */
274  measure();
275 }
276 
277 void
279 {
280  uint8_t v;
281 
283  _checked_values[_checked_next]) {
285 
286  /*
287  if we get the wrong value then we know the SPI bus
288  or sensor is very sick. We set _register_wait to 20
289  and wait until we have seen 20 good values in a row
290  before we consider the sensor to be OK again.
291  */
293 
294  /*
295  try to fix the bad register value. We only try to
296  fix one per loop to prevent a bad sensor hogging the
297  bus.
298  */
299  if (_register_wait == 0 || _checked_next == 0) {
300  // if the product_id is wrong then reset the
301  // sensor completely
303  _reset_wait = hrt_absolute_time() + 10000;
304  _checked_next = 0;
305 
306  } else {
307  write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]);
308  // waiting 3ms between register writes seems
309  // to raise the chance of the sensor
310  // recovering considerably
311  _reset_wait = hrt_absolute_time() + 3000;
312  }
313 
314  _register_wait = 20;
315  }
316 
317  _checked_next = (_checked_next + 1) % BMI055_GYRO_NUM_CHECKED_REGISTERS;
318 }
319 
320 void
322 {
323  if (hrt_absolute_time() < _reset_wait) {
324  // we're waiting for a reset to complete
325  return;
326  }
327 
328  struct BMI_GyroReport bmi_gyroreport;
329 
330  struct Report {
331  int16_t temp;
332  int16_t gyro_x;
333  int16_t gyro_y;
334  int16_t gyro_z;
335  } report;
336 
337  /* start measuring */
339 
340  /*
341  * Fetch the full set of measurements from the BMI055 gyro in one pass.
342  */
343  bmi_gyroreport.cmd = BMI055_GYR_X_L | DIR_READ;
344 
345  const hrt_abstime timestamp_sample = hrt_absolute_time();
346 
347  if (OK != transfer((uint8_t *)&bmi_gyroreport, ((uint8_t *)&bmi_gyroreport), sizeof(bmi_gyroreport))) {
348  return;
349  }
350 
351  check_registers();
352 
353  int8_t temp = read_reg(BMI055_ACC_TEMP);
354  report.temp = temp;
355 
356  report.gyro_x = bmi_gyroreport.gyro_x;
357  report.gyro_y = bmi_gyroreport.gyro_y;
358  report.gyro_z = bmi_gyroreport.gyro_z;
359 
360  if (report.temp == 0 &&
361  report.gyro_x == 0 &&
362  report.gyro_y == 0 &&
363  report.gyro_z == 0) {
364  // all zero data - probably a SPI bus error
367  // note that we don't call reset() here as a reset()
368  // costs 20ms with interrupts disabled. That means if
369  // the bmi055 does go bad it would cause a FMU failure,
370  // regardless of whether another sensor is available,
371  return;
372  }
373 
374  if (_register_wait != 0) {
375  // we are waiting for some good transfers before using
376  // the sensor again, but don't return any data yet
377  _register_wait--;
378  return;
379  }
380 
381  // report the error count as the sum of the number of bad
382  // transfers and bad register reads. This allows the higher
383  // level code to decide if it should use this sensor based on
384  // whether it has had failures
385  const uint64_t error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);
386  _px4_gyro.set_error_count(error_count);
387 
388  /*
389  * Temperature is reported as Eight-bit 2’s complement sensor temperature value
390  * with 0.5 °C/LSB sensitivity and an offset of 23.0 °C
391  */
392  _px4_gyro.set_temperature((report.temp * 0.5f) + 23.0f);
393 
394  /*
395  * 1) Scale raw value to SI units using scaling from datasheet.
396  * 2) Subtract static offset (in SI units)
397  * 3) Scale the statically calibrated values with a linear
398  * dynamically obtained factor
399  *
400  * Note: the static sensor offset is the number the sensor outputs
401  * at a nominally 'zero' input. Therefore the offset has to
402  * be subtracted.
403  *
404  * Example: A gyro outputs a value of 74 at zero angular rate
405  * the offset is 74 from the origin and subtracting
406  * 74 from all measurements centers them around zero.
407  */
408  _px4_gyro.update(timestamp_sample, report.gyro_x, report.gyro_y, report.gyro_z);
409 
410  /* stop measuring */
412 }
413 
414 void
416 {
417  PX4_INFO("Gyro");
418 
422 
423  ::printf("checked_next: %u\n", _checked_next);
424 
425  for (uint8_t i = 0; i < BMI055_GYRO_NUM_CHECKED_REGISTERS; i++) {
426  uint8_t v = read_reg(_checked_registers[i]);
427 
428  if (v != _checked_values[i]) {
429  ::printf("reg %02x:%02x should be %02x\n",
430  (unsigned)_checked_registers[i],
431  (unsigned)v,
432  (unsigned)_checked_values[i]);
433  }
434 
435  if (v != _checked_bad[i]) {
436  ::printf("reg %02x:%02x was bad %02x\n",
437  (unsigned)_checked_registers[i],
438  (unsigned)v,
439  (unsigned)_checked_bad[i]);
440  }
441  }
442 
444 }
445 
446 void
448 {
449  uint8_t index = 0;
450  printf("BMI055 gyro registers\n");
451 
452  uint8_t reg = _checked_registers[index++];
453  uint8_t v = read_reg(reg);
454  printf("Gyro Chip Id: %02x:%02x ", (unsigned)reg, (unsigned)v);
455  printf("\n");
456 
457  reg = _checked_registers[index++];
458  v = read_reg(reg);
459  printf("Gyro Power: %02x:%02x ", (unsigned)reg, (unsigned)v);
460  printf("\n");
461 
462  reg = _checked_registers[index++];
463  v = read_reg(reg);
464  printf("Gyro Bw: %02x:%02x ", (unsigned)reg, (unsigned)v);
465  printf("\n");
466 
467  reg = _checked_registers[index++];
468  v = read_reg(reg);
469  printf("Gyro Range: %02x:%02x ", (unsigned)reg, (unsigned)v);
470  printf("\n");
471 
472  reg = _checked_registers[index++];
473  v = read_reg(reg);
474  printf("Gyro Int-en-0: %02x:%02x ", (unsigned)reg, (unsigned)v);
475  printf("\n");
476 
477  reg = _checked_registers[index++];
478  v = read_reg(reg);
479  printf("Gyro Int-en-1: %02x:%02x ", (unsigned)reg, (unsigned)v);
480  printf("\n");
481 
482  reg = _checked_registers[index++];
483  v = read_reg(reg);
484  printf("Gyro Int-Map-1: %02x:%02x ", (unsigned)reg, (unsigned)v);
485 
486  printf("\n");
487 }
#define BMI055_GYRO_RATE_2000
PX4Gyroscope _px4_gyro
#define BMI055_GYRO_DEFAULT_RANGE_DPS
#define BMI055_GYR_CHIP_ID
Definition: BMI055_gyro.hpp:44
#define BMI055_GYR_WHO_AM_I
Definition: BMI055_gyro.hpp:94
measure the time elapsed performing an event
Definition: perf_counter.h:56
void print_status()
#define BMI055_GYR_BW
Definition: BMI055_gyro.hpp:57
#define BMI055_GYR_RANGE
Definition: BMI055_gyro.hpp:56
int gyro_set_sample_rate(float desired_sample_rate_hz)
#define BMI055_SOFT_RESET
Definition: BMI055.hpp:46
uint8_t _checked_bad[BMI055_GYRO_NUM_CHECKED_REGISTERS]
void check_registers(void)
Report conversation within the BMI055_gyro, including command byte and interrupt status.
#define BMI055_GYRO_RANGE_1000_DPS
#define BMI055_GYRO_DEFAULT_RATE
#define BMI055_GYRO_RANGE_125_DPS
uint64_t _reset_wait
Definition: BMI055.hpp:60
#define BMI055_GYRO_NORMAL
int set_gyro_range(unsigned max_dps)
Set the BMI055_gyro measurement range.
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
int reset()
Reset chip.
Definition: BMI055_gyro.cpp:87
#define BMI055_GYRO_BW_MASK
#define BMI055_TIMER_REDUCTION
Definition: BMI055.hpp:50
#define BMI055_GYRO_RATE_400
Definition: BMI055_gyro.hpp:99
void write_reg(unsigned reg, uint8_t value)
Write a register in the BMI055.
void set_device_type(uint8_t devtype)
#define BMI055_GYR_DRDY_INT_EN
#define BMI055_GYR_X_L
Definition: BMI055_gyro.hpp:45
virtual int init()
Definition: BMI055_gyro.cpp:73
#define BMI055_BUS_SPEED
Definition: BMI055.hpp:48
#define BMI055_ACC_TEMP
uint8_t read_reg(unsigned reg)
Read a register from the BMI055.
#define DIR_READ
Definition: bmp388_spi.cpp:46
void print_registers()
#define BMI055_GYR_LPM1
Definition: BMI055_gyro.hpp:58
#define BMI055_GYRO_RANGE_500_DPS
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.
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
Rotation
Enum for board and external compass rotations.
Definition: rotation.h:51
#define BMI055_GYR_SOFTRESET
Definition: BMI055_gyro.hpp:61
perf_counter_t _bad_registers
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
#define BMI055_GYRO_RATE_100
Definition: BMI055_gyro.hpp:97
static const uint8_t _checked_registers[BMI055_GYRO_NUM_CHECKED_REGISTERS]
#define BMI055_GYRO_RANGE_2000_DPS
void set_error_count(uint64_t error_count)
void perf_end(perf_counter_t handle)
End a performance event.
#define BMI055_GYR_INT_EN_1
Definition: BMI055_gyro.hpp:63
uint8_t _register_wait
whoami result
Definition: BMI055.hpp:59
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
perf_counter_t _bad_transfers
uint8_t _whoami
Definition: BMI055.hpp:57
#define DRV_GYR_DEVTYPE_BMI055
Definition: drv_sensor.h:93
#define BMI055_GYR_INT_EN_0
Definition: BMI055_gyro.hpp:62
void stop()
Stop automatic measurement.
void set_temperature(float temperature)
uint8_t _checked_values[BMI055_GYRO_NUM_CHECKED_REGISTERS]
#define BMI055_GYRO_NUM_CHECKED_REGISTERS
#define BMI055_GYRO_RANGE_250_DPS
virtual int probe()
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 modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
Modify a register in the BMI055_gyro.
BMI055_gyro(int bus, const char *path_gyro, uint32_t device, enum Rotation rotation)
Definition: BMI055_gyro.cpp:50
Definition: bst.cpp:62
void write_checked_reg(unsigned reg, uint8_t value)
Write a register in the BMI055_gyro, updating _checked_values.
#define OK
Definition: uavcan_main.cpp:71
#define M_PI_F
Definition: ashtech.cpp:44
uint8_t _checked_next
Definition: BMI055.hpp:64
void set_scale(float scale)
virtual ~BMI055_gyro()
Definition: BMI055_gyro.cpp:61
#define BMI055_GYRO_RATE_1000
#define BMI055_GYR_DRDY_INT1
void Run() override
void perf_begin(perf_counter_t handle)
Begin a performance event.
perf_counter_t _sample_perf
#define DEVICE_DEBUG(FMT,...)
Definition: Device.hpp:52
#define BMI055_GYR_INT_MAP_1
Definition: BMI055_gyro.hpp:65
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
void test_error()
void print_info()
Diagnostics - print some basic information about the driver.