Browse Source
The reason of defining BMI160_MAX_FIFO_SAMPLES as 8 can be found on the
following histogram of the number of samples in the FIFO on each read while
performing the accelerometer calibration process:
Samples Count Freq Acc. Freq
------------------------------
1 3842 0.1201 0.120111
2 13172 0.4118 0.531904
3 9065 0.2834 0.815300
4 2710 0.0847 0.900022
5 2231 0.0697 0.969769
6 816 0.0255 0.995279
7 137 0.0043 0.999562
8 13 0.0004 0.999969
13 1 0.0000 1.000000
master
Gustavo Jose de Sousa
9 years ago
committed by
Lucas De Marchi
3 changed files with 621 additions and 0 deletions
@ -0,0 +1,501 @@
@@ -0,0 +1,501 @@
|
||||
/*
|
||||
* Copyright (C) 2016 Intel Corporation. All rights reserved. |
||||
* |
||||
* This file is free software: you can redistribute it and/or modify it |
||||
* under the terms of the GNU General Public License as published by the |
||||
* Free Software Foundation, either version 3 of the License, or |
||||
* (at your option) any later version. |
||||
* |
||||
* This file is distributed in the hope that it will be useful, but |
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of |
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. |
||||
* See the GNU General Public License for more details. |
||||
* |
||||
* You should have received a copy of the GNU General Public License along |
||||
* with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/ |
||||
#include <utility> |
||||
|
||||
#include <AP_HAL/AP_HAL.h> |
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX |
||||
|
||||
#include <AP_HAL/utility/sparse-endian.h> |
||||
#include <AP_HAL_Linux/GPIO.h> |
||||
#include <AP_Math/AP_Math.h> |
||||
|
||||
#include "AP_InertialSensor_BMI160.h" |
||||
|
||||
/* Registers and bits definitions. The indented ones are the bits for the upper
|
||||
* register. */ |
||||
#define BMI160_REG_CHIPID 0x00 |
||||
#define BMI160_CHIPID 0xD1 |
||||
#define BMI160_REG_ERR_REG 0x02 |
||||
#define BMI160_REG_FIFO_LENGTH 0x22 |
||||
#define BMI160_REG_FIFO_DATA 0x24 |
||||
#define BMI160_REG_ACC_CONF 0x40 |
||||
#define BMI160_REG_ACC_RANGE 0x41 |
||||
/* For convenience, use log2(range) - 1 instead of bits defined in
|
||||
* the datasheet. See _configure_accel(). */ |
||||
#define BMI160_ACC_RANGE_16G 3 |
||||
#define BMI160_REG_GYR_CONF 0x42 |
||||
#define BMI160_REG_GYR_RANGE 0x43 |
||||
#define BMI160_GYR_RANGE_2000DPS 0x00 |
||||
#define BMI160_REG_FIFO_CONFIG_0 0x46 |
||||
#define BMI160_REG_FIFO_CONFIG_1 0x47 |
||||
#define BMI160_FIFO_ACC_EN 0x40 |
||||
#define BMI160_FIFO_GYR_EN 0x80 |
||||
#define BMI160_REG_INT_EN_1 0x51 |
||||
#define BMI160_INT_FWM_EN 0x40 |
||||
#define BMI160_REG_INT_OUT_CTRL 0x53 |
||||
#define BMI160_INT1_LVL 0x02 |
||||
#define BMI160_INT1_OUTPUT_EN 0x08 |
||||
#define BMI160_REG_INT_MAP_1 0x56 |
||||
#define BMI160_INT_MAP_INT1_FWM 0x40 |
||||
#define BMI160_REG_CMD 0x7E |
||||
#define BMI160_CMD_ACCEL_NORMAL_POWER_MODE 0x11 |
||||
#define BMI160_CMD_GYRO_NORMAL_POWER_MODE 0x15 |
||||
#define BMI160_CMD_FIFO_FLUSH 0xB0 |
||||
#define BMI160_CMD_SOFTRESET 0xB6 |
||||
|
||||
#define BMI160_OSR_NORMAL 0x20 |
||||
#define BMI160_ODR_1600HZ 0x0C |
||||
|
||||
/* Datasheet says that the device powers up in less than 10ms, so waiting for
|
||||
* 10 ms before initialization is enough. */ |
||||
#define BMI160_POWERUP_DELAY_MSEC 10 |
||||
/* TODO: Investigate this. The delay below is way too high and with that
|
||||
* there's still at least 1% of failures on initialization. Lower values |
||||
* increase that percentage. */ |
||||
#define BMI160_SOFTRESET_DELAY_MSEC 100 |
||||
/* Define a little bit more than the maximum value in the datasheet's timing
|
||||
* table. The datasheet recommends adding 300 us to the time for startup |
||||
* occasions. */ |
||||
#define BMI160_ACCEL_NORMAL_POWER_MODE_DELAY_MSEC 4 |
||||
#define BMI160_GYRO_NORMAL_POWER_MODE_DELAY_MSEC 81 |
||||
|
||||
#define BMI160_OSR BMI160_OSR_NORMAL |
||||
#define BMI160_ODR BMI160_ODR_1600HZ |
||||
#define BMI160_ACC_RANGE BMI160_ACC_RANGE_16G |
||||
#define BMI160_GYR_RANGE BMI160_GYR_RANGE_2000DPS |
||||
|
||||
/* By looking at the datasheet, the accel range i (as defined by the macros
|
||||
* BMI160_ACC_RANGE_*G) maps to the range bits by the function f defined: |
||||
* f(0) = 3; f(i) = f(i - 1) + i + 1 |
||||
* Which can be written as the closed formula: |
||||
* f(i) = (i * (i + 3)) / 2 + 3 */ |
||||
#define BMI160_ACC_RANGE_BITS \ |
||||
(BMI160_ACC_RANGE * (BMI160_ACC_RANGE + 3) / 2 + 3) |
||||
|
||||
/* The rate in Hz based on the ODR bits can be calculated with
|
||||
* 100 / (2 ^ (8 - odr) */ |
||||
#define BMI160_ODR_TO_HZ(odr_) \ |
||||
(uint16_t)(odr_ > 8 ? 100 << (odr_ - 8) : 100 >> (8 - odr_)) |
||||
|
||||
/* This number of samples should provide only one read burst operation on the
|
||||
* FIFO most of the time (99.99%). */ |
||||
#define BMI160_MAX_FIFO_SAMPLES 8 |
||||
|
||||
#define BMI160_READ_FLAG 0x80 |
||||
#define BMI160_HARDWARE_INIT_MAX_TRIES 5 |
||||
|
||||
#define BMI160_INT1_GPIO -1 |
||||
|
||||
extern const AP_HAL::HAL& hal; |
||||
|
||||
struct PACKED RawData { |
||||
struct { |
||||
le16_t x; |
||||
le16_t y; |
||||
le16_t z; |
||||
} gyro; |
||||
struct { |
||||
le16_t x; |
||||
le16_t y; |
||||
le16_t z; |
||||
} accel; |
||||
}; |
||||
|
||||
AP_InertialSensor_BMI160::AP_InertialSensor_BMI160(AP_InertialSensor &imu, |
||||
AP_HAL::OwnPtr<AP_HAL::Device> dev) |
||||
: AP_InertialSensor_Backend(imu) |
||||
, _dev(std::move(dev)) |
||||
{ |
||||
} |
||||
|
||||
AP_InertialSensor_Backend * |
||||
AP_InertialSensor_BMI160::probe(AP_InertialSensor &imu, |
||||
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev) |
||||
{ |
||||
auto sensor = new AP_InertialSensor_BMI160(imu, std::move(dev)); |
||||
|
||||
if (!sensor) { |
||||
return nullptr; |
||||
} |
||||
|
||||
if (!sensor->_init()) { |
||||
delete sensor; |
||||
return nullptr; |
||||
} |
||||
|
||||
return sensor; |
||||
} |
||||
|
||||
void AP_InertialSensor_BMI160::start() |
||||
{ |
||||
bool r; |
||||
|
||||
hal.scheduler->suspend_timer_procs(); |
||||
if (!_dev->get_semaphore()->take(100)) { |
||||
AP_HAL::panic("BMI160: Unable to get semaphore"); |
||||
} |
||||
|
||||
r = _configure_accel(); |
||||
if (!r) { |
||||
AP_HAL::panic("BMI160: Unable to configure accelerometer"); |
||||
} |
||||
|
||||
r = _configure_gyro(); |
||||
if (!r) { |
||||
AP_HAL::panic("BMI160: Unable to configure gyroscope"); |
||||
} |
||||
|
||||
r = _configure_fifo(); |
||||
if (!r) { |
||||
AP_HAL::panic("BMI160: Unable to configure FIFO"); |
||||
} |
||||
|
||||
if (BMI160_INT1_GPIO >= 0) { |
||||
r = _configure_int1_pin(); |
||||
if (!r) { |
||||
AP_HAL::panic("BMI160: unable to configure INT1 pin"); |
||||
} |
||||
} |
||||
|
||||
_dev->set_speed(AP_HAL::Device::SPEED_HIGH); |
||||
|
||||
_dev->get_semaphore()->give(); |
||||
|
||||
_accel_instance = _imu.register_accel(BMI160_ODR_TO_HZ(BMI160_ODR)); |
||||
_gyro_instance = _imu.register_gyro(BMI160_ODR_TO_HZ(BMI160_ODR)); |
||||
|
||||
hal.scheduler->resume_timer_procs(); |
||||
|
||||
hal.scheduler->register_timer_process( |
||||
FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI160::_poll_data, void)); |
||||
} |
||||
|
||||
bool AP_InertialSensor_BMI160::update() |
||||
{ |
||||
update_accel(_accel_instance); |
||||
update_gyro(_gyro_instance); |
||||
return true; |
||||
} |
||||
|
||||
void AP_InertialSensor_BMI160::_check_err_reg() |
||||
{ |
||||
#ifdef BMI160_DEBUG |
||||
uint8_t v; |
||||
bool r; |
||||
|
||||
r = _dev->read_registers(BMI160_REG_ERR_REG, &v, 1); |
||||
if (!r) { |
||||
AP_HAL::panic("BMI160: couldn't read ERR_REG\n"); |
||||
} |
||||
if (v) { |
||||
AP_HAL::panic("BMI160: error detected on ERR_REG\n"); |
||||
} |
||||
#endif |
||||
} |
||||
|
||||
bool AP_InertialSensor_BMI160::_configure_accel() |
||||
{ |
||||
bool r; |
||||
|
||||
r = _dev->write_register(BMI160_REG_ACC_CONF, BMI160_OSR | BMI160_ODR); |
||||
if (!r) { |
||||
return false; |
||||
} |
||||
hal.scheduler->delay(1); |
||||
|
||||
_check_err_reg(); |
||||
|
||||
r = _dev->write_register(BMI160_REG_ACC_RANGE, BMI160_ACC_RANGE_BITS); |
||||
if (!r) { |
||||
return false; |
||||
} |
||||
hal.scheduler->delay(1); |
||||
|
||||
/* The sensitivity in LSb/g for an accel range i (as defined by the macros
|
||||
* BMI160_ACC_RANGE_*G) can be calculated with: |
||||
* 2 ^ 16 / (2 * 2 ^ (i + 1)) = 2 ^(14 - i) |
||||
* That matches the typical values in the datasheet. */ |
||||
_accel_scale = GRAVITY_MSS / (1 << (14 - BMI160_ACC_RANGE)); |
||||
|
||||
return true; |
||||
} |
||||
|
||||
bool AP_InertialSensor_BMI160::_configure_gyro() |
||||
{ |
||||
bool r; |
||||
|
||||
r = _dev->write_register(BMI160_REG_GYR_CONF, BMI160_OSR | BMI160_ODR); |
||||
if (!r) { |
||||
return false; |
||||
} |
||||
hal.scheduler->delay(1); |
||||
|
||||
_check_err_reg(); |
||||
|
||||
r = _dev->write_register(BMI160_REG_GYR_RANGE, BMI160_GYR_RANGE); |
||||
if (!r) { |
||||
return false; |
||||
} |
||||
hal.scheduler->delay(1); |
||||
|
||||
/* The sensitivity in LSb/degrees/s a gyro range i can be calculated with:
|
||||
* 2 ^ 16 / (2 * 2000 / 2 ^ i) = 2 ^ (14 + i) / 1000 |
||||
* The scale is the inverse of that. */ |
||||
_gyro_scale = radians(1000.f / (1 << (14 + BMI160_GYR_RANGE))); |
||||
|
||||
return true; |
||||
} |
||||
|
||||
bool AP_InertialSensor_BMI160::_configure_int1_pin() |
||||
{ |
||||
bool r; |
||||
|
||||
r = _dev->write_register(BMI160_REG_INT_EN_1, BMI160_INT_FWM_EN); |
||||
if (!r) { |
||||
hal.console->printf("BMI160: Unable to enable FIFO watermark interrupt engine\n"); |
||||
return false; |
||||
} |
||||
hal.scheduler->delay(1); |
||||
|
||||
r = _dev->write_register(BMI160_REG_INT_MAP_1, BMI160_INT_MAP_INT1_FWM); |
||||
if (!r) { |
||||
hal.console->printf("BMI160: Unable to configure interrupt mapping\n"); |
||||
return false; |
||||
} |
||||
hal.scheduler->delay(1); |
||||
|
||||
r = _dev->write_register(BMI160_REG_INT_OUT_CTRL, |
||||
BMI160_INT1_OUTPUT_EN | BMI160_INT1_LVL); |
||||
if (!r) { |
||||
hal.console->printf("BMI160: Unable to configure interrupt output\n"); |
||||
return false; |
||||
} |
||||
hal.scheduler->delay(1); |
||||
|
||||
_int1_pin = hal.gpio->channel(BMI160_INT1_GPIO); |
||||
if (_int1_pin == nullptr) { |
||||
hal.console->printf("BMI160: Couldn't request data ready GPIO channel\n"); |
||||
return false; |
||||
} |
||||
_int1_pin->mode(HAL_GPIO_INPUT); |
||||
|
||||
return true; |
||||
} |
||||
|
||||
bool AP_InertialSensor_BMI160::_configure_fifo() |
||||
{ |
||||
bool r; |
||||
|
||||
/* The unit for the FIFO watermark is 4 bytes. */ |
||||
r = _dev->write_register(BMI160_REG_FIFO_CONFIG_0, |
||||
sizeof(struct RawData) / 4); |
||||
if (!r) { |
||||
hal.console->printf("BMI160: Unable to configure FIFO watermark level\n"); |
||||
return false; |
||||
} |
||||
hal.scheduler->delay(1); |
||||
|
||||
r = _dev->write_register(BMI160_REG_FIFO_CONFIG_1, |
||||
BMI160_FIFO_ACC_EN | BMI160_FIFO_GYR_EN); |
||||
if (!r) { |
||||
hal.console->printf("BMI160: Unable to enable FIFO\n"); |
||||
return false; |
||||
} |
||||
hal.scheduler->delay(1); |
||||
|
||||
_check_err_reg(); |
||||
|
||||
r = _dev->write_register(BMI160_REG_CMD, BMI160_CMD_FIFO_FLUSH); |
||||
if (!r) { |
||||
hal.console->printf("BMI160: Unable to flush FIFO\n"); |
||||
return false; |
||||
} |
||||
|
||||
return true; |
||||
} |
||||
|
||||
void AP_InertialSensor_BMI160::_read_fifo() |
||||
{ |
||||
struct RawData raw_data[BMI160_MAX_FIFO_SAMPLES]; |
||||
uint16_t num_bytes; |
||||
uint16_t excess; |
||||
uint8_t num_samples = 0; |
||||
bool r = true; |
||||
|
||||
static_assert(sizeof(raw_data) <= 100, "Too big to keep on stack"); |
||||
|
||||
/* If FIFO watermark not surpassed. */ |
||||
if (_int1_pin && _int1_pin->read() == 0) { |
||||
goto read_fifo_end; |
||||
} |
||||
|
||||
r = _dev->read_registers(BMI160_REG_FIFO_LENGTH, |
||||
(uint8_t *)&num_bytes, |
||||
sizeof(num_bytes)); |
||||
if (!r) { |
||||
goto read_fifo_end; |
||||
} |
||||
|
||||
num_bytes = le16toh(num_bytes); |
||||
if (!num_bytes) { |
||||
goto read_fifo_end; |
||||
} |
||||
|
||||
read_fifo_read_data: |
||||
if (num_bytes > sizeof(raw_data)) { |
||||
excess = num_bytes - sizeof(raw_data); |
||||
num_bytes = sizeof(raw_data); |
||||
} else { |
||||
excess = 0; |
||||
} |
||||
|
||||
r = _dev->read_registers(BMI160_REG_FIFO_DATA, |
||||
(uint8_t *)raw_data, |
||||
num_bytes); |
||||
if (!r) { |
||||
goto read_fifo_end; |
||||
} |
||||
|
||||
/* Read again just once */ |
||||
if (excess && num_samples) { |
||||
hal.console->printf("BMI160: dropping %u samples from fifo\n", |
||||
(uint8_t)(excess / sizeof(struct RawData))); |
||||
_dev->write_register(BMI160_REG_CMD, BMI160_CMD_FIFO_FLUSH); |
||||
excess = 0; |
||||
} |
||||
|
||||
num_samples = num_bytes / sizeof(struct RawData); |
||||
for (uint8_t i = 0; i < num_samples; i++) { |
||||
Vector3f accel{(float)(int16_t)le16toh(raw_data[i].accel.x), |
||||
(float)(int16_t)le16toh(raw_data[i].accel.y), |
||||
(float)(int16_t)le16toh(raw_data[i].accel.z)}; |
||||
Vector3f gyro{(float)(int16_t)le16toh(raw_data[i].gyro.x), |
||||
(float)(int16_t)le16toh(raw_data[i].gyro.y), |
||||
(float)(int16_t)le16toh(raw_data[i].gyro.z)}; |
||||
|
||||
accel *= _accel_scale; |
||||
gyro *= _gyro_scale; |
||||
|
||||
_rotate_and_correct_accel(_accel_instance, accel); |
||||
_rotate_and_correct_gyro(_gyro_instance, gyro); |
||||
|
||||
_notify_new_accel_raw_sample(_accel_instance, accel); |
||||
_notify_new_gyro_raw_sample(_gyro_instance, gyro); |
||||
} |
||||
|
||||
if (excess) { |
||||
num_bytes = excess; |
||||
goto read_fifo_read_data; |
||||
} |
||||
|
||||
read_fifo_end: |
||||
if (!r) { |
||||
hal.console->printf("BMI160: error on reading FIFO\n"); |
||||
} |
||||
} |
||||
|
||||
void AP_InertialSensor_BMI160::_poll_data() |
||||
{ |
||||
if (!_dev->get_semaphore()->take_nonblocking()) { |
||||
return; |
||||
} |
||||
|
||||
_read_fifo(); |
||||
|
||||
_dev->get_semaphore()->give(); |
||||
} |
||||
|
||||
bool AP_InertialSensor_BMI160::_hardware_init() |
||||
{ |
||||
bool ret = false; |
||||
|
||||
hal.scheduler->delay(BMI160_POWERUP_DELAY_MSEC); |
||||
|
||||
if (!_dev->get_semaphore()->take(100)) { |
||||
AP_HAL::panic("BMI160: Unable to get semaphore"); |
||||
} |
||||
|
||||
_dev->set_speed(AP_HAL::Device::SPEED_LOW); |
||||
|
||||
for (unsigned i = 0; i < BMI160_HARDWARE_INIT_MAX_TRIES; i++) { |
||||
uint8_t v; |
||||
ret = _dev->write_register(BMI160_REG_CMD, |
||||
BMI160_CMD_SOFTRESET); |
||||
|
||||
if (!ret) { |
||||
continue; |
||||
} |
||||
hal.scheduler->delay(BMI160_SOFTRESET_DELAY_MSEC); |
||||
|
||||
/* The datasheet recommends doing a read operation on the register 0x7F
|
||||
* in order to guarantee the sensor works using the SPI protocol. This |
||||
* shouldn't have side effects for I2C. */ |
||||
ret = _dev->read_registers(0x7F, &v, 1); |
||||
if (!ret) { |
||||
continue; |
||||
} |
||||
|
||||
ret = _dev->read_registers(BMI160_REG_CHIPID, &v, 1); |
||||
if (!ret) { |
||||
continue; |
||||
} |
||||
if (v != BMI160_CHIPID) { |
||||
ret = false; |
||||
continue; |
||||
} |
||||
|
||||
ret = _dev->write_register(BMI160_REG_CMD, |
||||
BMI160_CMD_ACCEL_NORMAL_POWER_MODE); |
||||
if (!ret) { |
||||
continue; |
||||
} |
||||
hal.scheduler->delay(BMI160_ACCEL_NORMAL_POWER_MODE_DELAY_MSEC); |
||||
|
||||
ret = _dev->write_register(BMI160_REG_CMD, |
||||
BMI160_CMD_GYRO_NORMAL_POWER_MODE); |
||||
if (!ret) { |
||||
continue; |
||||
} |
||||
hal.scheduler->delay(BMI160_GYRO_NORMAL_POWER_MODE_DELAY_MSEC); |
||||
|
||||
break; |
||||
} |
||||
|
||||
_dev->set_speed(AP_HAL::Device::SPEED_HIGH); |
||||
|
||||
_dev->get_semaphore()->give(); |
||||
return ret; |
||||
} |
||||
|
||||
bool AP_InertialSensor_BMI160::_init() |
||||
{ |
||||
bool ret = false; |
||||
_dev->set_read_flag(BMI160_READ_FLAG); |
||||
|
||||
hal.scheduler->suspend_timer_procs(); |
||||
ret = _hardware_init(); |
||||
hal.scheduler->resume_timer_procs(); |
||||
|
||||
if (!ret) { |
||||
hal.console->printf("BMI160: failed to init\n"); |
||||
} |
||||
|
||||
return ret; |
||||
} |
||||
|
||||
#endif |
||||
@ -0,0 +1,119 @@
@@ -0,0 +1,119 @@
|
||||
/*
|
||||
* Copyright (C) 2016 Intel Corporation. All rights reserved. |
||||
* |
||||
* This file is free software: you can redistribute it and/or modify it |
||||
* under the terms of the GNU General Public License as published by the |
||||
* Free Software Foundation, either version 3 of the License, or |
||||
* (at your option) any later version. |
||||
* |
||||
* This file is distributed in the hope that it will be useful, but |
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of |
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. |
||||
* See the GNU General Public License for more details. |
||||
* |
||||
* You should have received a copy of the GNU General Public License along |
||||
* with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/ |
||||
#pragma once |
||||
|
||||
#include <AP_HAL/AP_HAL.h> |
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX |
||||
|
||||
#include "AP_InertialSensor.h" |
||||
#include "AP_InertialSensor_Backend.h" |
||||
|
||||
class AP_InertialSensor_BMI160 : public AP_InertialSensor_Backend { |
||||
public: |
||||
static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu, |
||||
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev); |
||||
|
||||
/**
|
||||
* Configure the sensors and start reading routine. |
||||
*/ |
||||
void start() override; |
||||
|
||||
bool update() override; |
||||
|
||||
private: |
||||
AP_InertialSensor_BMI160(AP_InertialSensor &imu, |
||||
AP_HAL::OwnPtr<AP_HAL::Device> dev); |
||||
|
||||
/**
|
||||
* If the macro BMI160_DEBUG is defined, check if there are errors reported |
||||
* on the device's error register and panic if so. The implementation is |
||||
* empty if the BMI160_DEBUG isn't defined. |
||||
*/ |
||||
void _check_err_reg(); |
||||
|
||||
/**
|
||||
* Try to perform initialization of the BMI160 device. |
||||
* |
||||
* The device semaphore must be taken and released by the caller. |
||||
* |
||||
* @return true on success, false otherwise. |
||||
*/ |
||||
bool _hardware_init(); |
||||
|
||||
/**
|
||||
* Try to initialize this driver. |
||||
* |
||||
* Do sensor and other required initializations. |
||||
* |
||||
* @return true on success, false otherwise. |
||||
*/ |
||||
bool _init(); |
||||
|
||||
/**
|
||||
* Configure accelerometer sensor. The device semaphore must already be |
||||
* taken before calling this function. |
||||
* |
||||
* @return true on success, false otherwise. |
||||
*/ |
||||
bool _configure_accel(); |
||||
|
||||
/**
|
||||
* Configure gyroscope sensor. The device semaphore must already be |
||||
* taken before calling this function. |
||||
* |
||||
* @return true on success, false otherwise. |
||||
*/ |
||||
bool _configure_gyro(); |
||||
|
||||
/**
|
||||
* Configure INT1 pin as watermark interrupt pin at the level of one sample |
||||
* if using fifo or data-ready pin otherwise. |
||||
* |
||||
* @return true on success, false otherwise. |
||||
*/ |
||||
bool _configure_int1_pin(); |
||||
|
||||
/**
|
||||
* Configure FIFO. |
||||
* |
||||
* @return true on success, false otherwise. |
||||
*/ |
||||
bool _configure_fifo(); |
||||
|
||||
/**
|
||||
* Timer routine to read data from the sensors. |
||||
*/ |
||||
void _poll_data(); |
||||
|
||||
/**
|
||||
* Read samples from fifo. |
||||
*/ |
||||
void _read_fifo(); |
||||
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> _dev; |
||||
|
||||
uint8_t _accel_instance; |
||||
float _accel_scale; |
||||
|
||||
uint8_t _gyro_instance; |
||||
float _gyro_scale; |
||||
|
||||
AP_HAL::DigitalSource *_int1_pin; |
||||
}; |
||||
|
||||
#endif |
||||
Loading…
Reference in new issue