zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_InertialSensor: added BMI055 IMU driver

master
Andrew Tridgell 7 years ago
parent
cf1f2f9aeb
commit
89b8124560
4 changed files with 415 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 5
      libraries/AP_InertialSensor/AP_InertialSensor.cpp
  2. 340
      libraries/AP_InertialSensor/AP_InertialSensor_BMI055.cpp
  3. 69
      libraries/AP_InertialSensor/AP_InertialSensor_BMI055.h
  4. 1
      libraries/AP_InertialSensor/AP_InertialSensor_Backend.h

5
libraries/AP_InertialSensor/AP_InertialSensor.cpp
Unescape View File

@ -22,6 +22,7 @@ @@ -22,6 +22,7 @@
#include "AP_InertialSensor_SITL.h"
#include "AP_InertialSensor_RST.h"
#include "AP_InertialSensor_Revo.h"
#include "AP_InertialSensor_BMI055.h"
/* Define INS_TIMING_DEBUG to track down scheduling issues with the main loop.
* Output is on the debug console. */
@ -743,6 +744,10 @@ AP_InertialSensor::detect_backends(void) @@ -743,6 +744,10 @@ AP_InertialSensor::detect_backends(void)
_fast_sampling_mask.set_default(1);
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device("icm20689"), ROTATION_NONE));
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device("icm20602"), ROTATION_NONE));
ADD_BACKEND(AP_InertialSensor_BMI055::probe(*this,
hal.spi->get_device("bmi055_a"),
hal.spi->get_device("bmi055_g"),
ROTATION_ROLL_180_YAW_90));
break;
case AP_BoardConfig::PX4_BOARD_SP01:

340
libraries/AP_InertialSensor/AP_InertialSensor_BMI055.cpp
Unescape View File

