diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake index 1c274981d3..f268af3848 100644 --- a/cmake/configs/nuttx_px4fmu-v4_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -51,6 +51,7 @@ set(config_module_list drivers/bmp280 drivers/bma180 drivers/bmi160 + drivers/bmi055 drivers/tap_esc drivers/iridiumsbd diff --git a/src/drivers/bmi055/CMakeLists.txt b/src/drivers/bmi055/CMakeLists.txt new file mode 100644 index 0000000000..a7c48d907c --- /dev/null +++ b/src/drivers/bmi055/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# Copyright (c) 2015 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. +# +############################################################################ +px4_add_module( + MODULE drivers__bmi055 + MAIN bmi055 + STACK_MAIN 1200 + COMPILE_FLAGS + -Weffc++ + SRCS + bmi055_accel.cpp + bmi055_gyro.cpp + bmi055_main.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/bmi055/bmi055.hpp b/src/drivers/bmi055/bmi055.hpp new file mode 100644 index 0000000000..95c6b5fce0 --- /dev/null +++ b/src/drivers/bmi055/bmi055.hpp @@ -0,0 +1,660 @@ +#ifndef BMI055_HPP_ +#define BMI055_HPP_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#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 8 +#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_ONE_G 9.80665f + +#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 + +#ifdef PX4_SPI_BUS_EXT +#define EXTERNAL_BUS PX4_SPI_BUS_EXT +#else +#define EXTERNAL_BUS 0 +#endif + +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; + perf_counter_t _controller_latency_perf; + + 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, enum spi_dev_e 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, spi_dev_e 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); + + + /** + * Get the internal / external state + * + * @return true if the sensor is not on the main MCU board + */ + bool is_external() { return (_bus == EXTERNAL_BUS); } + + /** + * 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, spi_dev_e 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); + + + /** + * Get the internal / external state + * + * @return true if the sensor is not on the main MCU board + */ + bool is_external() { return (_bus == EXTERNAL_BUS); } + + /** + * 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_ */ diff --git a/src/drivers/bmi055/bmi055_accel.cpp b/src/drivers/bmi055/bmi055_accel.cpp new file mode 100644 index 0000000000..20997a3cf3 --- /dev/null +++ b/src/drivers/bmi055/bmi055_accel.cpp @@ -0,0 +1,893 @@ +#include "bmi055.hpp" + + +/* + list of registers that will be checked in check_registers(). Note + that ADDR_WHO_AM_I must be first in the list. + */ +const uint8_t BMI055_accel::_checked_registers[BMI055_ACCEL_NUM_CHECKED_REGISTERS] = { BMI055_ACC_CHIP_ID, + BMI055_ACC_BW, + BMI055_ACC_RANGE, + BMI055_ACC_INT_EN_1, + BMI055_ACC_INT_MAP_1, + }; + + + +BMI055_accel::BMI055_accel(int bus, const char *path_accel, spi_dev_e device, enum Rotation rotation) : + BMI055("BMI055_ACCEL", path_accel, bus, device, SPIDEV_MODE3, BMI055_BUS_SPEED, rotation), + _accel_reports(nullptr), + _accel_scale{}, + _accel_range_scale(0.0f), + _accel_range_m_s2(0.0f), + _accel_topic(nullptr), + _accel_orb_class_instance(-1), + _accel_class_instance(-1), + _accel_sample_rate(BMI055_ACCEL_DEFAULT_RATE), + _accel_reads(perf_alloc(PC_COUNT, "bmi055_accel_read")), + _accel_filter_x(BMI055_ACCEL_DEFAULT_RATE, BMI055_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _accel_filter_y(BMI055_ACCEL_DEFAULT_RATE, BMI055_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _accel_filter_z(BMI055_ACCEL_DEFAULT_RATE, BMI055_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _accel_int(1000000 / BMI055_ACCEL_MAX_PUBLISH_RATE), + _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 + _accel_scale.x_offset = 0; + _accel_scale.x_scale = 1.0f; + _accel_scale.y_offset = 0; + _accel_scale.y_scale = 1.0f; + _accel_scale.z_offset = 0; + _accel_scale.z_scale = 1.0f; + + memset(&_call, 0, sizeof(_call)); +} + + +BMI055_accel::~BMI055_accel() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_accel_reports != nullptr) { + delete _accel_reports; + } + + + if (_accel_class_instance != -1) { + unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); + } + + /* delete the perf counter */ + perf_free(_accel_reads); +} + + +int +BMI055_accel::init() +{ + int ret; + + /* do SPI init (and probe) first */ + ret = SPI::init(); + + /* if probe/setup failed, bail now */ + if (ret != OK) { + warnx("SPI error"); + DEVICE_DEBUG("SPI setup failed"); + return ret; + } + + /* allocate basic report buffers */ + _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); + + if (_accel_reports == nullptr) { + goto out; + } + + if (reset() != OK) { + goto out; + } + + /* Initialize offsets and scales */ + _accel_scale.x_offset = 0; + _accel_scale.x_scale = 1.0f; + _accel_scale.y_offset = 0; + _accel_scale.y_scale = 1.0f; + _accel_scale.z_offset = 0; + _accel_scale.z_scale = 1.0f; + + + _accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH); + + measure(); + + /* advertise sensor topic, measure manually to initialize valid report */ + struct accel_report arp; + _accel_reports->get(&arp); + + /* measurement will have generated a report, publish */ + _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, + &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1); + + if (_accel_topic == nullptr) { + warnx("ADVERT FAIL"); + } + +out: + return ret; +} + +int BMI055_accel::reset() +{ + write_reg(BMI055_ACC_SOFTRESET, BMI055_SOFT_RESET);//Soft-reset + up_udelay(5000); + + write_checked_reg(BMI055_ACC_BW, BMI055_ACCEL_BW_1000); //Write accel bandwidth + write_checked_reg(BMI055_ACC_RANGE, BMI055_ACCEL_RANGE_2_G);//Write range + write_checked_reg(BMI055_ACC_INT_EN_1, BMI055_ACC_DRDY_INT_EN); //Enable DRDY interrupt + write_checked_reg(BMI055_ACC_INT_MAP_1, BMI055_ACC_DRDY_INT1); //Map DRDY interrupt on pin INT1 + + set_accel_range(BMI055_ACCEL_DEFAULT_RANGE_G);//set accel range + accel_set_sample_rate(BMI055_ACCEL_DEFAULT_RATE);//set accel ODR + + //Enable Accelerometer in normal mode + write_reg(BMI055_ACC_PMU_LPW, BMI055_ACCEL_NORMAL); + up_udelay(1000); + + uint8_t retries = 10; + + while (retries--) { + bool all_ok = true; + + for (uint8_t i = 0; i < BMI055_ACCEL_NUM_CHECKED_REGISTERS; i++) { + if (read_reg(_checked_registers[i]) != _checked_values[i]) { + write_reg(_checked_registers[i], _checked_values[i]); + all_ok = false; + } + } + + if (all_ok) { + break; + } + } + + _accel_reads = 0; + + return OK; +} + + +int +BMI055_accel::probe() +{ + /* look for device ID */ + _whoami = read_reg(BMI055_ACC_CHIP_ID); + + // verify product revision + switch (_whoami) { + case BMI055_ACC_WHO_AM_I: + memset(_checked_values, 0, sizeof(_checked_values)); + memset(_checked_bad, 0, sizeof(_checked_bad)); + _checked_values[0] = _whoami; + _checked_bad[0] = _whoami; + return OK; + } + + DEVICE_DEBUG("unexpected whoami 0x%02x", _whoami); + 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; + + } else if (frequency < (625 / 10)) { + setbits |= BMI055_ACCEL_BW_15_63; + _accel_sample_rate = 625 / 10; + + } else if (frequency < (125)) { + setbits |= BMI055_ACCEL_BW_31_25; + _accel_sample_rate = 625 / 10; + + } else if (frequency < 250) { + setbits |= BMI055_ACCEL_BW_62_5; + _accel_sample_rate = 125; + + } else if (frequency < 500) { + setbits |= BMI055_ACCEL_BW_125; + _accel_sample_rate = 250; + + } else if (frequency < 1000) { + setbits |= BMI055_ACCEL_BW_250; + _accel_sample_rate = 500; + + } else if (frequency < 2000) { + setbits |= BMI055_ACCEL_BW_500; + _accel_sample_rate = 1000; + + } else if (frequency >= 2000) { + setbits |= BMI055_ACCEL_BW_1000; + _accel_sample_rate = 2000; + + } else { + return -EINVAL; + } + + /* Write accel ODR */ + modify_reg(BMI055_ACC_BW, clearbits, setbits); + + return OK; +} + + + +ssize_t +BMI055_accel::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(accel_report); + + /* buffer must be large enough */ + if (count < 1) { + return -ENOSPC; + } + + /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ + if (_call_interval == 0) { + _accel_reports->flush(); + measure(); + } + + /* if no data, error (we could block here) */ + if (_accel_reports->empty()) { + return -EAGAIN; + } + + perf_count(_accel_reads); + + /* copy reports out of our buffer to the caller */ + accel_report *arp = reinterpret_cast(buffer); + int transferred = 0; + + while (count--) { + if (!_accel_reports->get(arp)) { + break; + } + + transferred++; + arp++; + } + + /* return the number of bytes transferred */ + return (transferred * sizeof(accel_report)); +} + +int +BMI055_accel::self_test() +{ + if (perf_event_count(_sample_perf) == 0) { + measure(); + } + + /* return 0 on success, 1 else */ + return (perf_event_count(_sample_perf) > 0) ? 0 : 1; +} + +int +BMI055_accel::accel_self_test() +{ + if (self_test()) { + return 1; + } + + /* inspect accel offsets */ + if (fabsf(_accel_scale.x_offset) < 0.000001f) { + return 1; + } + + if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) { + return 1; + } + + if (fabsf(_accel_scale.y_offset) < 0.000001f) { + return 1; + } + + if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) { + return 1; + } + + if (fabsf(_accel_scale.z_offset) < 0.000001f) { + return 1; + } + + if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) { + return 1; + } + + return 0; +} + + +/* + deliberately trigger an error in the sensor to trigger recovery + */ +void +BMI055_accel::test_error() +{ + write_reg(BMI055_ACC_SOFTRESET, BMI055_SOFT_RESET); + ::printf("error triggered\n"); + print_registers(); +} + + +int +BMI055_accel::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCRESET: + return reset(); + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _call_interval = 0; + return OK; + + /* external signalling not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + 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 + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_call_interval == 0); + + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / arg; + + /* check against maximum rate */ + if (ticks < 1000) { + return -EINVAL; + } + + // adjust filters + float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); + float sample_rate = 1.0e6f / ticks; + + _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); + + /* update interval for next measurement */ + _call_interval = ticks; + + /* + set call interval faster than the sample time. We + then detect when we have duplicate samples and reject + them. This prevents aliasing due to a beat between the + stm32 clock and the bmi055 clock + */ + _call.period = _call_interval - BMI055_TIMER_REDUCTION; + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_call_interval == 0) { + return SENSOR_POLLRATE_MANUAL; + } + + return 1000000 / _call_interval; + + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = px4_enter_critical_section(); + + if (!_accel_reports->resize(arg)) { + px4_leave_critical_section(flags); + return -ENOMEM; + } + + px4_leave_critical_section(flags); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _accel_reports->size(); + + case ACCELIOCGSAMPLERATE: + return _accel_sample_rate; + + case ACCELIOCSSAMPLERATE: + return accel_set_sample_rate(arg); + + case ACCELIOCGLOWPASS: + return _accel_filter_x.get_cutoff_freq(); + + case ACCELIOCSLOWPASS: + // set software filtering + _accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); + _accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); + _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); + return OK; + + case ACCELIOCSSCALE: { + /* copy scale, but only if off by a few percent */ + struct accel_calibration_s *s = (struct accel_calibration_s *) arg; + float sum = s->x_scale + s->y_scale + s->z_scale; + + if (sum > 2.0f && sum < 4.0f) { + memcpy(&_accel_scale, s, sizeof(_accel_scale)); + return OK; + + } else { + return -EINVAL; + } + } + + case ACCELIOCGSCALE: + /* copy scale out */ + memcpy((struct accel_calibration_s *) arg, &_accel_scale, sizeof(_accel_scale)); + return OK; + + case ACCELIOCSRANGE: + return set_accel_range(arg); + + case ACCELIOCGRANGE: + return (unsigned long)((_accel_range_m_s2) / BMI055_ONE_G + 0.5f); + + case ACCELIOCSELFTEST: + return accel_self_test(); + +#ifdef ACCELIOCSHWLOWPASS + + case ACCELIOCSHWLOWPASS: + return OK; +#endif + +#ifdef ACCELIOCGHWLOWPASS + + case ACCELIOCGHWLOWPASS: + return _dlpf_freq; +#endif + + + default: + /* give it to the superclass */ + return SPI::ioctl(filp, cmd, arg); + } +} + + +void +BMI055_accel::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) +{ + uint8_t val; + + val = read_reg(reg); + val &= ~clearbits; + val |= setbits; + write_checked_reg(reg, val); +} + +void +BMI055_accel::write_checked_reg(unsigned reg, uint8_t value) +{ + write_reg(reg, value); + + for (uint8_t i = 0; i < BMI055_ACCEL_NUM_CHECKED_REGISTERS; i++) { + if (reg == _checked_registers[i]) { + _checked_values[i] = value; + _checked_bad[i] = value; + } + } +} + +int +BMI055_accel::set_accel_range(unsigned max_g) +{ + uint8_t setbits = 0; + uint8_t clearbits = BMI055_ACCEL_RANGE_2_G | BMI055_ACCEL_RANGE_16_G; + float lsb_per_g; + float max_accel_g; + + if (max_g == 0) { + max_g = 16; + } + + if (max_g <= 2) { + max_accel_g = 2; + setbits |= BMI055_ACCEL_RANGE_2_G; + lsb_per_g = 1024; + + } else if (max_g <= 4) { + max_accel_g = 4; + setbits |= BMI055_ACCEL_RANGE_4_G; + lsb_per_g = 512; + + } else if (max_g <= 8) { + max_accel_g = 8; + setbits |= BMI055_ACCEL_RANGE_8_G; + lsb_per_g = 256; + + } else if (max_g <= 16) { + max_accel_g = 16; + setbits |= BMI055_ACCEL_RANGE_16_G; + lsb_per_g = 128; + + } else { + return -EINVAL; + } + + _accel_range_scale = (BMI055_ONE_G / lsb_per_g); + _accel_range_m_s2 = max_accel_g * BMI055_ONE_G; + + modify_reg(BMI055_ACC_RANGE, clearbits, setbits); + + return OK; +} + + +void +BMI055_accel::start() +{ + /* make sure we are stopped first */ + stop(); + + /* discard any stale data in the buffers */ + _accel_reports->flush(); + + /* start polling at the specified rate */ + hrt_call_every(&_call, + 1000, + _call_interval - BMI055_TIMER_REDUCTION, + (hrt_callout)&BMI055_accel::measure_trampoline, this); + reset(); +} + +void +BMI055_accel::stop() +{ + hrt_cancel(&_call); +} + +void +BMI055_accel::measure_trampoline(void *arg) +{ + BMI055_accel *dev = reinterpret_cast(arg); + + /* make another measurement */ + dev->measure(); +} + +void +BMI055_accel::check_registers(void) +{ + uint8_t v; + + if ((v = read_reg(_checked_registers[_checked_next])) != + _checked_values[_checked_next]) { + _checked_bad[_checked_next] = v; + + /* + if we get the wrong value then we know the SPI bus + or sensor is very sick. We set _register_wait to 20 + and wait until we have seen 20 good values in a row + before we consider the sensor to be OK again. + */ + perf_count(_bad_registers); + + /* + try to fix the bad register value. We only try to + fix one per loop to prevent a bad sensor hogging the + bus. + */ + if (_register_wait == 0 || _checked_next == 0) { + // if the product_id is wrong then reset the + // sensor completely + write_reg(BMI055_ACC_SOFTRESET, BMI055_SOFT_RESET); + _reset_wait = hrt_absolute_time() + 10000; + _checked_next = 0; + + } else { + write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); + // waiting 3ms between register writes seems + // to raise the chance of the sensor + // recovering considerably + _reset_wait = hrt_absolute_time() + 3000; + } + + _register_wait = 20; + } + + _checked_next = (_checked_next + 1) % BMI055_ACCEL_NUM_CHECKED_REGISTERS; +} + + +void +BMI055_accel::measure() +{ + uint8_t index = 0, accel_data[7]; + uint16_t lsb, msb, msblsb; + uint8_t status_x, status_y, status_z; + + if (hrt_absolute_time() < _reset_wait) { + // we're waiting for a reset to complete + return; + } + + struct Report { + int16_t accel_x; + int16_t accel_y; + int16_t accel_z; + int16_t temp; + } report; + + /* start measuring */ + perf_begin(_sample_perf); + + /* + * Fetch the full set of measurements from the BMI055 in one pass. + */ + accel_data[index] = BMI055_ACC_X_L | DIR_READ; + + if (OK != transfer(accel_data, accel_data, sizeof(accel_data))) { + return; + } + + check_registers(); + + /* Extracting accel data from the read data */ + index = 1; + lsb = (uint16_t)accel_data[index++]; + status_x = (lsb & BMI055_NEW_DATA_MASK); + msb = (uint16_t)accel_data[index++]; + msblsb = (msb << 8) | lsb; + report.accel_x = ((int16_t)msblsb >> 4); /* Data in X axis */ + + lsb = (uint16_t)accel_data[index++]; + status_y = (lsb & BMI055_NEW_DATA_MASK); + msb = (uint16_t)accel_data[index++]; + msblsb = (msb << 8) | lsb; + report.accel_y = ((int16_t)msblsb >> 4); /* Data in Y axis */ + + lsb = (uint16_t)accel_data[index++]; + status_z = (lsb & BMI055_NEW_DATA_MASK); + msb = (uint16_t)accel_data[index++]; + msblsb = (msb << 8) | lsb; + report.accel_z = ((int16_t)msblsb >> 4); /* Data in Z axis */ + + // Checking the status of new data + if ((!status_x) || (!status_y) || (!status_z)) { + perf_end(_sample_perf); + perf_count(_duplicates); + _got_duplicate = true; + return; + } + + + _got_duplicate = false; + + uint8_t temp = read_reg(BMI055_ACC_TEMP); + report.temp = temp; + + if (report.accel_x == 0 && + report.accel_y == 0 && + report.accel_z == 0 && + report.temp == 0) { + // all zero data - probably a SPI bus error + perf_count(_bad_transfers); + perf_end(_sample_perf); + // note that we don't call reset() here as a reset() + // costs 20ms with interrupts disabled. That means if + // the bmi055 accel does go bad it would cause a FMU failure, + // regardless of whether another sensor is available, + return; + } + + + perf_count(_good_transfers); + + if (_register_wait != 0) { + // we are waiting for some good transfers before using + // the sensor again. We still increment + // _good_transfers, but don't return any data yet + _register_wait--; + return; + } + + /* + * Report buffers. + */ + 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 + // whether it has had failures + arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); + + /* + * 1) Scale raw value to SI units using scaling from datasheet. + * 2) Subtract static offset (in SI units) + * 3) Scale the statically calibrated values with a linear + * dynamically obtained factor + * + * Note: the static sensor offset is the number the sensor outputs + * at a nominally 'zero' input. Therefore the offset has to + * be subtracted. + * + */ + + arb.x_raw = report.accel_x; + arb.y_raw = report.accel_y; + arb.z_raw = report.accel_z; + + float xraw_f = report.accel_x; + float yraw_f = report.accel_y; + float zraw_f = report.accel_z; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + + arb.x = _accel_filter_x.apply(x_in_new); + arb.y = _accel_filter_y.apply(y_in_new); + arb.z = _accel_filter_z.apply(z_in_new); + + math::Vector<3> aval(x_in_new, y_in_new, z_in_new); + math::Vector<3> aval_integrated; + + bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt); + arb.x_integral = aval_integrated(0); + arb.y_integral = aval_integrated(1); + arb.z_integral = aval_integrated(2); + + arb.scaling = _accel_range_scale; + arb.range_m_s2 = _accel_range_m_s2; + + _last_temperature = 23 + report.temp * 1.0f / 512.0f; + + arb.temperature_raw = report.temp; + arb.temperature = _last_temperature; + + _accel_reports->force(&arb); + + /* notify anyone waiting for data */ + if (accel_notify) { + poll_notify(POLLIN); + } + + if (accel_notify && !(_pub_blocked)) { + /* log the time of this report */ + perf_begin(_controller_latency_perf); + /* publish it */ + orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); + } + + /* stop measuring */ + perf_end(_sample_perf); +} + + +void +BMI055_accel::print_info() +{ + warnx("BMI055 Accel"); + perf_print_counter(_sample_perf); + perf_print_counter(_accel_reads); + perf_print_counter(_bad_transfers); + perf_print_counter(_bad_registers); + 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); + + for (uint8_t i = 0; i < BMI055_ACCEL_NUM_CHECKED_REGISTERS; i++) { + uint8_t v = read_reg(_checked_registers[i]); + + if (v != _checked_values[i]) { + ::printf("reg %02x:%02x should be %02x\n", + (unsigned)_checked_registers[i], + (unsigned)v, + (unsigned)_checked_values[i]); + } + + if (v != _checked_bad[i]) { + ::printf("reg %02x:%02x was bad %02x\n", + (unsigned)_checked_registers[i], + (unsigned)v, + (unsigned)_checked_bad[i]); + } + } + + ::printf("temperature: %.1f\n", (double)_last_temperature); + printf("\n"); +} + + +void +BMI055_accel::print_registers() +{ + uint8_t index = 0; + printf("BMI055 accel registers\n"); + + uint8_t reg = _checked_registers[index++]; + uint8_t v = read_reg(reg); + printf("Accel Chip Id: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = _checked_registers[index++]; + v = read_reg(reg); + printf("Accel Bw: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = _checked_registers[index++]; + v = read_reg(reg); + printf("Accel Range: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = _checked_registers[index++]; + v = read_reg(reg); + printf("Accel Int-en-1: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = _checked_registers[index++]; + v = read_reg(reg); + printf("Accel Int-Map-1: %02x:%02x ", (unsigned)reg, (unsigned)v); + + printf("\n"); +} + + diff --git a/src/drivers/bmi055/bmi055_gyro.cpp b/src/drivers/bmi055/bmi055_gyro.cpp new file mode 100644 index 0000000000..bd845a46d6 --- /dev/null +++ b/src/drivers/bmi055/bmi055_gyro.cpp @@ -0,0 +1,872 @@ + +#include "bmi055.hpp" + + +/* + list of registers that will be checked in check_registers(). Note + that ADDR_WHO_AM_I must be first in the list. + */ + +const uint8_t BMI055_gyro::_checked_registers[BMI055_GYRO_NUM_CHECKED_REGISTERS] = { BMI055_GYR_CHIP_ID, + BMI055_GYR_LPM1, + BMI055_GYR_BW, + BMI055_GYR_RANGE, + BMI055_GYR_INT_EN_0, + BMI055_GYR_INT_EN_1, + BMI055_GYR_INT_MAP_1 + }; + + +BMI055_gyro::BMI055_gyro(int bus, const char *path_gyro, spi_dev_e device, enum Rotation rotation) : + BMI055("BMI055_GYRO", path_gyro, bus, device, SPIDEV_MODE3, BMI055_BUS_SPEED, rotation), + _gyro_reports(nullptr), + _gyro_scale{}, + _gyro_range_scale(0.0f), + _gyro_range_rad_s(0.0f), + _gyro_topic(nullptr), + _gyro_orb_class_instance(-1), + _gyro_class_instance(-1), + _gyro_sample_rate(BMI055_GYRO_DEFAULT_RATE), + _gyro_reads(perf_alloc(PC_COUNT, "bmi055_gyro_read")), + _gyro_filter_x(BMI055_GYRO_DEFAULT_RATE, BMI055_GYRO_DEFAULT_DRIVER_FILTER_FREQ), + _gyro_filter_y(BMI055_GYRO_DEFAULT_RATE, BMI055_GYRO_DEFAULT_DRIVER_FILTER_FREQ), + _gyro_filter_z(BMI055_GYRO_DEFAULT_RATE, BMI055_GYRO_DEFAULT_DRIVER_FILTER_FREQ), + _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 + _gyro_scale.x_offset = 0; + _gyro_scale.x_scale = 1.0f; + _gyro_scale.y_offset = 0; + _gyro_scale.y_scale = 1.0f; + _gyro_scale.z_offset = 0; + _gyro_scale.z_scale = 1.0f; + + memset(&_call, 0, sizeof(_call)); +} + + +BMI055_gyro::~BMI055_gyro() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_gyro_reports != nullptr) { + delete _gyro_reports; + } + + if (_gyro_class_instance != -1) { + unregister_class_devname(GYRO_BASE_DEVICE_PATH, _gyro_class_instance); + } + + /* delete the perf counter */ + perf_free(_gyro_reads); +} + +int +BMI055_gyro::init() +{ + int ret; + + /* do SPI init (and probe) first */ + ret = SPI::init(); + + /* if probe/setup failed, bail now */ + if (ret != OK) { + DEVICE_DEBUG("SPI setup failed"); + return ret; + } + + /* allocate basic report buffers */ + _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); + + if (_gyro_reports == nullptr) { + goto out; + } + + if (reset() != OK) { + goto out; + } + + /* Initialize offsets and scales */ + _gyro_scale.x_offset = 0; + _gyro_scale.x_scale = 1.0f; + _gyro_scale.y_offset = 0; + _gyro_scale.y_scale = 1.0f; + _gyro_scale.z_offset = 0; + _gyro_scale.z_scale = 1.0f; + + + /* if probe/setup failed, bail now */ + if (ret != OK) { + DEVICE_DEBUG("gyro init failed"); + return ret; + } + + _gyro_class_instance = register_class_devname(GYRO_BASE_DEVICE_PATH); + + measure(); + + /* advertise sensor topic, measure manually to initialize valid report */ + struct gyro_report grp; + _gyro_reports->get(&grp); + + _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, + &_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1); + + if (_gyro_topic == nullptr) { + warnx("ADVERT FAIL"); + } + +out: + return ret; +} + + +int BMI055_gyro::reset() +{ + write_reg(BMI055_GYR_SOFTRESET, BMI055_SOFT_RESET);//Soft-reset + usleep(5000); + write_checked_reg(BMI055_GYR_BW, 0); // Write Gyro Bandwidth + write_checked_reg(BMI055_GYR_RANGE, 0);// Write Gyro range + write_checked_reg(BMI055_GYR_INT_EN_0, BMI055_GYR_DRDY_INT_EN); //Enable DRDY interrupt + write_checked_reg(BMI055_GYR_INT_MAP_1, BMI055_GYR_DRDY_INT1); //Map DRDY interrupt on pin INT1 + + 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); + + uint8_t retries = 10; + + while (retries--) { + bool all_ok = true; + + for (uint8_t i = 0; i < BMI055_GYRO_NUM_CHECKED_REGISTERS; i++) { + if (read_reg(_checked_registers[i]) != _checked_values[i]) { + write_reg(_checked_registers[i], _checked_values[i]); + all_ok = false; + } + } + + if (all_ok) { + break; + } + } + + _gyro_reads = 0; + + return OK; +} + +int +BMI055_gyro::probe() +{ + /* look for device ID */ + _whoami = read_reg(BMI055_GYR_CHIP_ID); + + // verify product revision + switch (_whoami) { + case BMI055_GYR_WHO_AM_I: + memset(_checked_values, 0, sizeof(_checked_values)); + memset(_checked_bad, 0, sizeof(_checked_bad)); + _checked_values[0] = _whoami; + _checked_bad[0] = _whoami; + return OK; + } + + DEVICE_DEBUG("unexpected whoami 0x%02x", _whoami); + return -EIO; +} + + +int +BMI055_gyro::gyro_set_sample_rate(float frequency) +{ + uint8_t setbits = 0; + uint8_t clearbits = BMI055_GYRO_BW_MASK; + + if (frequency <= 100) { + setbits |= BMI055_GYRO_RATE_100; + _gyro_sample_rate = 100; + + } else if (frequency <= 250) { + setbits |= BMI055_GYRO_RATE_400; + _gyro_sample_rate = 400; + + } else if (frequency <= 1000) { + setbits |= BMI055_GYRO_RATE_1000; + _gyro_sample_rate = 1000; + + } else if (frequency > 1000) { + setbits |= BMI055_GYRO_RATE_2000; + _gyro_sample_rate = 2000; + + } else { + return -EINVAL; + } + + modify_reg(BMI055_GYR_BW, clearbits, setbits); + + return OK; +} + + +int +BMI055_gyro::self_test() +{ + if (perf_event_count(_sample_perf) == 0) { + measure(); + } + + /* return 0 on success, 1 else */ + return (perf_event_count(_sample_perf) > 0) ? 0 : 1; +} + +int +BMI055_gyro::gyro_self_test() +{ + if (self_test()) { + return 1; + } + + /* + * Maximum deviation of 10 degrees + */ + const float max_offset = (float)(10 * M_PI_F / 180.0f); + /* 30% scale error is chosen to catch completely faulty units but + * to let some slight scale error pass. Requires a rate table or correlation + * with mag rotations + data fit to + * calibrate properly and is not done by default. + */ + const float max_scale = 0.3f; + + /* evaluate gyro offsets, complain if offset -> zero or larger than 30 dps. */ + if (fabsf(_gyro_scale.x_offset) > max_offset) { + return 1; + } + + /* evaluate gyro scale, complain if off by more than 30% */ + if (fabsf(_gyro_scale.x_scale - 1.0f) > max_scale) { + return 1; + } + + if (fabsf(_gyro_scale.y_offset) > max_offset) { + return 1; + } + + if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale) { + return 1; + } + + if (fabsf(_gyro_scale.z_offset) > max_offset) { + return 1; + } + + if (fabsf(_gyro_scale.z_scale - 1.0f) > max_scale) { + return 1; + } + + /* check if all scales are zero */ + if ((fabsf(_gyro_scale.x_offset) < 0.000001f) && + (fabsf(_gyro_scale.y_offset) < 0.000001f) && + (fabsf(_gyro_scale.z_offset) < 0.000001f)) { + /* if all are zero, this device is not calibrated */ + return 1; + } + + return 0; +} + +/* + deliberately trigger an error in the sensor to trigger recovery + */ +void +BMI055_gyro::test_error() +{ + write_reg(BMI055_GYR_SOFTRESET, BMI055_SOFT_RESET); + ::printf("error triggered\n"); + print_registers(); +} + +ssize_t +BMI055_gyro::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(gyro_report); + + /* buffer must be large enough */ + if (count < 1) { + return -ENOSPC; + } + + /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ + if (_call_interval == 0) { + _gyro_reports->flush(); + measure(); + } + + /* if no data, error (we could block here) */ + if (_gyro_reports->empty()) { + return -EAGAIN; + } + + perf_count(_gyro_reads); + + /* copy reports out of our buffer to the caller */ + gyro_report *grp = reinterpret_cast(buffer); + int transferred = 0; + + while (count--) { + if (!_gyro_reports->get(grp)) { + break; + } + + transferred++; + grp++; + } + + /* return the number of bytes transferred */ + return (transferred * sizeof(gyro_report)); +} + + +int +BMI055_gyro::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _call_interval = 0; + return OK; + + /* external signalling not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + return ioctl(filp, SENSORIOCSPOLLRATE, BMI055_GYRO_MAX_RATE); + + case SENSOR_POLLRATE_DEFAULT: + return ioctl(filp, SENSORIOCSPOLLRATE, BMI055_GYRO_DEFAULT_RATE); + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_call_interval == 0); + + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / arg; + + /* check against maximum rate */ + if (ticks < 1000) { + return -EINVAL; + } + + 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); + _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); + _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); + + /* update interval for next measurement */ + _call_interval = ticks; + + /* + set call interval faster than the sample time. We + then detect when we have duplicate samples and reject + them. This prevents aliasing due to a beat between the + stm32 clock and the bmi055 clock + */ + _call.period = _call_interval - BMI055_TIMER_REDUCTION; + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_call_interval == 0) { + return SENSOR_POLLRATE_MANUAL; + } + + 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)) { + return -EINVAL; + } + + irqstate_t flags = px4_enter_critical_section(); + + if (!_gyro_reports->resize(arg)) { + px4_leave_critical_section(flags); + return -ENOMEM; + } + + px4_leave_critical_section(flags); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _gyro_reports->size(); + + case GYROIOCGSAMPLERATE: + return _gyro_sample_rate; + + case GYROIOCSSAMPLERATE: + return gyro_set_sample_rate(arg); + + case GYROIOCGLOWPASS: + return _gyro_filter_x.get_cutoff_freq(); + + case GYROIOCSLOWPASS: + // set software filtering + _gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); + _gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); + _gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); + return OK; + + case GYROIOCSSCALE: + /* copy scale in */ + memcpy(&_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_gyro_scale)); + return OK; + + case GYROIOCGSCALE: + /* copy scale out */ + memcpy((struct gyro_calibration_s *) arg, &_gyro_scale, sizeof(_gyro_scale)); + return OK; + + case GYROIOCSRANGE: + return set_gyro_range(arg); + + case GYROIOCGRANGE: + return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f); + + case GYROIOCSELFTEST: + return gyro_self_test(); + +#ifdef GYROIOCSHWLOWPASS + + case GYROIOCSHWLOWPASS: + return OK; +#endif + +#ifdef GYROIOCGHWLOWPASS + + case GYROIOCGHWLOWPASS: + return _dlpf_freq; +#endif + + default: + /* give it to the superclass */ + return SPI::ioctl(filp, cmd, arg); + } +} + + + +void +BMI055_gyro::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) +{ + uint8_t val; + + val = read_reg(reg); + val &= ~clearbits; + val |= setbits; + write_checked_reg(reg, val); +} + +void +BMI055_gyro::write_checked_reg(unsigned reg, uint8_t value) +{ + write_reg(reg, value); + + for (uint8_t i = 0; i < BMI055_GYRO_NUM_CHECKED_REGISTERS; i++) { + if (reg == _checked_registers[i]) { + _checked_values[i] = value; + _checked_bad[i] = value; + } + } +} + + +int +BMI055_gyro::set_gyro_range(unsigned max_dps) +{ + uint8_t setbits = 0; + uint8_t clearbits = BMI055_GYRO_RANGE_125_DPS | BMI055_GYRO_RANGE_250_DPS; + float lsb_per_dps; + float max_gyro_dps; + + if (max_dps == 0) { + max_dps = 2000; + } + + if (max_dps <= 125) { + max_gyro_dps = 125; + lsb_per_dps = 262.4; + setbits |= BMI055_GYRO_RANGE_125_DPS; + + } else if (max_dps <= 250) { + max_gyro_dps = 250; + lsb_per_dps = 131.2; + setbits |= BMI055_GYRO_RANGE_250_DPS; + + } else if (max_dps <= 500) { + max_gyro_dps = 500; + lsb_per_dps = 65.6; + setbits |= BMI055_GYRO_RANGE_500_DPS; + + } else if (max_dps <= 1000) { + max_gyro_dps = 1000; + lsb_per_dps = 32.8; + setbits |= BMI055_GYRO_RANGE_1000_DPS; + + } else if (max_dps <= 2000) { + max_gyro_dps = 2000; + lsb_per_dps = 16.4; + setbits |= BMI055_GYRO_RANGE_2000_DPS; + + } else { + return -EINVAL; + } + + _gyro_range_rad_s = (max_gyro_dps / 180.0f * M_PI_F); + _gyro_range_scale = (M_PI_F / (180.0f * lsb_per_dps)); + + modify_reg(BMI055_GYR_RANGE, clearbits, setbits); + + return OK; +} + +void +BMI055_gyro::start() +{ + /* make sure we are stopped first */ + stop(); + + /* discard any stale data in the buffers */ + _gyro_reports->flush(); + + /* start polling at the specified rate */ + hrt_call_every(&_call, + 1000, + _call_interval - BMI055_TIMER_REDUCTION, + (hrt_callout)&BMI055_gyro::measure_trampoline, this); + reset(); +} + +void +BMI055_gyro::stop() +{ + hrt_cancel(&_call); +} + +void +BMI055_gyro::measure_trampoline(void *arg) +{ + BMI055_gyro *dev = reinterpret_cast(arg); + + /* make another measurement */ + dev->measure(); +} + +void +BMI055_gyro::check_registers(void) +{ + uint8_t v; + + if ((v = read_reg(_checked_registers[_checked_next])) != + _checked_values[_checked_next]) { + _checked_bad[_checked_next] = v; + + /* + if we get the wrong value then we know the SPI bus + or sensor is very sick. We set _register_wait to 20 + and wait until we have seen 20 good values in a row + before we consider the sensor to be OK again. + */ + perf_count(_bad_registers); + + /* + try to fix the bad register value. We only try to + fix one per loop to prevent a bad sensor hogging the + bus. + */ + if (_register_wait == 0 || _checked_next == 0) { + // if the product_id is wrong then reset the + // sensor completely + write_reg(BMI055_GYR_SOFTRESET, BMI055_SOFT_RESET); + _reset_wait = hrt_absolute_time() + 10000; + _checked_next = 0; + + } else { + write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); + // waiting 3ms between register writes seems + // to raise the chance of the sensor + // recovering considerably + _reset_wait = hrt_absolute_time() + 3000; + } + + _register_wait = 20; + } + + _checked_next = (_checked_next + 1) % BMI055_GYRO_NUM_CHECKED_REGISTERS; +} + + +void +BMI055_gyro::measure() +{ + if (hrt_absolute_time() < _reset_wait) { + // we're waiting for a reset to complete + return; + } + + struct BMI_GyroReport bmi_gyroreport; + + struct Report { + int16_t temp; + int16_t gyro_x; + int16_t gyro_y; + int16_t gyro_z; + } report; + + /* start measuring */ + perf_begin(_sample_perf); + + /* + * Fetch the full set of measurements from the BMI055 gyro in one pass. + */ + bmi_gyroreport.cmd = BMI055_GYR_X_L | DIR_READ; + + + if (OK != transfer((uint8_t *)&bmi_gyroreport, ((uint8_t *)&bmi_gyroreport), sizeof(bmi_gyroreport))) { + return; + } + + check_registers(); + + uint8_t temp = read_reg(BMI055_ACC_TEMP); + + report.temp = temp; + + report.gyro_x = bmi_gyroreport.gyro_x; + report.gyro_y = bmi_gyroreport.gyro_y; + report.gyro_z = bmi_gyroreport.gyro_z; + + if (report.temp == 0 && + report.gyro_x == 0 && + report.gyro_y == 0 && + report.gyro_z == 0) { + // all zero data - probably a SPI bus error + perf_count(_bad_transfers); + perf_end(_sample_perf); + // note that we don't call reset() here as a reset() + // costs 20ms with interrupts disabled. That means if + // the bmi055 does go bad it would cause a FMU failure, + // regardless of whether another sensor is available, + return; + } + + perf_count(_good_transfers); + + if (_register_wait != 0) { + // we are waiting for some good transfers before using + // the sensor again. We still increment + // _good_transfers, but don't return any data yet + _register_wait--; + return; + } + + /* + * Report buffers. + */ + gyro_report grb; + + + 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 + // level code to decide if it should use this sensor based on + // whether it has had failures + grb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); + + /* + * 1) Scale raw value to SI units using scaling from datasheet. + * 2) Subtract static offset (in SI units) + * 3) Scale the statically calibrated values with a linear + * dynamically obtained factor + * + * Note: the static sensor offset is the number the sensor outputs + * at a nominally 'zero' input. Therefore the offset has to + * be subtracted. + * + * Example: A gyro outputs a value of 74 at zero angular rate + * the offset is 74 from the origin and subtracting + * 74 from all measurements centers them around zero. + */ + + grb.x_raw = report.gyro_x; + grb.y_raw = report.gyro_y; + grb.z_raw = report.gyro_z; + + float xraw_f = report.gyro_x; + float yraw_f = report.gyro_y; + float zraw_f = report.gyro_z; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; + float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; + float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; + + grb.x = _gyro_filter_x.apply(x_gyro_in_new); + grb.y = _gyro_filter_y.apply(y_gyro_in_new); + grb.z = _gyro_filter_z.apply(z_gyro_in_new); + + math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new); + math::Vector<3> gval_integrated; + + bool gyro_notify = _gyro_int.put(grb.timestamp, gval, gval_integrated, grb.integral_dt); + grb.x_integral = gval_integrated(0); + grb.y_integral = gval_integrated(1); + grb.z_integral = gval_integrated(2); + + grb.scaling = _gyro_range_scale; + grb.range_rad_s = _gyro_range_rad_s; + + grb.temperature_raw = report.temp; + grb.temperature = _last_temperature; + + _gyro_reports->force(&grb); + + /* notify anyone waiting for data */ + if (gyro_notify) { + poll_notify(POLLIN); + } + + if (gyro_notify && !(_pub_blocked)) { + /* log the time of this report */ + perf_begin(_controller_latency_perf); + /* publish it */ + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb); + } + + /* stop measuring */ + perf_end(_sample_perf); +} + +void +BMI055_gyro::print_info() +{ + warnx("BMI055 Gyro"); + perf_print_counter(_sample_perf); + perf_print_counter(_gyro_reads); + perf_print_counter(_bad_transfers); + perf_print_counter(_bad_registers); + 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); + + for (uint8_t i = 0; i < BMI055_GYRO_NUM_CHECKED_REGISTERS; i++) { + uint8_t v = read_reg(_checked_registers[i]); + + if (v != _checked_values[i]) { + ::printf("reg %02x:%02x should be %02x\n", + (unsigned)_checked_registers[i], + (unsigned)v, + (unsigned)_checked_values[i]); + } + + if (v != _checked_bad[i]) { + ::printf("reg %02x:%02x was bad %02x\n", + (unsigned)_checked_registers[i], + (unsigned)v, + (unsigned)_checked_bad[i]); + } + } + + ::printf("temperature: %.1f\n", (double)_last_temperature); + printf("\n"); +} + + +void +BMI055_gyro::print_registers() +{ + uint8_t index = 0; + printf("BMI055 gyro registers\n"); + + uint8_t reg = _checked_registers[index++]; + uint8_t v = read_reg(reg); + printf("Gyro Chip Id: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = _checked_registers[index++]; + v = read_reg(reg); + printf("Gyro Power: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = _checked_registers[index++]; + v = read_reg(reg); + printf("Gyro Bw: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = _checked_registers[index++]; + v = read_reg(reg); + printf("Gyro Range: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = _checked_registers[index++]; + v = read_reg(reg); + printf("Gyro Int-en-0: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = _checked_registers[index++]; + v = read_reg(reg); + printf("Gyro Int-en-1: %02x:%02x ", (unsigned)reg, (unsigned)v); + printf("\n"); + + reg = _checked_registers[index++]; + v = read_reg(reg); + printf("Gyro Int-Map-1: %02x:%02x ", (unsigned)reg, (unsigned)v); + + printf("\n"); +} + + + + diff --git a/src/drivers/bmi055/bmi055_main.cpp b/src/drivers/bmi055/bmi055_main.cpp new file mode 100644 index 0000000000..deec79355a --- /dev/null +++ b/src/drivers/bmi055/bmi055_main.cpp @@ -0,0 +1,611 @@ +#include "bmi055.hpp" + +/** driver 'main' command */ +extern "C" { __EXPORT int bmi055_main(int argc, char *argv[]); } + +/** + * Local functions in support of the shell command. + */ + +enum sensor_type { + BMI055_NONE = 0, + BMI055_ACCEL = 1, + BMI055_GYRO +}; + + +namespace bmi055 +{ + +BMI055_accel *g_acc_dev_int; // on internal bus (accel) +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); +void reset(bool, enum sensor_type); +void info(bool, enum sensor_type); +void regdump(bool, enum sensor_type); +void testerror(bool, enum sensor_type); +void usage(); + + +/** + * Start the driver. + * + * This function only returns if the driver is up and running + * or failed to detect the sensor. + */ +void +start(bool external_bus, enum Rotation rotation, enum sensor_type sensor) +{ + + int fd_acc, fd_gyr; + BMI055_accel **g_dev_acc_ptr = external_bus ? &g_acc_dev_ext : &g_acc_dev_int; + const char *path_accel = external_bus ? BMI055_DEVICE_PATH_ACCEL_EXT : BMI055_DEVICE_PATH_ACCEL; + 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 */ + { + errx(0, "bmi055 accel sensor already started"); + } + + /* create the driver */ + if (external_bus) { +#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_BMI) + *g_dev_acc_ptr = new BMI055_accel(PX4_SPI_BUS_EXT, path_accel, (spi_dev_e)PX4_SPIDEV_EXT_BMI, rotation); +#else + errx(0, "External SPI not available"); +#endif + + } else { + *g_dev_acc_ptr = new BMI055_accel(PX4_SPI_BUS_SENSORS, path_accel, (spi_dev_e)PX4_SPIDEV_BMI055_ACC, rotation); + } + + if (*g_dev_acc_ptr == nullptr) { + goto fail_accel; + } + + if (OK != (*g_dev_acc_ptr)->init()) { + goto fail_accel; + } + + /* set the poll rate to default, starts automatic data collection */ + fd_acc = open(path_accel, O_RDONLY); + + if (fd_acc < 0) { + goto fail_accel; + } + + if (ioctl(fd_acc, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + goto fail_accel; + } + + close(fd_acc); + } + + if (sensor == BMI055_GYRO) { + + if (*g_dev_gyr_ptr != nullptr) { + errx(0, "bmi055 gyro sensor already started"); + } + + /* create the driver */ + if (external_bus) { +#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_BMI) + *g_dev_ptr = new BMI055_gyro(PX4_SPI_BUS_EXT, path_gyro, (spi_dev_e)PX4_SPIDEV_EXT_BMI, rotation); +#else + errx(0, "External SPI not available"); +#endif + + } else { + *g_dev_gyr_ptr = new BMI055_gyro(PX4_SPI_BUS_SENSORS, path_gyro, (spi_dev_e)PX4_SPIDEV_BMI055_GYR, rotation); + } + + if (*g_dev_gyr_ptr == nullptr) { + goto fail_gyro; + } + + if (OK != (*g_dev_gyr_ptr)->init()) { + goto fail_gyro; + } + + /* set the poll rate to default, starts automatic data collection */ + fd_gyr = open(path_gyro, O_RDONLY); + + if (fd_gyr < 0) { + goto fail_gyro; + } + + if (ioctl(fd_gyr, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + goto fail_gyro; + } + + close(fd_gyr); + } + + exit(0); + +fail_accel: + + if (*g_dev_acc_ptr != nullptr) { + delete(*g_dev_acc_ptr); + *g_dev_acc_ptr = nullptr; + } + + errx(1, "bmi055 accel driver start failed"); + +fail_gyro: + + if (*g_dev_gyr_ptr != nullptr) { + delete(*g_dev_gyr_ptr); + *g_dev_gyr_ptr = nullptr; + } + + errx(1, "bmi055 gyro driver start failed"); + +} + +void +stop(bool external_bus, enum sensor_type sensor) +{ + BMI055_accel **g_dev_acc_ptr = external_bus ? &g_acc_dev_ext : &g_acc_dev_int; + BMI055_gyro **g_dev_gyr_ptr = external_bus ? &g_gyr_dev_ext : &g_gyr_dev_int; + + if (sensor == BMI055_ACCEL) { + if (*g_dev_acc_ptr != nullptr) { + delete *g_dev_acc_ptr; + *g_dev_acc_ptr = nullptr; + + } else { + /* warn, but not an error */ + warnx("bmi055 accel sensor already stopped."); + } + } + + if (sensor == BMI055_GYRO) { + if (*g_dev_gyr_ptr != nullptr) { + delete *g_dev_gyr_ptr; + *g_dev_gyr_ptr = nullptr; + + } else { + /* warn, but not an error */ + warnx("bmi055 gyro sensor already stopped."); + } + } + + exit(0); + +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test(bool external_bus, enum sensor_type sensor) +{ + const char *path_accel = external_bus ? BMI055_DEVICE_PATH_ACCEL_EXT : BMI055_DEVICE_PATH_ACCEL; + const char *path_gyro = external_bus ? BMI055_DEVICE_PATH_GYRO_EXT : BMI055_DEVICE_PATH_GYRO; + accel_report a_report; + gyro_report g_report; + ssize_t sz; + + if (sensor == BMI055_ACCEL) { + /* get the accel driver */ + int fd_acc = open(path_accel, O_RDONLY); + + if (fd_acc < 0) + err(1, "%s Accel file open failed (try 'bmi055 -A start')", + path_accel); + + + /* reset to manual polling */ + if (ioctl(fd_acc, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) { + err(1, "accel reset to manual polling"); + } + + /* do a simple demand read */ + sz = read(fd_acc, &a_report, sizeof(a_report)); + + if (sz != sizeof(a_report)) { + warnx("ret: %d, expected: %d", sz, sizeof(a_report)); + err(1, "immediate accel read failed"); + } + + warnx("single read"); + warnx("time: %lld", a_report.timestamp); + warnx("acc x: \t%8.4f\tm/s^2", (double)a_report.x); + warnx("acc y: \t%8.4f\tm/s^2", (double)a_report.y); + warnx("acc z: \t%8.4f\tm/s^2", (double)a_report.z); + warnx("acc x: \t%d\traw 0x%0x", (short)a_report.x_raw, (unsigned short)a_report.x_raw); + warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw); + warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw); + warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2, + (double)(a_report.range_m_s2 / BMI055_ONE_G)); + warnx("temp: \t%8.4f\tdeg celsius", (double)a_report.temperature); + warnx("temp: \t%d\traw 0x%0x", (short)g_report.temperature_raw, (unsigned short)a_report.temperature_raw); + + + /* reset to default polling */ + if (ioctl(fd_acc, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + err(1, "accel reset to default polling"); + } + + close(fd_acc); + } + + if (sensor == BMI055_GYRO) { + + /* get the gyro driver */ + int fd_gyr = open(path_gyro, O_RDONLY); + + if (fd_gyr < 0) { + err(1, "%s Gyro file open failed (try 'bmi055 -G start')", path_gyro); + } + + /* reset to manual polling */ + if (ioctl(fd_gyr, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) { + err(1, "gyro reset to manual polling"); + } + + + /* do a simple demand read */ + sz = read(fd_gyr, &g_report, sizeof(g_report)); + + if (sz != sizeof(g_report)) { + warnx("ret: %d, expected: %d", sz, sizeof(g_report)); + err(1, "immediate gyro read failed"); + } + + warnx("gyr x: \t% 9.5f\trad/s", (double)g_report.x); + warnx("gyr y: \t% 9.5f\trad/s", (double)g_report.y); + warnx("gyr z: \t% 9.5f\trad/s", (double)g_report.z); + warnx("gyr x: \t%d\traw", (int)g_report.x_raw); + warnx("gyr y: \t%d\traw", (int)g_report.y_raw); + warnx("gyr z: \t%d\traw", (int)g_report.z_raw); + warnx("gyr range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, + (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); + + + /* reset to default polling */ + if (ioctl(fd_gyr, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + err(1, "gyro reset to default polling"); + } + + close(fd_gyr); + + } + + if ((sensor == BMI055_ACCEL) || (sensor == BMI055_GYRO)) { + /* XXX add poll-rate tests here too */ + reset(external_bus, sensor); + } + + errx(0, "PASS"); + +} + +/** + * Reset the driver. + */ +void +reset(bool external_bus, enum sensor_type sensor) +{ + const char *path_accel = external_bus ? BMI055_DEVICE_PATH_ACCEL_EXT : BMI055_DEVICE_PATH_ACCEL; + const char *path_gyro = external_bus ? BMI055_DEVICE_PATH_GYRO_EXT : BMI055_DEVICE_PATH_GYRO; + + if (sensor == BMI055_ACCEL) { + int fd_acc = open(path_accel, O_RDONLY); + + if (fd_acc < 0) { + err(1, "Opening accel file failed "); + } + + if (ioctl(fd_acc, SENSORIOCRESET, 0) < 0) { + err(1, "accel driver reset failed"); + } + + + if (ioctl(fd_acc, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + err(1, "accel driver poll restart failed"); + } + + close(fd_acc); + } + + if (sensor == BMI055_GYRO) { + int fd_gyr = open(path_gyro, O_RDONLY); + + if (fd_gyr < 0) { + err(1, "Opening gyro file failed "); + } + + if (ioctl(fd_gyr, SENSORIOCRESET, 0) < 0) { + err(1, "gyro driver reset failed"); + } + + if (ioctl(fd_gyr, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + err(1, "gyro driver poll restart failed"); + } + + close(fd_gyr); + } + + exit(0); +} + + +/** + * Print a little info about the driver. + */ +void +info(bool external_bus, enum sensor_type sensor) +{ + BMI055_accel **g_dev_acc_ptr = external_bus ? &g_acc_dev_ext : &g_acc_dev_int; + BMI055_gyro **g_dev_gyr_ptr = external_bus ? &g_gyr_dev_ext : &g_gyr_dev_int; + + if (sensor == BMI055_ACCEL) { + if (*g_dev_acc_ptr == nullptr) { + errx(1, "bmi055 accel driver not running"); + } + + printf("state @ %p\n", *g_dev_acc_ptr); + (*g_dev_acc_ptr)->print_info(); + } + + if (sensor == BMI055_GYRO) { + if (*g_dev_gyr_ptr == nullptr) { + errx(1, "bmi055 gyro driver not running"); + } + + printf("state @ %p\n", *g_dev_gyr_ptr); + (*g_dev_gyr_ptr)->print_info(); + } + + exit(0); +} + +/** + * Dump the register information + */ +void +regdump(bool external_bus, enum sensor_type sensor) +{ + BMI055_accel **g_dev_acc_ptr = external_bus ? &g_acc_dev_ext : &g_acc_dev_int; + BMI055_gyro **g_dev_gyr_ptr = external_bus ? &g_gyr_dev_ext : &g_gyr_dev_int; + + if (sensor == BMI055_ACCEL) { + if (*g_dev_acc_ptr == nullptr) { + errx(1, "bmi055 accel driver not running"); + } + + printf("regdump @ %p\n", *g_dev_acc_ptr); + (*g_dev_acc_ptr)->print_registers(); + } + + if (sensor == BMI055_GYRO) { + if (*g_dev_gyr_ptr == nullptr) { + errx(1, "bmi055 gyro driver not running"); + } + + printf("regdump @ %p\n", *g_dev_gyr_ptr); + (*g_dev_gyr_ptr)->print_registers(); + } + + exit(0); +} + + +/** + * deliberately produce an error to test recovery + */ +void +testerror(bool external_bus, enum sensor_type sensor) +{ + BMI055_accel **g_dev_acc_ptr = external_bus ? &g_acc_dev_ext : &g_acc_dev_int; + BMI055_gyro **g_dev_gyr_ptr = external_bus ? &g_gyr_dev_ext : &g_gyr_dev_int; + + if (sensor == BMI055_ACCEL) { + if (*g_dev_acc_ptr == nullptr) { + errx(1, "bmi055 accel driver not running"); + } + + (*g_dev_acc_ptr)->test_error(); + } + + if (sensor == BMI055_GYRO) { + if (*g_dev_gyr_ptr == nullptr) { + errx(1, "bmi055 gyro driver not running"); + } + + (*g_dev_gyr_ptr)->test_error(); + } + + exit(0); +} + +void +usage() +{ + warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump', 'testerror'"); + warnx("options:"); + warnx(" -X (external bus)"); + warnx(" -R rotation"); + warnx(" -A (Enable Accelerometer)"); + warnx(" -G (Enable Gyroscope)"); + +} + +}//namespace ends + + +BMI055::BMI055(const char *name, const char *devname, int bus, enum spi_dev_e device, enum spi_mode_e mode, + uint32_t frequency, enum Rotation rotation): + SPI(name, devname, bus, device, mode, frequency), + _whoami(0), + _call{}, + _call_interval(0), + _dlpf_freq(0), + _sample_perf(perf_alloc(PC_ELAPSED, "bmi055_read")), + _bad_transfers(perf_alloc(PC_COUNT, "bmi055_bad_transfers")), + _bad_registers(perf_alloc(PC_COUNT, "bmi055_bad_registers")), + _good_transfers(perf_alloc(PC_COUNT, "bmi055_good_transfers")), + _reset_retries(perf_alloc(PC_COUNT, "bmi055_reset_retries")), + _duplicates(perf_alloc(PC_COUNT, "bmi055_duplicates")), + _controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")), + _register_wait(0), + _reset_wait(0), + _rotation(rotation), + _checked_next(0) +{ + +} + + + +BMI055::~BMI055() +{ + /* delete the perf counter */ + perf_free(_sample_perf); + perf_free(_bad_transfers); + perf_free(_bad_registers); + perf_free(_good_transfers); + perf_free(_reset_retries); + perf_free(_duplicates); + +} + +uint8_t +BMI055::read_reg(unsigned reg) +{ + uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0}; + + transfer(cmd, cmd, sizeof(cmd)); + + return cmd[1]; + +} + +uint16_t +BMI055::read_reg16(unsigned reg) +{ + uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 }; + + transfer(cmd, cmd, sizeof(cmd)); + + return (uint16_t)(cmd[1] << 8) | cmd[2]; +} + + +void +BMI055::write_reg(unsigned reg, uint8_t value) +{ + uint8_t cmd[2]; + + cmd[0] = reg | DIR_WRITE; + cmd[1] = value; + + transfer(cmd, nullptr, sizeof(cmd)); +} + + + +int +bmi055_main(int argc, char *argv[]) +{ + bool external_bus = false; + int ch; + enum Rotation rotation = ROTATION_NONE; + enum sensor_type sensor = BMI055_NONE; + + /* jump over start/off/etc and look at options first */ + while ((ch = getopt(argc, argv, "XR:AG")) != EOF) { + switch (ch) { + case 'X': + external_bus = true; + break; + + case 'R': + rotation = (enum Rotation)atoi(optarg); + break; + + case 'A': + sensor = BMI055_ACCEL; + break; + + case 'G': + sensor = BMI055_GYRO; + break; + + default: + bmi055::usage(); + exit(0); + } + } + + const char *verb = argv[optind]; + + if (sensor == BMI055_NONE) { + bmi055::usage(); + exit(0); + } + + /* + * Start/load the driver. + */ + if (!strcmp(verb, "start")) { + bmi055::start(external_bus, rotation, sensor); + } + + /* + * Stop the driver. + */ + if (!strcmp(verb, "stop")) { + bmi055::stop(external_bus, sensor); + } + + /* + * Test the driver/device. + */ + if (!strcmp(verb, "test")) { + bmi055::test(external_bus, sensor); + } + + /* + * Reset the driver. + */ + if (!strcmp(verb, "reset")) { + bmi055::reset(external_bus, sensor); + } + + /* + * Print driver information. + */ + if (!strcmp(verb, "info")) { + bmi055::info(external_bus, sensor); + } + + /* + * Print register information. + */ + if (!strcmp(verb, "regdump")) { + bmi055::regdump(external_bus, sensor); + } + + if (!strcmp(verb, "testerror")) { + bmi055::testerror(external_bus, sensor); + } + + bmi055::usage(); + exit(1); +} + diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index 6d6e99b91b..64ce4e98b3 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -86,6 +86,10 @@ #define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) +#define GPIO_SPI_CS_BMI055_ACC (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) +#define GPIO_SPI_CS_BMI055_GYR (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN15) + + /* Define the Ready interrupts */ #define GPIO_DRDY_MPU9250 (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15) @@ -105,9 +109,10 @@ #define GPIO_SPI_CS_OFF_MS5611 _PIN_OFF(GPIO_SPI_CS_MS5611) #define GPIO_SPI_CS_OFF_ICM_2060X _PIN_OFF(GPIO_SPI_CS_ICM_2060X) #define GPIO_SPI_CS_OFF_BMI160 _PIN_OFF(GPIO_SPI_CS_BMI160) +#define GPIO_SPI_CS_OFF_BMI055_ACC _PIN_OFF(GPIO_SPI_CS_BMI055_ACC) +#define GPIO_SPI_CS_OFF_BMI055_GYR _PIN_OFF(GPIO_SPI_CS_BMI055_GYR) #define GPIO_DRDY_OFF_MPU9250 _PIN_OFF(GPIO_DRDY_MPU9250) -#define GPIO_DRDY_OFF_HMC5983 _PIN_OFF(GPIO_DRDY_HMC5983) #define GPIO_DRDY_OFF_ICM_2060X _PIN_OFF(GPIO_DRDY_ICM_2060X) /* SPI1 off */ @@ -130,6 +135,8 @@ #define PX4_SPIDEV_BMA 9 #define PX4_SPIDEV_ICM_20608 10 #define PX4_SPIDEV_ICM_20602 11 +#define PX4_SPIDEV_BMI055_ACC 12 +#define PX4_SPIDEV_BMI055_GYR 13 /* onboard MS5611 and FRAM are both on bus SPI2 * spi_dev_e:SPIDEV_FLASH has the value 2 and is used in the NuttX ramtron driver diff --git a/src/drivers/boards/px4fmu-v4/px4fmu_spi.c b/src/drivers/boards/px4fmu-v4/px4fmu_spi.c index 677a2a42e1..0f96b80cd3 100644 --- a/src/drivers/boards/px4fmu-v4/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v4/px4fmu_spi.c @@ -77,6 +77,20 @@ __EXPORT void stm32_spiinitialize(void) px4_arch_configgpio(GPIO_SPI_CS_MS5611); px4_arch_configgpio(GPIO_SPI_CS_ICM_2060X); px4_arch_configgpio(GPIO_SPI_CS_BMI160); + px4_arch_configgpio(GPIO_SPI_CS_BMI055_ACC); + px4_arch_configgpio(GPIO_SPI_CS_BMI055_GYR); + + /* De-activate all peripherals, + * required for some peripheral + * state machines + */ + px4_arch_gpiowrite(GPIO_SPI_CS_MPU9250, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_HMC5983, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_MS5611, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_ICM_2060X, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_BMI160, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_BMI055_ACC, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_BMI055_GYR, 1); px4_arch_configgpio(GPIO_DRDY_MPU9250); px4_arch_configgpio(GPIO_DRDY_HMC5983); @@ -103,6 +117,8 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, case PX4_SPIDEV_ICM_20608: /* Making sure the other peripherals are not selected */ px4_arch_gpiowrite(GPIO_SPI_CS_BMI160, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_BMI055_ACC, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_BMI055_GYR, 1); px4_arch_gpiowrite(GPIO_SPI_CS_MPU9250, 1); px4_arch_gpiowrite(GPIO_SPI_CS_HMC5983, 1); px4_arch_gpiowrite(GPIO_SPI_CS_MS5611, 1); @@ -116,6 +132,8 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, case PX4_SPIDEV_BARO: /* Making sure the other peripherals are not selected */ px4_arch_gpiowrite(GPIO_SPI_CS_BMI160, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_BMI055_ACC, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_BMI055_GYR, 1); px4_arch_gpiowrite(GPIO_SPI_CS_MPU9250, 1); px4_arch_gpiowrite(GPIO_SPI_CS_HMC5983, 1); px4_arch_gpiowrite(GPIO_SPI_CS_MS5611, !selected); @@ -125,6 +143,8 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, case PX4_SPIDEV_HMC: /* Making sure the other peripherals are not selected */ px4_arch_gpiowrite(GPIO_SPI_CS_BMI160, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_BMI055_ACC, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_BMI055_GYR, 1); px4_arch_gpiowrite(GPIO_SPI_CS_MPU9250, 1); px4_arch_gpiowrite(GPIO_SPI_CS_HMC5983, !selected); px4_arch_gpiowrite(GPIO_SPI_CS_MS5611, 1); @@ -134,6 +154,8 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, case PX4_SPIDEV_MPU: /* Making sure the other peripherals are not selected */ px4_arch_gpiowrite(GPIO_SPI_CS_BMI160, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_BMI055_ACC, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_BMI055_GYR, 1); px4_arch_gpiowrite(GPIO_SPI_CS_MPU9250, !selected); px4_arch_gpiowrite(GPIO_SPI_CS_HMC5983, 1); px4_arch_gpiowrite(GPIO_SPI_CS_MS5611, 1); @@ -146,9 +168,31 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, px4_arch_gpiowrite(GPIO_SPI_CS_HMC5983, 1); px4_arch_gpiowrite(GPIO_SPI_CS_MS5611, 1); px4_arch_gpiowrite(GPIO_SPI_CS_ICM_2060X, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_BMI055_ACC, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_BMI055_GYR, 1); px4_arch_gpiowrite(GPIO_SPI_CS_BMI160, !selected); break; + case PX4_SPIDEV_BMI055_ACC: + /* Making sure the other peripherals are not selected */ + px4_arch_gpiowrite(GPIO_SPI_CS_MPU9250, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_HMC5983, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_MS5611, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_ICM_2060X, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_BMI160, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_BMI055_GYR, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_BMI055_ACC, !selected); + break; + case PX4_SPIDEV_BMI055_GYR: + /* Making sure the other peripherals are not selected */ + px4_arch_gpiowrite(GPIO_SPI_CS_MPU9250, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_HMC5983, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_MS5611, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_ICM_2060X, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_BMI160, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_BMI055_ACC, 1); + px4_arch_gpiowrite(GPIO_SPI_CS_BMI055_GYR, !selected); + break; default: break; } @@ -198,12 +242,16 @@ __EXPORT void board_spi_reset(int ms) px4_arch_configgpio(GPIO_SPI_CS_OFF_MS5611); px4_arch_configgpio(GPIO_SPI_CS_OFF_ICM_2060X); px4_arch_configgpio(GPIO_SPI_CS_OFF_BMI160); + px4_arch_configgpio(GPIO_SPI_CS_OFF_BMI055_ACC); + px4_arch_configgpio(GPIO_SPI_CS_OFF_BMI055_GYR); px4_arch_gpiowrite(GPIO_SPI_CS_OFF_MPU9250, 0); px4_arch_gpiowrite(GPIO_SPI_CS_OFF_HMC5983, 0); px4_arch_gpiowrite(GPIO_SPI_CS_OFF_MS5611, 0); px4_arch_gpiowrite(GPIO_SPI_CS_OFF_ICM_2060X, 0); px4_arch_gpiowrite(GPIO_SPI_CS_OFF_BMI160, 0); + px4_arch_gpiowrite(GPIO_SPI_CS_OFF_BMI055_ACC, 0); + px4_arch_gpiowrite(GPIO_SPI_CS_OFF_BMI055_GYR, 0); stm32_configgpio(GPIO_SPI1_SCK_OFF); stm32_configgpio(GPIO_SPI1_MISO_OFF); @@ -244,6 +292,8 @@ __EXPORT void board_spi_reset(int ms) px4_arch_configgpio(GPIO_SPI_CS_MS5611); px4_arch_configgpio(GPIO_SPI_CS_ICM_2060X); px4_arch_configgpio(GPIO_SPI_CS_BMI160); + px4_arch_configgpio(GPIO_SPI_CS_BMI055_ACC); + px4_arch_configgpio(GPIO_SPI_CS_BMI055_GYR); stm32_configgpio(GPIO_SPI1_SCK); stm32_configgpio(GPIO_SPI1_MISO); diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 948eefa749..77ae6d3e7c 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -86,6 +86,8 @@ #define DRV_BARO_DEVTYPE_MS5607 0x3E #define DRV_BARO_DEVTYPE_BMP280 0x3F #define DRV_BARO_DEVTYPE_LPS25H 0x40 +#define DRV_ACC_DEVTYPE_BMI055 0x41 +#define DRV_GYR_DEVTYPE_BMI055 0x42 /* * ioctl() definitions diff --git a/src/platforms/px4_spi.h b/src/platforms/px4_spi.h index 4194b27326..049f2154a5 100644 --- a/src/platforms/px4_spi.h +++ b/src/platforms/px4_spi.h @@ -16,6 +16,7 @@ enum spi_dev_e { SPIDEV_MUX, /* Select SPI multiplexer device */ SPIDEV_AUDIO_DATA, /* Select SPI audio codec device data port */ SPIDEV_AUDIO_CTRL, /* Select SPI audio codec device control port */ + SPIDEV_BMI055_GYR /* Select SPI BMI055 Gyroscope */ }; /* Certain SPI devices may required different clocking modes */