fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

bmi055 split accel and gyro headers

Browse Source
This commit is contained in:
Daniel Agar 2018-07-07 10:58:52 -04:00 committed by Lorenz Meier
parent 4c5ace9b75
commit eef325e2af
8 changed files with 833 additions and 710 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

109
src/drivers/imu/bmi055/BMI055.hpp Normal file
View File

@ -0,0 +1,109 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <drivers/device/integrator.h>
#include <drivers/device/spi.h>
#include <drivers/drv_hrt.h>
#include <lib/conversion/rotation.h>
#include <lib/perf/perf_counter.h>
#include <px4_config.h>
#include <systemlib/conversions.h>
#define DIR_READ 0x80
#define DIR_WRITE 0x00
//Soft-reset command Value
#define BMI055_SOFT_RESET 0xB6
#define BMI055_BUS_SPEED 10*1000*1000
#define BMI055_TIMER_REDUCTION 200
class BMI055 : public device::SPI
{
protected:
uint8_t _whoami; /** whoami result */
struct hrt_call _call;
unsigned _call_interval;
unsigned _dlpf_freq;
perf_counter_t _sample_perf;
perf_counter_t _bad_transfers;
perf_counter_t _bad_registers;
perf_counter_t _good_transfers;
perf_counter_t _reset_retries;
perf_counter_t _duplicates;
uint8_t _register_wait;
uint64_t _reset_wait;
enum Rotation _rotation;
uint8_t _checked_next;
/**
* Read a register from the BMI055
*
* @param The register to read.
* @return The value that was read.
*/
uint8_t read_reg(unsigned reg);
uint16_t read_reg16(unsigned reg);
/**
* Write a register in the BMI055
*
* @param reg The register to write.
* @param value The new value to write.
*/
void write_reg(unsigned reg, uint8_t value);
/* do not allow to copy this class due to pointer data members */
BMI055(const BMI055 &);
BMI055 operator=(const BMI055 &);
public:
BMI055(const char *name, const char *devname, int bus, uint32_t device, enum spi_mode_e mode, uint32_t frequency,
enum Rotation rotation);
virtual ~BMI055();
};

79
src/drivers/imu/bmi055/bmi055_accel.cpp → src/drivers/imu/bmi055/BMI055_accel.cpp
View File