@ -0,0 +1,340 @@ @@ -0,0 +1,340 @@
/*
* 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>
#include <AP_Math/AP_Math.h>
#include "AP_InertialSensor_BMI055.h"
/*
device registers, names follow datasheet conventions, with REGA_
prefix for accel, and REGG_ prefix for gyro
*/
#define REGA_BGW_CHIPID 0x00
#define REGA_ACCD_X_LSB 0x02
#define REGA_ACCD_TEMP 0x08
#define REGA_INT_STATUS_0 0x09
#define REGA_INT_STATUS_1 0x0A
#define REGA_INT_STATUS_2 0x0B
#define REGA_INT_STATUS_3 0x0C
#define REGA_FIFO_STATUS 0x0E
#define REGA_PMU_RANGE 0x0F
#define REGA_PMU_BW 0x10
#define REGA_PMU_LPW 0x11
#define REGA_ACCD_HBW 0x13
#define REGA_BGW_SOFTRESET 0x14
#define REGA_OUT_CTRL 0x20
#define REGA_EST_LATCH 0x21
#define REGA_FIFO_CONFIG_0 0x30
#define REGA_PMU_SELF_TEST 0x32
#define REGA_FIFO_CONFIG_1 0x3E
#define REGA_FIFO_DATA 0x3F
#define REGG_CHIPID 0x00
#define REGA_RATE_X_LSB 0x02
#define REGG_INT_STATUS_0 0x09
#define REGG_INT_STATUS_1 0x0A
#define REGG_INT_STATUS_2 0x0B
#define REGG_INT_STATUS_3 0x0C
#define REGG_FIFO_STATUS 0x0E
#define REGG_RANGE 0x0F
#define REGG_BW 0x10
#define REGG_LPM1 0x11
#define REGG_RATE_HBW 0x13
#define REGG_BGW_SOFTRESET 0x14
#define REGG_FIFO_CONFIG_1 0x3E
#define REGG_FIFO_DATA 0x3F
extern const AP_HAL::HAL& hal;
#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]))
AP_InertialSensor_BMI055::AP_InertialSensor_BMI055(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::Device> _dev_accel,
AP_HAL::OwnPtr<AP_HAL::Device> _dev_gyro,
enum Rotation _rotation)
: AP_InertialSensor_Backend(imu)
, dev_accel(std::move(_dev_accel))
, dev_gyro(std::move(_dev_gyro))
, rotation(_rotation)
{
}
AP_InertialSensor_Backend *
AP_InertialSensor_BMI055::probe(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev_accel,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev_gyro,
enum Rotation rotation)
{
if (!dev_accel || !dev_gyro) {
return nullptr;
}
auto sensor = new AP_InertialSensor_BMI055(imu, std::move(dev_accel), std::move(dev_gyro), rotation);
if (!sensor) {
return nullptr;
}
if (!sensor->init()) {
delete sensor;
return nullptr;
}
return sensor;
}
void AP_InertialSensor_BMI055::start()
{
accel_instance = _imu.register_accel(2000, dev_accel->get_bus_id_devtype(DEVTYPE_INS_BMI055));
gyro_instance = _imu.register_gyro(2000, dev_gyro->get_bus_id_devtype(DEVTYPE_INS_BMI055));
// setup sensor rotations from probe()
set_gyro_orientation(gyro_instance, rotation);
set_accel_orientation(accel_instance, rotation);
// setup callbacks
dev_accel->register_periodic_callback(1000,
FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI055::read_fifo_accel, void));
dev_gyro->register_periodic_callback(1000,
FUNCTOR_BIND_MEMBER(&AP_InertialSensor_BMI055::read_fifo_gyro, void));
}
/*
probe and initialise accelerometer
*/
bool AP_InertialSensor_BMI055::accel_init()
{
dev_accel->get_semaphore()->take_blocking();
uint8_t v;
if (!dev_accel->read_registers(REGA_BGW_CHIPID, &v, 1) || v != 0xFA) {
goto failed;
}
if (!dev_accel->write_register(REGA_BGW_SOFTRESET, 0xB6)) {
goto failed;
}
hal.scheduler->delay(10);
dev_accel->setup_checked_registers(5, 20);
// setup 16g range
if (!dev_accel->write_register(REGA_PMU_RANGE, 0x0C, true)) {
goto failed;
}
// setup filter bandwidth 1kHz
if (!dev_accel->write_register(REGA_PMU_BW, 0x0F, true)) {
goto failed;
}
// disable low-power mode
if (!dev_accel->write_register(REGA_PMU_LPW, 0, true)) {
goto failed;
}
// setup for unfiltered data
if (!dev_accel->write_register(REGA_ACCD_HBW, 0x80, true)) {
goto failed;
}
// setup FIFO for streaming X,Y,Z
if (!dev_accel->write_register(REGA_FIFO_CONFIG_1, 0x80, true)) {
goto failed;
}
hal.console->printf("BMI055: found accel\n");
dev_accel->get_semaphore()->give();
return true;
failed:
dev_accel->get_semaphore()->give();
return false;
}
/*
probe and initialise gyro
*/
bool AP_InertialSensor_BMI055::gyro_init()
{
dev_gyro->get_semaphore()->take_blocking();
uint8_t v;
if (!dev_gyro->read_registers(REGG_CHIPID, &v, 1) || v != 0x0F) {
goto failed;
}
if (!dev_gyro->write_register(REGG_BGW_SOFTRESET, 0xB6)) {
goto failed;
}
hal.scheduler->delay(10);
dev_gyro->setup_checked_registers(5, 20);
// setup 2000dps range
if (!dev_gyro->write_register(REGG_RANGE, 0x00, true)) {
goto failed;
}
// setup filter bandwidth 230Hz, no decimation
if (!dev_gyro->write_register(REGG_BW, 0x81, true)) {
goto failed;
}
// disable low-power mode
if (!dev_gyro->write_register(REGG_LPM1, 0, true)) {
goto failed;
}
// setup for filtered data
if (!dev_gyro->write_register(REGG_RATE_HBW, 0x00, true)) {
goto failed;
}
// setup FIFO for streaming X,Y,Z
if (!dev_gyro->write_register(REGG_FIFO_CONFIG_1, 0x80, true)) {
goto failed;
}
hal.console->printf("BMI055: found gyro\n");
dev_gyro->get_semaphore()->give();
return true;
failed:
dev_gyro->get_semaphore()->give();
return false;
}
bool AP_InertialSensor_BMI055::init()
{
dev_accel->set_read_flag(0x80);
dev_gyro->set_read_flag(0x80);
return accel_init() && gyro_init();
}
/*
read accel fifo
*/
void AP_InertialSensor_BMI055::read_fifo_accel(void)
{
uint8_t num_frames;
if (!dev_accel->read_registers(REGA_FIFO_STATUS, &num_frames, 1)) {
_inc_accel_error_count(accel_instance);
return;
}
num_frames &= 0x7F;
// don't read more than 8 frames at a time
if (num_frames > 8) {
num_frames = 8;
}
if (num_frames == 0) {
return;
}
uint8_t data[6*num_frames];
if (!dev_accel->read_registers(REGA_FIFO_DATA, data, num_frames*6)) {
_inc_accel_error_count(accel_instance);
return;
}
// data is 12 bits with 16g range, 7.81mg/LSB
const float scale = 7.81 * 0.001 * GRAVITY_MSS / 16.0;
for (uint8_t i = 0; i < num_frames; i++) {
const uint8_t *d = &data[i*6];
int16_t xyz[3] {
int16_t(uint16_t((d[0]&0xF0) | (d[1]<<8))),
int16_t(uint16_t((d[2]&0xF0) | (d[3]<<8))),
int16_t(uint16_t((d[4]&0xF0) | (d[5]<<8))) };
Vector3f accel(xyz[0], xyz[1], xyz[2]);
accel *= scale;
_rotate_and_correct_accel(accel_instance, accel);
_notify_new_accel_raw_sample(accel_instance, accel);
}
if (temperature_counter++ == 100) {
temperature_counter = 0;
int8_t t;
if (!dev_accel->read_registers(REGA_ACCD_TEMP, (uint8_t *)&t, 1)) {
_inc_accel_error_count(accel_instance);
} else {
float temp_degc = (0.5 * t) + 23.0;
_publish_temperature(accel_instance, temp_degc);
}
}
if (!dev_accel->check_next_register()) {
_inc_accel_error_count(accel_instance);
}
}
/*
read gyro fifo
*/
void AP_InertialSensor_BMI055::read_fifo_gyro(void)
{
uint8_t num_frames;
if (!dev_gyro->read_registers(REGG_FIFO_STATUS, &num_frames, 1)) {
_inc_gyro_error_count(gyro_instance);
return;
}
num_frames &= 0x7F;
// don't read more than 8 frames at a time
if (num_frames > 8) {
num_frames = 8;
}
if (num_frames == 0) {
return;
}
uint8_t data[6*num_frames];
if (!dev_gyro->read_registers(REGG_FIFO_DATA, data, num_frames*6)) {
_inc_gyro_error_count(gyro_instance);
return;
}
// data is 16 bits with 2000dps range
const float scale = radians(2000.0) / 32767.0;
for (uint8_t i = 0; i < num_frames; i++) {
const uint8_t *d = &data[i*6];
int16_t xyz[3] {
int16_t(uint16_t(d[0] | d[1]<<8)),
int16_t(uint16_t(d[2] | d[3]<<8)),
int16_t(uint16_t(d[4] | d[5]<<8)) };
Vector3f gyro(xyz[0], xyz[1], xyz[2]);
gyro *= scale;
_rotate_and_correct_gyro(gyro_instance, gyro);
_notify_new_gyro_raw_sample(gyro_instance, gyro);
}
if (!dev_gyro->check_next_register()) {
_inc_gyro_error_count(gyro_instance);
}
}
bool AP_InertialSensor_BMI055::update()
{
update_accel(accel_instance);
update_gyro(gyro_instance);
return true;
}

