PX4 Firmware
PX4 Autopilot Software http://px4.io
BMI055_accel.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_accel.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  */
45  };
46 
47 BMI055_accel::BMI055_accel(int bus, const char *path_accel, uint32_t device, enum Rotation rotation) :
48  BMI055("BMI055_ACCEL", path_accel, bus, device, SPIDEV_MODE3, BMI055_BUS_SPEED, rotation),
49  ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
50  _px4_accel(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation),
51  _sample_perf(perf_alloc(PC_ELAPSED, "bmi055_accel_read")),
52  _bad_transfers(perf_alloc(PC_COUNT, "bmi055_accel_bad_transfers")),
53  _bad_registers(perf_alloc(PC_COUNT, "bmi055_accel_bad_registers")),
54  _duplicates(perf_alloc(PC_COUNT, "bmi055_accel_duplicates")),
55  _got_duplicate(false)
56 {
58 }
59 
61 {
62  /* make sure we are truly inactive */
63  stop();
64 
65  /* 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  up_udelay(5000);
91 
92  write_checked_reg(BMI055_ACC_BW, BMI055_ACCEL_BW_500); //Write accel bandwidth (DLPF)
95  write_checked_reg(BMI055_ACC_INT_MAP_1, BMI055_ACC_DRDY_INT1); //Map DRDY interrupt on pin INT1
96 
98 
99  //Enable Accelerometer 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_ACCEL_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_ACC_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 /*
144  deliberately trigger an error in the sensor to trigger recovery
145  */
146 void
148 {
150  ::printf("error triggered\n");
151  print_registers();
152 }
153 
154 void
155 BMI055_accel::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
156 {
157  uint8_t val = read_reg(reg);
158  val &= ~clearbits;
159  val |= setbits;
160  write_checked_reg(reg, val);
161 }
162 
163 void
164 BMI055_accel::write_checked_reg(unsigned reg, uint8_t value)
165 {
166  write_reg(reg, value);
167 
168  for (uint8_t i = 0; i < BMI055_ACCEL_NUM_CHECKED_REGISTERS; i++) {
169  if (reg == _checked_registers[i]) {
170  _checked_values[i] = value;
171  _checked_bad[i] = value;
172  }
173  }
174 }
175 
176 int
178 {
179  uint8_t setbits = 0;
180  uint8_t clearbits = BMI055_ACCEL_RANGE_2_G | BMI055_ACCEL_RANGE_16_G;
181  float lsb_per_g;
182 
183  if (max_g == 0) {
184  max_g = 16;
185  }
186 
187  if (max_g <= 2) {
188  //max_accel_g = 2;
189  setbits |= BMI055_ACCEL_RANGE_2_G;
190  lsb_per_g = 1024;
191 
192  } else if (max_g <= 4) {
193  //max_accel_g = 4;
194  setbits |= BMI055_ACCEL_RANGE_4_G;
195  lsb_per_g = 512;
196 
197  } else if (max_g <= 8) {
198  //max_accel_g = 8;
199  setbits |= BMI055_ACCEL_RANGE_8_G;
200  lsb_per_g = 256;
201 
202  } else if (max_g <= 16) {
203  //max_accel_g = 16;
204  setbits |= BMI055_ACCEL_RANGE_16_G;
205  lsb_per_g = 128;
206 
207  } else {
208  return -EINVAL;
209  }
210 
211  _px4_accel.set_scale(CONSTANTS_ONE_G / lsb_per_g);
212 
213  modify_reg(BMI055_ACC_RANGE, clearbits, setbits);
214 
215  return OK;
216 }
217 
218 void
220 {
221  /* make sure we are stopped first */
222  stop();
223 
224  /* start polling at the specified rate */
225  ScheduleOnInterval(BMI055_ACCEL_DEFAULT_RATE - BMI055_TIMER_REDUCTION, 1000);
226 }
227 
228 void
230 {
231  ScheduleClear();
232 }
233 
234 void
236 {
237  /* make another measurement */
238  measure();
239 }
240 
241 void
243 {
244  uint8_t v;
245 
247  _checked_values[_checked_next]) {
249 
250  /*
251  if we get the wrong value then we know the SPI bus
252  or sensor is very sick. We set _register_wait to 20
253  and wait until we have seen 20 good values in a row
254  before we consider the sensor to be OK again.
255  */
257 
258  /*
259  try to fix the bad register value. We only try to
260  fix one per loop to prevent a bad sensor hogging the
261  bus.
262  */
263  if (_register_wait == 0 || _checked_next == 0) {
264  // if the product_id is wrong then reset the
265  // sensor completely
267  _reset_wait = hrt_absolute_time() + 10000;
268  _checked_next = 0;
269 
270  } else {
271  write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]);
272  // waiting 3ms between register writes seems
273  // to raise the chance of the sensor
274  // recovering considerably
275  _reset_wait = hrt_absolute_time() + 3000;
276  }
277 
278  _register_wait = 20;
279  }
280 
281  _checked_next = (_checked_next + 1) % BMI055_ACCEL_NUM_CHECKED_REGISTERS;
282 }
283 
284 void
286 {
287  if (hrt_absolute_time() < _reset_wait) {
288  // we're waiting for a reset to complete
289  return;
290  }
291 
292  struct Report {
293  int16_t accel_x;
294  int16_t accel_y;
295  int16_t accel_z;
296  int8_t temp;
297  } report;
298 
299  /* start measuring */
301 
302  /*
303  * Fetch the full set of measurements from the BMI055 in one pass.
304  */
305  uint8_t index = 0;
306  uint8_t accel_data[8] {};
307  accel_data[index] = BMI055_ACC_X_L | DIR_READ;
308 
309  const hrt_abstime timestamp_sample = hrt_absolute_time();
310 
311  if (OK != transfer(accel_data, accel_data, sizeof(accel_data))) {
312  return;
313  }
314 
315  check_registers();
316 
317  /* Extracting accel data from the read data */
318  index = 1;
319  uint16_t lsb, msb, msblsb;
320 
321  lsb = (uint16_t)accel_data[index++];
322  uint8_t status_x = (lsb & BMI055_NEW_DATA_MASK);
323  msb = (uint16_t)accel_data[index++];
324  msblsb = (msb << 8) | lsb;
325  report.accel_x = ((int16_t)msblsb >> 4); /* Data in X axis */
326 
327  lsb = (uint16_t)accel_data[index++];
328  uint8_t status_y = (lsb & BMI055_NEW_DATA_MASK);
329  msb = (uint16_t)accel_data[index++];
330  msblsb = (msb << 8) | lsb;
331  report.accel_y = ((int16_t)msblsb >> 4); /* Data in Y axis */
332 
333  lsb = (uint16_t)accel_data[index++];
334  uint8_t status_z = (lsb & BMI055_NEW_DATA_MASK);
335  msb = (uint16_t)accel_data[index++];
336  msblsb = (msb << 8) | lsb;
337  report.accel_z = ((int16_t)msblsb >> 4); /* Data in Z axis */
338 
339  // Byte
340  report.temp = accel_data[index];
341 
342  // Checking the status of new data
343  if ((!status_x) || (!status_y) || (!status_z)) {
346  _got_duplicate = true;
347  return;
348  }
349 
350 
351  _got_duplicate = false;
352 
353  if (report.accel_x == 0 &&
354  report.accel_y == 0 &&
355  report.accel_z == 0 &&
356  report.temp == 0) {
357  // all zero data - probably a SPI bus error
360  // note that we don't call reset() here as a reset()
361  // costs 20ms with interrupts disabled. That means if
362  // the bmi055 accel does go bad it would cause a FMU failure,
363  // regardless of whether another sensor is available,
364  return;
365  }
366 
367  if (_register_wait != 0) {
368  // we are waiting for some good transfers before using
369  // the sensor again, but don't return any data yet
370  _register_wait--;
371  return;
372  }
373 
374  // report the error count as the sum of the number of bad
375  // transfers and bad register reads. This allows the higher
376  // level code to decide if it should use this sensor based on
377  // whether it has had failures
378  const uint64_t error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);
379  _px4_accel.set_error_count(error_count);
380 
381  /*
382  * Temperature is reported as Eight-bit 2’s complement sensor temperature value
383  * with 0.5 °C/LSB sensitivity and an offset of 23.0 °C
384  */
385  _px4_accel.set_temperature((report.temp * 0.5f) + 23.0f);
386 
387  /*
388  * 1) Scale raw value to SI units using scaling from datasheet.
389  * 2) Subtract static offset (in SI units)
390  * 3) Scale the statically calibrated values with a linear
391  * dynamically obtained factor
392  *
393  * Note: the static sensor offset is the number the sensor outputs
394  * at a nominally 'zero' input. Therefore the offset has to
395  * be subtracted.
396  *
397  */
398  _px4_accel.update(timestamp_sample, report.accel_x, report.accel_y, report.accel_z);
399 
400  /* stop measuring */
402 }
403 
404 void
406 {
411 
412  ::printf("checked_next: %u\n", _checked_next);
413 
414  for (uint8_t i = 0; i < BMI055_ACCEL_NUM_CHECKED_REGISTERS; i++) {
415  uint8_t v = read_reg(_checked_registers[i]);
416 
417  if (v != _checked_values[i]) {
418  ::printf("reg %02x:%02x should be %02x\n",
419  (unsigned)_checked_registers[i],
420  (unsigned)v,
421  (unsigned)_checked_values[i]);
422  }
423 
424  if (v != _checked_bad[i]) {
425  ::printf("reg %02x:%02x was bad %02x\n",
426  (unsigned)_checked_registers[i],
427  (unsigned)v,
428  (unsigned)_checked_bad[i]);
429  }
430  }
431 
433 }
434 
435 void
437 {
438  uint8_t index = 0;
439  printf("BMI055 accel registers\n");
440 
441  uint8_t reg = _checked_registers[index++];
442  uint8_t v = read_reg(reg);
443  printf("Accel Chip Id: %02x:%02x ", (unsigned)reg, (unsigned)v);
444  printf("\n");
445 
446  reg = _checked_registers[index++];
447  v = read_reg(reg);
448  printf("Accel Bw: %02x:%02x ", (unsigned)reg, (unsigned)v);
449  printf("\n");
450 
451  reg = _checked_registers[index++];
452  v = read_reg(reg);
453  printf("Accel Range: %02x:%02x ", (unsigned)reg, (unsigned)v);
454  printf("\n");
455 
456  reg = _checked_registers[index++];
457  v = read_reg(reg);
458  printf("Accel Int-en-1: %02x:%02x ", (unsigned)reg, (unsigned)v);
459  printf("\n");
460 
461  reg = _checked_registers[index++];
462  v = read_reg(reg);
463  printf("Accel Int-Map-1: %02x:%02x ", (unsigned)reg, (unsigned)v);
464 
465  printf("\n");
466 }
virtual ~BMI055_accel()
#define BMI055_ACC_INT_MAP_1
measure the time elapsed performing an event
Definition: perf_counter.h:56
#define BMI055_ACC_PMU_LPW
#define BMI055_ACC_RANGE
void update(hrt_abstime timestamp, float x, float y, float z)
#define BMI055_ACC_DRDY_INT1
#define BMI055_ACCEL_RANGE_16_G
#define BMI055_SOFT_RESET
Definition: BMI055.hpp:46
void print_info()
Diagnostics - print some basic information about the driver.
#define BMI055_ACCEL_DEFAULT_RATE
uint8_t _checked_values[BMI055_ACCEL_NUM_CHECKED_REGISTERS]
uint64_t _reset_wait
Definition: BMI055.hpp:60
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 BMI055_NEW_DATA_MASK
#define BMI055_TIMER_REDUCTION
Definition: BMI055.hpp:50
static const uint8_t _checked_registers[BMI055_ACCEL_NUM_CHECKED_REGISTERS]
void write_reg(unsigned reg, uint8_t value)
Write a register in the BMI055.
static constexpr float CONSTANTS_ONE_G
Definition: geo.h:51
void set_error_count(uint64_t error_count)
#define BMI055_BUS_SPEED
Definition: BMI055.hpp:48
uint8_t read_reg(unsigned reg)
Read a register from the BMI055.
#define DIR_READ
Definition: bmp388_spi.cpp:46
virtual int init()
perf_counter_t _duplicates
void print_registers()
perf_counter_t _sample_perf
void perf_count(perf_counter_t handle)
Count a performance event.
int reset()
Reset chip.
#define BMI055_ACC_INT_EN_1
void perf_free(perf_counter_t handle)
Free a counter.
void init()
Activates/configures the hardware registers.
void set_temperature(float temperature)
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
Modify a register in the BMI055_accel.
#define BMI055_ACCEL_RANGE_8_G
#define perf_alloc(a, b)
Definition: px4io.h:59
Rotation
Enum for board and external compass rotations.
Definition: rotation.h:51
uint8_t _checked_bad[BMI055_ACCEL_NUM_CHECKED_REGISTERS]
#define BMI055_ACC_SOFTRESET
void Run() override
void measure()
Fetch measurements from the sensor and update the report buffers.
int set_accel_range(unsigned max_g)
Set the BMI055_accel measurement range.
void perf_end(perf_counter_t handle)
End a performance event.
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
#define DRV_ACC_DEVTYPE_BMI055
Definition: drv_sensor.h:92
#define BMI055_ACC_BW
void set_scale(float scale)
uint8_t _whoami
Definition: BMI055.hpp:57
void write_checked_reg(unsigned reg, uint8_t value)
Write a register in the BMI055_accel, updating _checked_values.
#define BMI055_ACCEL_DEFAULT_RANGE_G
#define BMI055_ACCEL_RANGE_4_G
PX4Accelerometer _px4_accel
perf_counter_t _bad_registers
#define BMI055_ACCEL_NUM_CHECKED_REGISTERS
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
#define BMI055_ACC_WHO_AM_I
uint64_t perf_event_count(perf_counter_t handle)
Return current event_count.
virtual int probe()
Definition: bst.cpp:62
#define BMI055_ACCEL_RANGE_2_G
#define OK
Definition: uavcan_main.cpp:71
BMI055_accel(int bus, const char *path_accel, uint32_t device, enum Rotation rotation)
uint8_t _checked_next
Definition: BMI055.hpp:64
#define BMI055_ACCEL_NORMAL
void check_registers(void)
void set_device_type(uint8_t devtype)
#define BMI055_ACC_CHIP_ID
#define BMI055_ACCEL_BW_500
#define BMI055_ACC_DRDY_INT_EN
void perf_begin(perf_counter_t handle)
Begin a performance event.
#define BMI055_ACC_X_L
perf_counter_t _bad_transfers
#define DEVICE_DEBUG(FMT,...)
Definition: Device.hpp:52
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
void stop()
Stop automatic measurement.