@ -1,4 +1,38 @@
#include "bmi055.hpp"
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "BMI055_accel.hpp"
#include <ecl/geo/geo.h>
/*
@ -12,8 +46,6 @@ const uint8_t BMI055_accel::_checked_registers[BMI055_ACCEL_NUM_CHECKED_REGISTER
BMI055_ACC_INT_MAP_1,
};
BMI055_accel::BMI055_accel(int bus, const char *path_accel, uint32_t device, enum Rotation rotation) :
BMI055("BMI055_ACCEL", path_accel, bus, device, SPIDEV_MODE3, BMI055_BUS_SPEED, rotation),
_accel_reports(nullptr),
@ -32,9 +64,6 @@ BMI055_accel::BMI055_accel(int bus, const char *path_accel, uint32_t device, enu
_last_temperature(0),
_got_duplicate(false)
{
// disable debug() calls
_debug_enabled = false;
_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_BMI055;
// default accel scale factors
@ -48,7 +77,6 @@ BMI055_accel::BMI055_accel(int bus, const char *path_accel, uint32_t device, enu
memset(&_call, 0, sizeof(_call));
}
BMI055_accel::~BMI055_accel()
{
/* make sure we are truly inactive */
@ -59,7 +87,6 @@ BMI055_accel::~BMI055_accel()
delete _accel_reports;
}
if (_accel_class_instance != -1) {
unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance);
}
@ -68,14 +95,11 @@ BMI055_accel::~BMI055_accel()
perf_free(_accel_reads);
}
int
BMI055_accel::init()
{
int ret;
/* do SPI init (and probe) first */
ret = SPI::init();
int ret = SPI::init();
/* if probe/setup failed, bail now */
if (ret != OK) {
@ -102,7 +126,6 @@ BMI055_accel::init()
_accel_scale.z_offset = 0;
_accel_scale.z_scale = 1.0f;
_accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
measure();
@ -162,7 +185,6 @@ int BMI055_accel::reset()
return OK;
}
int
BMI055_accel::probe()
{
@ -183,15 +205,12 @@ BMI055_accel::probe()
return -EIO;
}
int
BMI055_accel::accel_set_sample_rate(float frequency)
{
uint8_t setbits = 0;
uint8_t clearbits = BMI055_ACCEL_BW_1000;
if (frequency < (3125 / 100)) {
setbits |= BMI055_ACCEL_BW_7_81;
_accel_sample_rate = 1563 / 100;
@ -234,8 +253,6 @@ BMI055_accel::accel_set_sample_rate(float frequency)
return OK;
}
ssize_t
BMI055_accel::read(struct file *filp, char *buffer, size_t buflen)
{
@ -322,7 +339,6 @@ BMI055_accel::accel_self_test()
return 0;
}
/*
deliberately trigger an error in the sensor to trigger recovery
*/
@ -334,7 +350,6 @@ BMI055_accel::test_error()
print_registers();
}
int
BMI055_accel::ioctl(struct file *filp, int cmd, unsigned long arg)
{
@ -364,8 +379,8 @@ BMI055_accel::ioctl(struct file *filp, int cmd, unsigned long arg)
return ioctl(filp, SENSORIOCSPOLLRATE, BMI055_ACCEL_MAX_RATE);
case SENSOR_POLLRATE_DEFAULT:
return ioctl(filp, SENSORIOCSPOLLRATE,
BMI055_ACCEL_DEFAULT_RATE); //Polling at the highest frequency. We may get duplicate values on the sensors
// Polling at the highest frequency. We may get duplicate values on the sensors
return ioctl(filp, SENSORIOCSPOLLRATE, BMI055_ACCEL_DEFAULT_RATE);
/* adjust to a legal polling interval in Hz */
default: {
@ -474,11 +489,10 @@ BMI055_accel::ioctl(struct file *filp, int cmd, unsigned long arg)
}
}
void
BMI055_accel::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
{
uint8_t val;
uint8_t val;
val = read_reg(reg);
val &= ~clearbits;
@ -543,7 +557,6 @@ BMI055_accel::set_accel_range(unsigned max_g)
return OK;
}
void
BMI055_accel::start()
{
@ -619,7 +632,6 @@ BMI055_accel::check_registers(void)
_checked_next = (_checked_next + 1) % BMI055_ACCEL_NUM_CHECKED_REGISTERS;
}
void
BMI055_accel::measure()
{
@ -701,7 +713,6 @@ BMI055_accel::measure()
return;
}
perf_count(_good_transfers);
if (_register_wait != 0) {
@ -715,12 +726,10 @@ BMI055_accel::measure()
/*
* Report buffers.
*/
accel_report arb;
accel_report arb;
arb.timestamp = hrt_absolute_time();
// report the error count as the sum of the number of bad
// transfers and bad register reads. This allows the higher
// level code to decide if it should use this sensor based on
@ -791,11 +800,11 @@ BMI055_accel::measure()
perf_end(_sample_perf);
}
void
BMI055_accel::print_info()
{
warnx("BMI055 Accel");
PX4_INFO("Accel");
perf_print_counter(_sample_perf);
perf_print_counter(_accel_reads);
perf_print_counter(_bad_transfers);
@ -803,6 +812,7 @@ BMI055_accel::print_info()
perf_print_counter(_good_transfers);
perf_print_counter(_reset_retries);
perf_print_counter(_duplicates);
_accel_reports->print_info("accel queue");
::printf("checked_next: %u\n", _checked_next);
@ -828,7 +838,6 @@ BMI055_accel::print_info()
printf("\n");
}
void
BMI055_accel::print_registers()
{
@ -861,5 +870,3 @@ BMI055_accel::print_registers()
printf("\n");
}

298
src/drivers/imu/bmi055/BMI055_accel.hpp Normal file
View File

@ -0,0 +1,298 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include "BMI055.hpp"
#include <drivers/device/integrator.h>
#include <drivers/device/ringbuffer.h>
#include <drivers/device/spi.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_hrt.h>
#include <lib/conversion/rotation.h>
#include <lib/perf/perf_counter.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <px4_config.h>
#include <systemlib/conversions.h>
#define BMI055_DEVICE_PATH_ACCEL "/dev/bmi055_accel"
#define BMI055_DEVICE_PATH_ACCEL_EXT "/dev/bmi055_accel_ext"
// BMI055 Accel registers
#define BMI055_ACC_CHIP_ID 0x00
#define BMI055_ACC_X_L 0x02
#define BMI055_ACC_X_H 0x03
#define BMI055_ACC_Y_L 0x04
#define BMI055_ACC_Y_H 0x05
#define BMI055_ACC_Z_L 0x06
#define BMI055_ACC_Z_H 0x07
#define BMI055_ACC_TEMP 0x08
#define BMI055_ACC_INT_STATUS_0 0x09
#define BMI055_ACC_INT_STATUS_1 0x0A
#define BMI055_ACC_INT_STATUS_2 0x0B
#define BMI055_ACC_INT_STATUS_3 0x0C
#define BMI055_ACC_FIFO_STATUS 0x0E
#define BMI055_ACC_RANGE 0x0F
#define BMI055_ACC_BW 0x10
#define BMI055_ACC_PMU_LPW 0x11
#define BMI055_ACC_PMU_LOW_POWER 0x12
#define BMI055_ACC_DATA_CTRL 0x13
#define BMI055_ACC_SOFTRESET 0x14
#define BMI055_ACC_INT_EN_0 0x16
#define BMI055_ACC_INT_EN_1 0x17
#define BMI055_ACC_INT_EN_2 0x18
#define BMI055_ACC_INT_MAP_0 0x19
#define BMI055_ACC_INT_MAP_1 0x1A
#define BMI055_ACC_INT_MAP_2 0x1B
#define BMI055_ACC_INT_SRC 0x1E
#define BMI055_ACC_INT_OUT_CTRL 0x20
#define BMI055_ACC_INT_LATCH 0x21
#define BMI055_ACC_INT_LH_0 0x22
#define BMI055_ACC_INT_LH_1 0x23
#define BMI055_ACC_INT_LH_2 0x24
#define BMI055_ACC_INT_LH_3 0x25
#define BMI055_ACC_INT_LH_4 0x26
#define BMI055_ACC_INT_MOT_0 0x27
#define BMI055_ACC_INT_MOT_1 0x28
#define BMI055_ACC_INT_MOT_2 0x29
#define BMI055_ACC_INT_TAP_0 0x2A
#define BMI055_ACC_INT_TAP_1 0x2B
#define BMI055_ACC_INT_ORIE_0 0x2C
#define BMI055_ACC_INT_ORIE_1 0x2D
#define BMI055_ACC_INT_FLAT_0 0x2E
#define BMI055_ACC_INT_FLAT_1 0x2F
#define BMI055_ACC_FIFO_CONFIG_0 0x30
#define BMI055_ACC_SELF_TEST 0x32
#define BMI055_ACC_EEPROM_CTRL 0x33
#define BMI055_ACC_SERIAL_CTRL 0x34
#define BMI055_ACC_OFFSET_CTRL 0x36
#define BMI055_ACC_OFC_SETTING 0x37
#define BMI055_ACC_OFFSET_X 0x38
#define BMI055_ACC_OFFSET_Y 0x39
#define BMI055_ACC_OFFSET_Z 0x3A
#define BMI055_ACC_TRIM_GPO 0x3B
#define BMI055_ACC_TRIM_GP1 0x3C
#define BMI055_ACC_FIFO_CONFIG_1 0x3E
#define BMI055_ACC_FIFO_DATA 0x3F
// BMI055 Accelerometer Chip-Id
#define BMI055_ACC_WHO_AM_I 0xFA
//BMI055_ACC_BW 0x10
#define BMI055_ACCEL_BW_7_81 (1<<3) | (0<<2) | (0<<1) | (0<<0)
#define BMI055_ACCEL_BW_15_63 (1<<3) | (0<<2) | (0<<1) | (1<<0)
#define BMI055_ACCEL_BW_31_25 (1<<3) | (0<<2) | (1<<1) | (0<<0)
#define BMI055_ACCEL_BW_62_5 (1<<3) | (0<<2) | (1<<1) | (1<<0)
#define BMI055_ACCEL_BW_125 (1<<3) | (1<<2) | (0<<1) | (0<<0)
#define BMI055_ACCEL_BW_250 (1<<3) | (1<<2) | (0<<1) | (1<<0)
#define BMI055_ACCEL_BW_500 (1<<3) | (1<<2) | (1<<1) | (0<<0)
#define BMI055_ACCEL_BW_1000 (1<<3) | (1<<2) | (1<<1) | (1<<0)
//BMI055_ACC_PMU_LPW 0x11
#define BMI055_ACCEL_NORMAL (0<<7) | (0<<6) | (0<<5)
#define BMI055_ACCEL_DEEP_SUSPEND (0<<7) | (0<<6) | (1<<5)
#define BMI055_ACCEL_LOW_POWER (0<<7) | (1<<6) | (0<<5)
#define BMI055_ACCEL_SUSPEND (1<<7) | (0<<6) | (0<<5)
//BMI055_ACC_RANGE 0x0F
#define BMI055_ACCEL_RANGE_2_G (0<<3) | (0<<2) | (1<<1) | (1<<0)
#define BMI055_ACCEL_RANGE_4_G (0<<3) | (1<<2) | (0<<1) | (1<<0)
#define BMI055_ACCEL_RANGE_8_G (1<<3) | (0<<2) | (0<<1) | (0<<0)
#define BMI055_ACCEL_RANGE_16_G (1<<3) | (1<<2) | (0<<1) | (0<<0)
//BMI055_ACC_INT_EN_1 0x17
#define BMI055_ACC_DRDY_INT_EN (1<<4)
//BMI055_ACC_INT_MAP_1 0x1A
#define BMI055_ACC_DRDY_INT1 (1<<0)
// Default and Max values
#define BMI055_ACCEL_DEFAULT_RANGE_G 16
#define BMI055_ACCEL_DEFAULT_RATE 1000
#define BMI055_ACCEL_MAX_RATE 1000
#define BMI055_ACCEL_MAX_PUBLISH_RATE 280
#define BMI055_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 50
/* Mask definitions for ACCD_X_LSB, ACCD_Y_LSB and ACCD_Z_LSB Register */
#define BMI055_NEW_DATA_MASK 0x01
class BMI055_accel : public BMI055
{
public:
BMI055_accel(int bus, const char *path_accel, uint32_t device, enum Rotation rotation);
virtual ~BMI055_accel();
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
void print_registers();
// deliberately cause a sensor error
void test_error();
protected:
virtual int probe();
private:
ringbuffer::RingBuffer *_accel_reports;
struct accel_calibration_s _accel_scale;
float _accel_range_scale;
float _accel_range_m_s2;
orb_advert_t _accel_topic;
int _accel_orb_class_instance;
int _accel_class_instance;
float _accel_sample_rate;
perf_counter_t _accel_reads;
math::LowPassFilter2p _accel_filter_x;
math::LowPassFilter2p _accel_filter_y;
math::LowPassFilter2p _accel_filter_z;
Integrator _accel_int;
// this is used to support runtime checking of key
// configuration registers to detect SPI bus errors and sensor
// reset
#define BMI055_ACCEL_NUM_CHECKED_REGISTERS 5
static const uint8_t _checked_registers[BMI055_ACCEL_NUM_CHECKED_REGISTERS];
uint8_t _checked_values[BMI055_ACCEL_NUM_CHECKED_REGISTERS];
uint8_t _checked_bad[BMI055_ACCEL_NUM_CHECKED_REGISTERS];
// last temperature reading for print_info()
float _last_temperature;
bool _got_duplicate;
/**
* Start automatic measurement.
*/
void start();
/**
* Stop automatic measurement.
*/
void stop();
/**
* Reset chip.
*
* Resets the chip and measurements ranges, but not scale and offset.
*/
int reset();
/**
* Static trampoline from the hrt_call context; because we don't have a
* generic hrt wrapper yet.
*
* Called by the HRT in interrupt context at the specified rate if
* automatic polling is enabled.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void measure_trampoline(void *arg);
/**
* Fetch measurements from the sensor and update the report buffers.
*/
void measure();
/**
* Modify a register in the BMI055_accel
*
* Bits are cleared before bits are set.
*
* @param reg The register to modify.
* @param clearbits Bits in the register to clear.
* @param setbits Bits in the register to set.
*/
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
/**
* Write a register in the BMI055_accel, updating _checked_values
*
* @param reg The register to write.
* @param value The new value to write.
*/
void write_checked_reg(unsigned reg, uint8_t value);
/**
* Set the BMI055_accel measurement range.
*
* @param max_g The maximum G value the range must support.
* @return OK if the value can be supported, -EINVAL otherwise.
*/
int set_accel_range(unsigned max_g);
/**
* Measurement self test
*
* @return 0 on success, 1 on failure
*/
int self_test();
/**
* Accel self test
*
* @return 0 on success, 1 on failure
*/
int accel_self_test();
/*
* set accel sample rate
*/
int accel_set_sample_rate(float desired_sample_rate_hz);
/*
* check that key registers still have the right value
*/
void check_registers(void);
/* do not allow to copy this class due to pointer data members */
BMI055_accel(const BMI055_accel &);
BMI055_accel operator=(const BMI055_accel &);
};

76
src/drivers/imu/bmi055/bmi055_gyro.cpp → src/drivers/imu/bmi055/BMI055_gyro.cpp
View File

@ -1,6 +1,37 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "bmi055.hpp"
#include "BMI055_gyro.hpp"
/*
list of registers that will be checked in check_registers(). Note
@ -16,7 +47,6 @@ const uint8_t BMI055_gyro::_checked_registers[BMI055_GYRO_NUM_CHECKED_REGISTERS]
BMI055_GYR_INT_MAP_1
};
BMI055_gyro::BMI055_gyro(int bus, const char *path_gyro, uint32_t device, enum Rotation rotation) :
BMI055("BMI055_GYRO", path_gyro, bus, device, SPIDEV_MODE3, BMI055_BUS_SPEED, rotation),
_gyro_reports(nullptr),
@ -34,9 +64,6 @@ BMI055_gyro::BMI055_gyro(int bus, const char *path_gyro, uint32_t device, enum R
_gyro_int(1000000 / BMI055_GYRO_MAX_PUBLISH_RATE, true),
_last_temperature(0)
{
// disable debug() calls
_debug_enabled = false;
_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_BMI055;
// default gyro scale factors
@ -50,7 +77,6 @@ BMI055_gyro::BMI055_gyro(int bus, const char *path_gyro, uint32_t device, enum R
memset(&_call, 0, sizeof(_call));
}
BMI055_gyro::~BMI055_gyro()
{
/* make sure we are truly inactive */
@ -72,10 +98,8 @@ BMI055_gyro::~BMI055_gyro()
int
BMI055_gyro::init()
{
int ret;
/* do SPI init (and probe) first */
ret = SPI::init();
int ret = SPI::init();
/* if probe/setup failed, bail now */
if (ret != OK) {
@ -84,7 +108,7 @@ BMI055_gyro::init()
}
/* allocate basic report buffers */
_gyro_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report));
_gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report));
if (_gyro_reports == nullptr) {
goto out;
@ -128,7 +152,6 @@ out:
return ret;
}
int BMI055_gyro::reset()
{
write_reg(BMI055_GYR_SOFTRESET, BMI055_SOFT_RESET);//Soft-reset
@ -141,7 +164,6 @@ int BMI055_gyro::reset()
set_gyro_range(BMI055_GYRO_DEFAULT_RANGE_DPS);// set Gyro range
gyro_set_sample_rate(BMI055_GYRO_DEFAULT_RATE);// set Gyro ODR
//Enable Gyroscope in normal mode
write_reg(BMI055_GYR_LPM1, BMI055_GYRO_NORMAL);
up_udelay(1000);
@ -188,7 +210,6 @@ BMI055_gyro::probe()
return -EIO;
}
int
BMI055_gyro::gyro_set_sample_rate(float frequency)
{
@ -220,7 +241,6 @@ BMI055_gyro::gyro_set_sample_rate(float frequency)
return OK;
}
int
BMI055_gyro::self_test()
{
@ -338,12 +358,14 @@ BMI055_gyro::read(struct file *filp, char *buffer, size_t buflen)
return (transferred * sizeof(gyro_report));
}
int
BMI055_gyro::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCRESET:
return reset();
case SENSORIOCSPOLLRATE: {
switch (arg) {
@ -380,6 +402,7 @@ BMI055_gyro::ioctl(struct file *filp, int cmd, unsigned long arg)
return -EINVAL;
}
// adjust filters
float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq();
float sample_rate = 1.0e6f / ticks;
_gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
@ -414,9 +437,6 @@ BMI055_gyro::ioctl(struct file *filp, int cmd, unsigned long arg)
return 1000000 / _call_interval;
case SENSORIOCRESET:
return reset();
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100)) {
@ -466,8 +486,6 @@ BMI055_gyro::ioctl(struct file *filp, int cmd, unsigned long arg)
}
}
void
BMI055_gyro::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
{
@ -492,7 +510,6 @@ BMI055_gyro::write_checked_reg(unsigned reg, uint8_t value)
}
}
int
BMI055_gyro::set_gyro_range(unsigned max_dps)
{
@ -617,7 +634,6 @@ BMI055_gyro::check_registers(void)
_checked_next = (_checked_next + 1) % BMI055_GYRO_NUM_CHECKED_REGISTERS;
}
void
BMI055_gyro::measure()
{
@ -685,10 +701,9 @@ BMI055_gyro::measure()
/*
* Report buffers.
*/
gyro_report grb;
gyro_report grb;
grb.timestamp = hrt_absolute_time();
grb.timestamp = hrt_absolute_time();
// report the error count as the sum of the number of bad
// transfers and bad register reads. This allows the higher
@ -764,7 +779,8 @@ BMI055_gyro::measure()
void
BMI055_gyro::print_info()
{
warnx("BMI055 Gyro");
PX4_INFO("Gyro");
perf_print_counter(_sample_perf);
perf_print_counter(_gyro_reads);
perf_print_counter(_bad_transfers);
@ -772,6 +788,7 @@ BMI055_gyro::print_info()
perf_print_counter(_good_transfers);
perf_print_counter(_reset_retries);
perf_print_counter(_duplicates);
_gyro_reports->print_info("gyro queue");
::printf("checked_next: %u\n", _checked_next);
@ -797,7 +814,6 @@ BMI055_gyro::print_info()
printf("\n");
}
void
BMI055_gyro::print_registers()
{
@ -840,7 +856,3 @@ BMI055_gyro::print_registers()
printf("\n");
}

301
src/drivers/imu/bmi055/BMI055_gyro.hpp Normal file
View File

@ -0,0 +1,301 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include "BMI055.hpp"
#include <drivers/device/integrator.h>
#include <drivers/device/ringbuffer.h>
#include <drivers/device/spi.h>
#include <drivers/drv_gyro.h>
#include <drivers/drv_hrt.h>
#include <lib/conversion/rotation.h>
#include <lib/perf/perf_counter.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <px4_config.h>
#include <systemlib/conversions.h>
#define BMI055_DEVICE_PATH_GYRO "/dev/bmi055_gyro"
#define BMI055_DEVICE_PATH_GYRO_EXT "/dev/bmi055_gyro_ext"
// BMI055 Gyro registers
#define BMI055_GYR_CHIP_ID 0x00
#define BMI055_GYR_X_L 0x02
#define BMI055_GYR_X_H 0x03
#define BMI055_GYR_Y_L 0x04
#define BMI055_GYR_Y_H 0x05
#define BMI055_GYR_Z_L 0x06
#define BMI055_GYR_Z_H 0x07
#define BMI055_GYR_INT_STATUS_0 0x09
#define BMI055_GYR_INT_STATUS_1 0x0A
#define BMI055_GYR_INT_STATUS_2 0x0B
#define BMI055_GYR_INT_STATUS_3 0x0C
#define BMI055_GYR_FIFO_STATUS 0x0E
#define BMI055_GYR_RANGE 0x0F
#define BMI055_GYR_BW 0x10
#define BMI055_GYR_LPM1 0x11
#define BMI055_GYR_LPM2 0x12
#define BMI055_GYR_RATE_HBW 0x13
#define BMI055_GYR_SOFTRESET 0x14
#define BMI055_GYR_INT_EN_0 0x15
#define BMI055_GYR_INT_EN_1 0x16
#define BMI055_GYR_INT_MAP_0 0x17
#define BMI055_GYR_INT_MAP_1 0x18
#define BMI055_GYR_INT_MAP_2 0x19
#define BMI055_GYRO_0_REG 0x1A
#define BMI055_GYRO_1_REG 0x1B
#define BMI055_GYRO_2_REG 0x1C
#define BMI055_GYRO_3_REG 0x1E
#define BMI055_GYR_INT_LATCH 0x21
#define BMI055_GYR_INT_LH_0 0x22
#define BMI055_GYR_INT_LH_1 0x23
#define BMI055_GYR_INT_LH_2 0x24
#define BMI055_GYR_INT_LH_3 0x25
#define BMI055_GYR_INT_LH_4 0x26
#define BMI055_GYR_INT_LH_5 0x27
#define BMI055_GYR_SOC 0x31
#define BMI055_GYR_A_FOC 0x32
#define BMI055_GYR_TRIM_NVM_CTRL 0x33
#define BMI055_BGW_SPI3_WDT 0x34
#define BMI055_GYR_OFFSET_COMP 0x36
#define BMI055_GYR_OFFSET_COMP_X 0x37
#define BMI055_GYR_OFFSET_COMP_Y 0x38
#define BMI055_GYR_OFFSET_COMP_Z 0x39
#define BMI055_GYR_TRIM_GPO 0x3A
#define BMI055_GYR_TRIM_GP1 0x3B
#define BMI055_GYR_SELF_TEST 0x3C
#define BMI055_GYR_FIFO_CONFIG_0 0x3D
#define BMI055_GYR_FIFO_CONFIG_1 0x3E
#define BMI055_GYR_FIFO_DATA 0x3F
// BMI055 Gyroscope Chip-Id
#define BMI055_GYR_WHO_AM_I 0x0F
//BMI055_GYR_BW 0x10
#define BMI055_GYRO_RATE_100 (0<<3) | (1<<2) | (1<<1) | (1<<0)
#define BMI055_GYRO_RATE_200 (0<<3) | (1<<2) | (1<<1) | (0<<0)
#define BMI055_GYRO_RATE_400 (0<<3) | (0<<2) | (1<<1) | (1<<0)
#define BMI055_GYRO_RATE_1000 (0<<3) | (0<<2) | (1<<1) | (0<<0)
#define BMI055_GYRO_RATE_2000 (0<<3) | (0<<2) | (0<<1) | (1<<0)
//BMI055_GYR_LPM1 0x11
#define BMI055_GYRO_NORMAL (0<<7) | (0<<5)
#define BMI055_GYRO_DEEP_SUSPEND (0<<7) | (1<<5)
#define BMI055_GYRO_SUSPEND (1<<7) | (0<<5)
//BMI055_GYR_RANGE 0x0F
#define BMI055_GYRO_RANGE_2000_DPS (0<<2) | (0<<1) | (0<<0)
#define BMI055_GYRO_RANGE_1000_DPS (0<<2) | (0<<1) | (1<<0)
#define BMI055_GYRO_RANGE_500_DPS (0<<2) | (1<<1) | (0<<0)
#define BMI055_GYRO_RANGE_250_DPS (0<<2) | (1<<1) | (1<<0)
#define BMI055_GYRO_RANGE_125_DPS (1<<2) | (0<<1) | (0<<0)
//BMI055_GYR_INT_EN_0 0x15
#define BMI055_GYR_DRDY_INT_EN (1<<7)
//BMI055_GYR_INT_MAP_1 0x18
#define BMI055_GYR_DRDY_INT1 (1<<0)
// Default and Max values
#define BMI055_GYRO_DEFAULT_RANGE_DPS 2000
#define BMI055_GYRO_DEFAULT_RATE 1000
#define BMI055_GYRO_MAX_RATE 1000
#define BMI055_GYRO_MAX_PUBLISH_RATE 280
#define BMI055_GYRO_DEFAULT_DRIVER_FILTER_FREQ 50
/* Mask definitions for Gyro bandwidth */
#define BMI055_GYRO_BW_MASK 0x0F
#define BMI055_ACC_TEMP 0x08
class BMI055_gyro : public BMI055
{
public:
BMI055_gyro(int bus, const char *path_gyro, uint32_t device, enum Rotation rotation);
virtual ~BMI055_gyro();
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
void print_registers();
// deliberately cause a sensor error
void test_error();
protected:
virtual int probe();
private:
ringbuffer::RingBuffer *_gyro_reports;
struct gyro_calibration_s _gyro_scale;
float _gyro_range_scale;
float _gyro_range_rad_s;
orb_advert_t _gyro_topic;
int _gyro_orb_class_instance;
int _gyro_class_instance;
float _gyro_sample_rate;
perf_counter_t _gyro_reads;
math::LowPassFilter2p _gyro_filter_x;
math::LowPassFilter2p _gyro_filter_y;
math::LowPassFilter2p _gyro_filter_z;
Integrator _gyro_int;
// this is used to support runtime checking of key
// configuration registers to detect SPI bus errors and sensor
// reset
#define BMI055_GYRO_NUM_CHECKED_REGISTERS 7
static const uint8_t _checked_registers[BMI055_GYRO_NUM_CHECKED_REGISTERS];
uint8_t _checked_values[BMI055_GYRO_NUM_CHECKED_REGISTERS];
uint8_t _checked_bad[BMI055_GYRO_NUM_CHECKED_REGISTERS];
// last temperature reading for print_info()
float _last_temperature;
/**
* Start automatic measurement.
*/
void start();
/**
* Stop automatic measurement.
*/
void stop();
/**
* Reset chip.
*
* Resets the chip and measurements ranges, but not scale and offset.
*/
int reset();
/**
* Static trampoline from the hrt_call context; because we don't have a
* generic hrt wrapper yet.
*
* Called by the HRT in interrupt context at the specified rate if
* automatic polling is enabled.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void measure_trampoline(void *arg);
/**
* Fetch measurements from the sensor and update the report buffers.
*/
void measure();
/**
* Modify a register in the BMI055_gyro
*
* Bits are cleared before bits are set.
*
* @param reg The register to modify.
* @param clearbits Bits in the register to clear.
* @param setbits Bits in the register to set.
*/
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
/**
* Write a register in the BMI055_gyro, updating _checked_values
*
* @param reg The register to write.
* @param value The new value to write.
*/
void write_checked_reg(unsigned reg, uint8_t value);
/**
* Set the BMI055_gyro measurement range.
*
* @param max_dps The maximum DPS value the range must support.
* @return OK if the value can be supported, -EINVAL otherwise.
*/
int set_gyro_range(unsigned max_dps);
/**
* Measurement self test
*
* @return 0 on success, 1 on failure
*/
int self_test();
/**
* Gyro self test
*
* @return 0 on success, 1 on failure
*/
int gyro_self_test();
/*
* set gyro sample rate
*/
int gyro_set_sample_rate(float desired_sample_rate_hz);
/*
* check that key registers still have the right value
*/
void check_registers(void);
/* do not allow to copy this class due to pointer data members */
BMI055_gyro(const BMI055_gyro &);
BMI055_gyro operator=(const BMI055_gyro &);
#pragma pack(push, 1)
/**
* Report conversation within the BMI055_gyro, including command byte and
* interrupt status.
*/
struct BMI_GyroReport {
uint8_t cmd;
int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;
};
#pragma pack(pop)
};

5
src/drivers/imu/bmi055/CMakeLists.txt
View File

@ -36,9 +36,8 @@ px4_add_module(
STACK_MAIN 1200
COMPILE_FLAGS
SRCS
bmi055_accel.cpp
bmi055_gyro.cpp
BMI055_accel.cpp
BMI055_gyro.cpp
bmi055_main.cpp
DEPENDS
)

635
src/drivers/imu/bmi055/bmi055.hpp
View File

@ -1,635 +0,0 @@
#ifndef BMI055_HPP_
#define BMI055_HPP_
#include <px4_config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <stddef.h>
#include <stdlib.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <getopt.h>
#include <perf/perf_counter.h>
#include <systemlib/err.h>
#include <systemlib/conversions.h>
#include <nuttx/arch.h>
#include <nuttx/clock.h>
#include <board_config.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/spi.h>
#include <drivers/device/ringbuffer.h>
#include <drivers/device/integrator.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <drivers/drv_mag.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <lib/conversion/rotation.h>
#define DIR_READ 0x80
#define DIR_WRITE 0x00
#define BMI055_DEVICE_PATH_ACCEL "/dev/bmi055_accel"
#define BMI055_DEVICE_PATH_GYRO "/dev/bmi055_gyro"
#define BMI055_DEVICE_PATH_ACCEL_EXT "/dev/bmi055_accel_ext"
#define BMI055_DEVICE_PATH_GYRO_EXT "/dev/bmi055_gyro_ext"
// BMI055 Accel registers
#define BMI055_ACC_CHIP_ID 0x00
#define BMI055_ACC_X_L 0x02
#define BMI055_ACC_X_H 0x03
#define BMI055_ACC_Y_L 0x04
#define BMI055_ACC_Y_H 0x05
#define BMI055_ACC_Z_L 0x06
#define BMI055_ACC_Z_H 0x07
#define BMI055_ACC_TEMP 0x08
#define BMI055_ACC_INT_STATUS_0 0x09
#define BMI055_ACC_INT_STATUS_1 0x0A
#define BMI055_ACC_INT_STATUS_2 0x0B
#define BMI055_ACC_INT_STATUS_3 0x0C
#define BMI055_ACC_FIFO_STATUS 0x0E
#define BMI055_ACC_RANGE 0x0F
#define BMI055_ACC_BW 0x10
#define BMI055_ACC_PMU_LPW 0x11
#define BMI055_ACC_PMU_LOW_POWER 0x12
#define BMI055_ACC_DATA_CTRL 0x13
#define BMI055_ACC_SOFTRESET 0x14
#define BMI055_ACC_INT_EN_0 0x16
#define BMI055_ACC_INT_EN_1 0x17
#define BMI055_ACC_INT_EN_2 0x18
#define BMI055_ACC_INT_MAP_0 0x19
#define BMI055_ACC_INT_MAP_1 0x1A
#define BMI055_ACC_INT_MAP_2 0x1B
#define BMI055_ACC_INT_SRC 0x1E
#define BMI055_ACC_INT_OUT_CTRL 0x20
#define BMI055_ACC_INT_LATCH 0x21
#define BMI055_ACC_INT_LH_0 0x22
#define BMI055_ACC_INT_LH_1 0x23
#define BMI055_ACC_INT_LH_2 0x24
#define BMI055_ACC_INT_LH_3 0x25
#define BMI055_ACC_INT_LH_4 0x26
#define BMI055_ACC_INT_MOT_0 0x27
#define BMI055_ACC_INT_MOT_1 0x28
#define BMI055_ACC_INT_MOT_2 0x29
#define BMI055_ACC_INT_TAP_0 0x2A
#define BMI055_ACC_INT_TAP_1 0x2B
#define BMI055_ACC_INT_ORIE_0 0x2C
#define BMI055_ACC_INT_ORIE_1 0x2D
#define BMI055_ACC_INT_FLAT_0 0x2E
#define BMI055_ACC_INT_FLAT_1 0x2F
#define BMI055_ACC_FIFO_CONFIG_0 0x30
#define BMI055_ACC_SELF_TEST 0x32
#define BMI055_ACC_EEPROM_CTRL 0x33
#define BMI055_ACC_SERIAL_CTRL 0x34
#define BMI055_ACC_OFFSET_CTRL 0x36
#define BMI055_ACC_OFC_SETTING 0x37
#define BMI055_ACC_OFFSET_X 0x38
#define BMI055_ACC_OFFSET_Y 0x39
#define BMI055_ACC_OFFSET_Z 0x3A
#define BMI055_ACC_TRIM_GPO 0x3B
#define BMI055_ACC_TRIM_GP1 0x3C
#define BMI055_ACC_FIFO_CONFIG_1 0x3E
#define BMI055_ACC_FIFO_DATA 0x3F
// BMI055 Gyro registers
#define BMI055_GYR_CHIP_ID 0x00
#define BMI055_GYR_X_L 0x02
#define BMI055_GYR_X_H 0x03
#define BMI055_GYR_Y_L 0x04
#define BMI055_GYR_Y_H 0x05
#define BMI055_GYR_Z_L 0x06
#define BMI055_GYR_Z_H 0x07
#define BMI055_GYR_INT_STATUS_0 0x09
#define BMI055_GYR_INT_STATUS_1 0x0A
#define BMI055_GYR_INT_STATUS_2 0x0B
#define BMI055_GYR_INT_STATUS_3 0x0C
#define BMI055_GYR_FIFO_STATUS 0x0E
#define BMI055_GYR_RANGE 0x0F
#define BMI055_GYR_BW 0x10
#define BMI055_GYR_LPM1 0x11
#define BMI055_GYR_LPM2 0x12
#define BMI055_GYR_RATE_HBW 0x13
#define BMI055_GYR_SOFTRESET 0x14
#define BMI055_GYR_INT_EN_0 0x15
#define BMI055_GYR_INT_EN_1 0x16
#define BMI055_GYR_INT_MAP_0 0x17
#define BMI055_GYR_INT_MAP_1 0x18
#define BMI055_GYR_INT_MAP_2 0x19
#define BMI055_GYRO_0_REG 0x1A
#define BMI055_GYRO_1_REG 0x1B
#define BMI055_GYRO_2_REG 0x1C
#define BMI055_GYRO_3_REG 0x1E
#define BMI055_GYR_INT_LATCH 0x21
#define BMI055_GYR_INT_LH_0 0x22
#define BMI055_GYR_INT_LH_1 0x23
#define BMI055_GYR_INT_LH_2 0x24
#define BMI055_GYR_INT_LH_3 0x25
#define BMI055_GYR_INT_LH_4 0x26
#define BMI055_GYR_INT_LH_5 0x27
#define BMI055_GYR_SOC 0x31
#define BMI055_GYR_A_FOC 0x32
#define BMI055_GYR_TRIM_NVM_CTRL 0x33
#define BMI055_BGW_SPI3_WDT 0x34
#define BMI055_GYR_OFFSET_COMP 0x36
#define BMI055_GYR_OFFSET_COMP_X 0x37
#define BMI055_GYR_OFFSET_COMP_Y 0x38
#define BMI055_GYR_OFFSET_COMP_Z 0x39
#define BMI055_GYR_TRIM_GPO 0x3A
#define BMI055_GYR_TRIM_GP1 0x3B
#define BMI055_GYR_SELF_TEST 0x3C
#define BMI055_GYR_FIFO_CONFIG_0 0x3D
#define BMI055_GYR_FIFO_CONFIG_1 0x3E
#define BMI055_GYR_FIFO_DATA 0x3F
// BMI055 Accelerometer Chip-Id
#define BMI055_ACC_WHO_AM_I 0xFA
// BMI055 Gyroscope Chip-Id
#define BMI055_GYR_WHO_AM_I 0x0F
//BMI055_ACC_BW 0x10
#define BMI055_ACCEL_BW_7_81 (1<<3) | (0<<2) | (0<<1) | (0<<0)
#define BMI055_ACCEL_BW_15_63 (1<<3) | (0<<2) | (0<<1) | (1<<0)
#define BMI055_ACCEL_BW_31_25 (1<<3) | (0<<2) | (1<<1) | (0<<0)
#define BMI055_ACCEL_BW_62_5 (1<<3) | (0<<2) | (1<<1) | (1<<0)
#define BMI055_ACCEL_BW_125 (1<<3) | (1<<2) | (0<<1) | (0<<0)
#define BMI055_ACCEL_BW_250 (1<<3) | (1<<2) | (0<<1) | (1<<0)
#define BMI055_ACCEL_BW_500 (1<<3) | (1<<2) | (1<<1) | (0<<0)
#define BMI055_ACCEL_BW_1000 (1<<3) | (1<<2) | (1<<1) | (1<<0)
//BMI055_ACC_PMU_LPW 0x11
#define BMI055_ACCEL_NORMAL (0<<7) | (0<<6) | (0<<5)
#define BMI055_ACCEL_DEEP_SUSPEND (0<<7) | (0<<6) | (1<<5)
#define BMI055_ACCEL_LOW_POWER (0<<7) | (1<<6) | (0<<5)
#define BMI055_ACCEL_SUSPEND (1<<7) | (0<<6) | (0<<5)
//BMI055_ACC_RANGE 0x0F
#define BMI055_ACCEL_RANGE_2_G (0<<3) | (0<<2) | (1<<1) | (1<<0)
#define BMI055_ACCEL_RANGE_4_G (0<<3) | (1<<2) | (0<<1) | (1<<0)
#define BMI055_ACCEL_RANGE_8_G (1<<3) | (0<<2) | (0<<1) | (0<<0)
#define BMI055_ACCEL_RANGE_16_G (1<<3) | (1<<2) | (0<<1) | (0<<0)
//BMI055_GYR_BW 0x10
#define BMI055_GYRO_RATE_100 (0<<3) | (1<<2) | (1<<1) | (1<<0)
#define BMI055_GYRO_RATE_200 (0<<3) | (1<<2) | (1<<1) | (0<<0)
#define BMI055_GYRO_RATE_400 (0<<3) | (0<<2) | (1<<1) | (1<<0)
#define BMI055_GYRO_RATE_1000 (0<<3) | (0<<2) | (1<<1) | (0<<0)
#define BMI055_GYRO_RATE_2000 (0<<3) | (0<<2) | (0<<1) | (1<<0)
//BMI055_GYR_LPM1 0x11
#define BMI055_GYRO_NORMAL (0<<7) | (0<<5)
#define BMI055_GYRO_DEEP_SUSPEND (0<<7) | (1<<5)
#define BMI055_GYRO_SUSPEND (1<<7) | (0<<5)
//BMI055_GYR_RANGE 0x0F
#define BMI055_GYRO_RANGE_2000_DPS (0<<2) | (0<<1) | (0<<0)
#define BMI055_GYRO_RANGE_1000_DPS (0<<2) | (0<<1) | (1<<0)
#define BMI055_GYRO_RANGE_500_DPS (0<<2) | (1<<1) | (0<<0)
#define BMI055_GYRO_RANGE_250_DPS (0<<2) | (1<<1) | (1<<0)
#define BMI055_GYRO_RANGE_125_DPS (1<<2) | (0<<1) | (0<<0)
//BMI055_ACC_INT_EN_1 0x17
#define BMI055_ACC_DRDY_INT_EN (1<<4)
//BMI055_GYR_INT_EN_0 0x15
#define BMI055_GYR_DRDY_INT_EN (1<<7)
//BMI055_ACC_INT_MAP_1 0x1A
#define BMI055_ACC_DRDY_INT1 (1<<0)
//BMI055_GYR_INT_MAP_1 0x18
#define BMI055_GYR_DRDY_INT1 (1<<0)
//Soft-reset command Value
#define BMI055_SOFT_RESET 0xB6
// Default and Max values
#define BMI055_ACCEL_DEFAULT_RANGE_G 16
#define BMI055_GYRO_DEFAULT_RANGE_DPS 2000
#define BMI055_ACCEL_DEFAULT_RATE 1000
#define BMI055_ACCEL_MAX_RATE 1000
#define BMI055_ACCEL_MAX_PUBLISH_RATE 280
#define BMI055_GYRO_DEFAULT_RATE 1000
#define BMI055_GYRO_MAX_RATE 1000
#define BMI055_GYRO_MAX_PUBLISH_RATE BMI055_ACCEL_MAX_PUBLISH_RATE
#define BMI055_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 50
#define BMI055_GYRO_DEFAULT_DRIVER_FILTER_FREQ 50
#define BMI055_BUS_SPEED 10*1000*1000
#define BMI055_TIMER_REDUCTION 200
/* Mask definitions for ACCD_X_LSB, ACCD_Y_LSB and ACCD_Z_LSB Register */
#define BMI055_NEW_DATA_MASK 0x01
/* Mask definitions for Gyro bandwidth */
#define BMI055_GYRO_BW_MASK 0x0F
class BMI055 : public device::SPI
{
protected:
uint8_t _whoami; /** whoami result */
struct hrt_call _call;
unsigned _call_interval;
unsigned _dlpf_freq;
perf_counter_t _sample_perf;
perf_counter_t _bad_transfers;
perf_counter_t _bad_registers;
perf_counter_t _good_transfers;
perf_counter_t _reset_retries;
perf_counter_t _duplicates;
uint8_t _register_wait;
uint64_t _reset_wait;
enum Rotation _rotation;
uint8_t _checked_next;
/**
* Read a register from the BMI055
*
* @param The register to read.
* @return The value that was read.
*/
uint8_t read_reg(unsigned reg);
uint16_t read_reg16(unsigned reg);
/**
* Write a register in the BMI055
*
* @param reg The register to write.
* @param value The new value to write.
*/
void write_reg(unsigned reg, uint8_t value);
/* do not allow to copy this class due to pointer data members */
BMI055(const BMI055 &);
BMI055 operator=(const BMI055 &);
public:
BMI055(const char *name, const char *devname, int bus, uint32_t device, enum spi_mode_e mode, uint32_t frequency,
enum Rotation rotation);
virtual ~BMI055();
};
class BMI055_accel : public BMI055
{
public:
BMI055_accel(int bus, const char *path_accel, uint32_t device, enum Rotation rotation);
virtual ~BMI055_accel();
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
void print_registers();
// deliberately cause a sensor error
void test_error();
protected:
virtual int probe();
private:
ringbuffer::RingBuffer *_accel_reports;
struct accel_calibration_s _accel_scale;
float _accel_range_scale;
float _accel_range_m_s2;
orb_advert_t _accel_topic;
int _accel_orb_class_instance;
int _accel_class_instance;
float _accel_sample_rate;
perf_counter_t _accel_reads;
math::LowPassFilter2p _accel_filter_x;
math::LowPassFilter2p _accel_filter_y;
math::LowPassFilter2p _accel_filter_z;
Integrator _accel_int;
// this is used to support runtime checking of key
// configuration registers to detect SPI bus errors and sensor
// reset
#define BMI055_ACCEL_NUM_CHECKED_REGISTERS 5
static const uint8_t _checked_registers[BMI055_ACCEL_NUM_CHECKED_REGISTERS];
uint8_t _checked_values[BMI055_ACCEL_NUM_CHECKED_REGISTERS];
uint8_t _checked_bad[BMI055_ACCEL_NUM_CHECKED_REGISTERS];
// last temperature reading for print_info()
float _last_temperature;
bool _got_duplicate;
/**
* Start automatic measurement.
*/
void start();
/**
* Stop automatic measurement.
*/
void stop();
/**
* Reset chip.
*
* Resets the chip and measurements ranges, but not scale and offset.
*/
int reset();
/**
* Static trampoline from the hrt_call context; because we don't have a
* generic hrt wrapper yet.
*
* Called by the HRT in interrupt context at the specified rate if
* automatic polling is enabled.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void measure_trampoline(void *arg);
/**
* Fetch measurements from the sensor and update the report buffers.
*/
void measure();
/**
* Modify a register in the BMI055_accel
*
* Bits are cleared before bits are set.
*
* @param reg The register to modify.
* @param clearbits Bits in the register to clear.
* @param setbits Bits in the register to set.
*/
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
/**
* Write a register in the BMI055_accel, updating _checked_values
*
* @param reg The register to write.
* @param value The new value to write.
*/
void write_checked_reg(unsigned reg, uint8_t value);
/**
* Set the BMI055_accel measurement range.
*
* @param max_g The maximum G value the range must support.
* @return OK if the value can be supported, -EINVAL otherwise.
*/
int set_accel_range(unsigned max_g);
/**
* Measurement self test
*
* @return 0 on success, 1 on failure
*/
int self_test();
/**
* Accel self test
*
* @return 0 on success, 1 on failure
*/
int accel_self_test();
/*
set accel sample rate
*/
int accel_set_sample_rate(float desired_sample_rate_hz);
/*
check that key registers still have the right value
*/
void check_registers(void);
/* do not allow to copy this class due to pointer data members */
BMI055_accel(const BMI055_accel &);
BMI055_accel operator=(const BMI055_accel &);
};
class BMI055_gyro : public BMI055
{
public:
BMI055_gyro(int bus, const char *path_gyro, uint32_t device, enum Rotation rotation);
virtual ~BMI055_gyro();
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
void print_registers();
// deliberately cause a sensor error
void test_error();
protected:
virtual int probe();
private:
ringbuffer::RingBuffer *_gyro_reports;
struct gyro_calibration_s _gyro_scale;
float _gyro_range_scale;
float _gyro_range_rad_s;
orb_advert_t _gyro_topic;
int _gyro_orb_class_instance;
int _gyro_class_instance;
float _gyro_sample_rate;
perf_counter_t _gyro_reads;
math::LowPassFilter2p _gyro_filter_x;
math::LowPassFilter2p _gyro_filter_y;
math::LowPassFilter2p _gyro_filter_z;
Integrator _gyro_int;
// this is used to support runtime checking of key
// configuration registers to detect SPI bus errors and sensor
// reset
#define BMI055_GYRO_NUM_CHECKED_REGISTERS 7
static const uint8_t _checked_registers[BMI055_GYRO_NUM_CHECKED_REGISTERS];
uint8_t _checked_values[BMI055_GYRO_NUM_CHECKED_REGISTERS];
uint8_t _checked_bad[BMI055_GYRO_NUM_CHECKED_REGISTERS];
// last temperature reading for print_info()
float _last_temperature;
/**
* Start automatic measurement.
*/
void start();
/**
* Stop automatic measurement.
*/
void stop();
/**
* Reset chip.
*
* Resets the chip and measurements ranges, but not scale and offset.
*/
int reset();
/**
* Static trampoline from the hrt_call context; because we don't have a
* generic hrt wrapper yet.
*
* Called by the HRT in interrupt context at the specified rate if
* automatic polling is enabled.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void measure_trampoline(void *arg);
/**
* Fetch measurements from the sensor and update the report buffers.
*/
void measure();
/**
* Modify a register in the BMI055_gyro
*
* Bits are cleared before bits are set.
*
* @param reg The register to modify.
* @param clearbits Bits in the register to clear.
* @param setbits Bits in the register to set.
*/
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
/**
* Write a register in the BMI055_gyro, updating _checked_values
*
* @param reg The register to write.
* @param value The new value to write.
*/
void write_checked_reg(unsigned reg, uint8_t value);
/**
* Set the BMI055_gyro measurement range.
*
* @param max_dps The maximum DPS value the range must support.
* @return OK if the value can be supported, -EINVAL otherwise.
*/
int set_gyro_range(unsigned max_dps);
/**
* Measurement self test
*
* @return 0 on success, 1 on failure
*/
int self_test();
/**
* Gyro self test
*
* @return 0 on success, 1 on failure
*/
int gyro_self_test();
/*
* set gyro sample rate
*/
int gyro_set_sample_rate(float desired_sample_rate_hz);
/*
* check that key registers still have the right value
*/
void check_registers(void);
/* do not allow to copy this class due to pointer data members */
BMI055_gyro(const BMI055_gyro &);
BMI055_gyro operator=(const BMI055_gyro &);
#pragma pack(push, 1)
/**
* Report conversation within the BMI055_gyro, including command byte and
* interrupt status.
*/
struct BMI_GyroReport {
uint8_t cmd;
int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;
};
#pragma pack(pop)
};
#endif /* BMI055_HPP_ */

40
src/drivers/imu/bmi055/bmi055_main.cpp
View File

@ -1,6 +1,41 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "BMI055_accel.hpp"
#include "BMI055_gyro.hpp"
#include <px4_config.h>
#include <platforms/px4_getopt.h>
#include "bmi055.hpp"
/** driver 'main' command */
extern "C" { __EXPORT int bmi055_main(int argc, char *argv[]); }
@ -15,7 +50,6 @@ enum sensor_type {
BMI055_GYRO
};
namespace bmi055
{
@ -24,7 +58,6 @@ BMI055_accel *g_acc_dev_ext; // on external bus (accel)
BMI055_gyro *g_gyr_dev_int; // on internal bus (gyro)
BMI055_gyro *g_gyr_dev_ext; // on external bus (gyro)
void start(bool, enum Rotation, enum sensor_type);
void stop(bool, enum sensor_type);
void test(bool, enum sensor_type);
@ -51,7 +84,6 @@ start(bool external_bus, enum Rotation rotation, enum sensor_type sensor)
BMI055_gyro **g_dev_gyr_ptr = external_bus ? &g_gyr_dev_ext : &g_gyr_dev_int;
const char *path_gyro = external_bus ? BMI055_DEVICE_PATH_GYRO_EXT : BMI055_DEVICE_PATH_GYRO;
if (sensor == BMI055_ACCEL) {
if (*g_dev_acc_ptr != nullptr)
/* if already started, the still command succeeded */