69
libraries/AP_InertialSensor/AP_InertialSensor_BMI055.h
Unescape View File

@ -0,0 +1,69 @@ @@ -0,0 +1,69 @@
/*
* 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/>.
*/
/*
the BMI055 is unusual as it has separate chip-select for accel and
gyro, which means it needs two SPIDevice pointers
*/
#pragma once
#include <AP_HAL/AP_HAL.h>
#include "AP_InertialSensor.h"
#include "AP_InertialSensor_Backend.h"
class AP_InertialSensor_BMI055 : public AP_InertialSensor_Backend {
public:
static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev_accel,
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev_gyro,
enum Rotation rotation = ROTATION_NONE);
/**
* Configure the sensors and start reading routine.
*/
void start() override;
bool update() override;
private:
AP_InertialSensor_BMI055(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::Device> dev_accel,
AP_HAL::OwnPtr<AP_HAL::Device> dev_gyro,
enum Rotation rotation = ROTATION_NONE);
/*
initialise hardware layer
*/
bool accel_init();
bool gyro_init();
/*
initialise driver
*/
bool init();
/*
read data from the FIFOs
*/
void read_fifo_accel();
void read_fifo_gyro();
AP_HAL::OwnPtr<AP_HAL::Device> dev_accel;
AP_HAL::OwnPtr<AP_HAL::Device> dev_gyro;
uint8_t accel_instance;
uint8_t gyro_instance;
enum Rotation rotation;
uint8_t temperature_counter;
};

1
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
Unescape View File

@ -104,6 +104,7 @@ public: @@ -104,6 +104,7 @@ public:
DEVTYPE_GYR_LSM9DS1 = 0x26,
DEVTYPE_INS_ICM20789 = 0x27,
DEVTYPE_INS_ICM20689 = 0x28,
DEVTYPE_INS_BMI055 = 0x29,
};
protected:

Write Preview
Loading…
Cancel
Save