diff --git a/src/drivers/barometer/bmp280/BMP280.cpp b/src/drivers/barometer/bmp280/BMP280.cpp index 8a2d43ec3dd..95d92cd3f17 100644 --- a/src/drivers/barometer/bmp280/BMP280.cpp +++ b/src/drivers/barometer/bmp280/BMP280.cpp @@ -63,7 +63,7 @@ BMP280::init() // check id if (_interface->get_reg(BMP280_ADDR_ID) != BMP280_VALUE_ID) { - PX4_WARN("id of your baro is not: 0x%02x", BMP280_VALUE_ID); + PX4_DEBUG("id of your baro is not: 0x%02x", BMP280_VALUE_ID); return -EIO; } diff --git a/src/drivers/imu/adis16477/ADIS16477.cpp b/src/drivers/imu/adis16477/ADIS16477.cpp index 7477ba38fe9..5b9346e1b22 100644 --- a/src/drivers/imu/adis16477/ADIS16477.cpp +++ b/src/drivers/imu/adis16477/ADIS16477.cpp @@ -245,13 +245,13 @@ ADIS16477::read_reg16(uint8_t reg) return cmd[0]; } -void +int ADIS16477::write_reg(uint8_t reg, uint8_t val) { uint8_t cmd[2] {}; cmd[0] = reg | 0x8; cmd[1] = val; - transfer(cmd, cmd, sizeof(cmd)); + return transfer(cmd, cmd, sizeof(cmd)); } void diff --git a/src/drivers/imu/adis16477/ADIS16477.hpp b/src/drivers/imu/adis16477/ADIS16477.hpp index ee99a16a559..9a4d7ae80ec 100644 --- a/src/drivers/imu/adis16477/ADIS16477.hpp +++ b/src/drivers/imu/adis16477/ADIS16477.hpp @@ -116,7 +116,7 @@ private: uint16_t read_reg16(uint8_t reg); - void write_reg(uint8_t reg, uint8_t value); + int write_reg(uint8_t reg, uint8_t value); void write_reg16(uint8_t reg, uint16_t value); // ADIS16477 onboard self test diff --git a/src/drivers/imu/bmi055/BMI055.hpp b/src/drivers/imu/bmi055/BMI055.hpp index 041b17a78dc..66ada4c7bb2 100644 --- a/src/drivers/imu/bmi055/BMI055.hpp +++ b/src/drivers/imu/bmi055/BMI055.hpp @@ -86,7 +86,7 @@ protected: * @param The register to read. * @return The value that was read. */ - uint8_t read_reg(unsigned reg); + uint8_t read_reg(unsigned reg) override; uint16_t read_reg16(unsigned reg); /** @@ -94,7 +94,8 @@ protected: * * @param reg The register to write. * @param value The new value to write. + * @return OK on success, negative errno otherwise. */ - void write_reg(unsigned reg, uint8_t value); + int write_reg(unsigned reg, uint8_t value) override; }; diff --git a/src/drivers/imu/bmi055/bmi055_main.cpp b/src/drivers/imu/bmi055/bmi055_main.cpp index 5a0b74ac243..04f6687214b 100644 --- a/src/drivers/imu/bmi055/bmi055_main.cpp +++ b/src/drivers/imu/bmi055/bmi055_main.cpp @@ -129,7 +129,7 @@ BMI055::read_reg16(unsigned reg) return (uint16_t)(cmd[1] << 8) | cmd[2]; } -void +int BMI055::write_reg(unsigned reg, uint8_t value) { uint8_t cmd[2]; @@ -137,7 +137,7 @@ BMI055::write_reg(unsigned reg, uint8_t value) cmd[0] = reg | DIR_WRITE; cmd[1] = value; - transfer(cmd, nullptr, sizeof(cmd)); + return transfer(cmd, nullptr, sizeof(cmd)); } int diff --git a/src/drivers/imu/bmi088/BMI088.hpp b/src/drivers/imu/bmi088/BMI088.hpp index 8ac39971af0..4285adac514 100644 --- a/src/drivers/imu/bmi088/BMI088.hpp +++ b/src/drivers/imu/bmi088/BMI088.hpp @@ -86,7 +86,7 @@ protected: * @param The register to read. * @return The value that was read. */ - virtual uint8_t read_reg(unsigned reg); // This needs to be declared as virtual, because the + uint8_t read_reg(unsigned reg) override; virtual uint16_t read_reg16(unsigned reg); /** @@ -94,6 +94,7 @@ protected: * * @param reg The register to write. * @param value The new value to write. + * @return OK on success, negative errno otherwise. */ - void write_reg(unsigned reg, uint8_t value); + int write_reg(unsigned reg, uint8_t value) override; }; diff --git a/src/drivers/imu/bmi088/bmi088_main.cpp b/src/drivers/imu/bmi088/bmi088_main.cpp index fcc5cd51a03..d018f3aa62d 100644 --- a/src/drivers/imu/bmi088/bmi088_main.cpp +++ b/src/drivers/imu/bmi088/bmi088_main.cpp @@ -130,7 +130,7 @@ BMI088::read_reg16(unsigned reg) return (uint16_t)(cmd[1] << 8) | cmd[2]; } -void +int BMI088::write_reg(unsigned reg, uint8_t value) { uint8_t cmd[2]; @@ -138,7 +138,7 @@ BMI088::write_reg(unsigned reg, uint8_t value) cmd[0] = reg | DIR_WRITE; cmd[1] = value; - transfer(cmd, nullptr, sizeof(cmd)); + return transfer(cmd, nullptr, sizeof(cmd)); } int diff --git a/src/drivers/imu/bmi160/bmi160.cpp b/src/drivers/imu/bmi160/bmi160.cpp index 23b283ff583..1ef0e3ed016 100644 --- a/src/drivers/imu/bmi160/bmi160.cpp +++ b/src/drivers/imu/bmi160/bmi160.cpp @@ -295,16 +295,15 @@ uint8_t BMI160::read_reg(uint8_t reg) return cmd[1]; } -void BMI160::write_reg(uint8_t reg, uint8_t value) +int BMI160::write_reg(unsigned reg, uint8_t value) { uint8_t cmd[2]; cmd[0] = reg | DIR_WRITE; cmd[1] = value; - transfer(cmd, nullptr, sizeof(cmd)); + return transfer(cmd, nullptr, sizeof(cmd)); } -void -BMI160::modify_reg(uint8_t reg, uint8_t clearbits, uint8_t setbits) +void BMI160::modify_reg(uint8_t reg, uint8_t clearbits, uint8_t setbits) { uint8_t val = read_reg(reg); val &= ~clearbits; diff --git a/src/drivers/imu/bmi160/bmi160.hpp b/src/drivers/imu/bmi160/bmi160.hpp index 50e6e96aac3..02f5da9622b 100644 --- a/src/drivers/imu/bmi160/bmi160.hpp +++ b/src/drivers/imu/bmi160/bmi160.hpp @@ -314,8 +314,9 @@ private: * * @param reg The register to write. * @param value The new value to write. + * @return OK on success, negative errno otherwise. */ - void write_reg(uint8_t reg, uint8_t value); + int write_reg(uint8_t reg, uint8_t value); /** * Modify a register in the BMI160 diff --git a/src/drivers/imu/fxas21002c/CMakeLists.txt b/src/drivers/imu/fxas21002c/CMakeLists.txt index ea04e45239d..56f373064d4 100644 --- a/src/drivers/imu/fxas21002c/CMakeLists.txt +++ b/src/drivers/imu/fxas21002c/CMakeLists.txt @@ -38,5 +38,7 @@ px4_add_module( SRCS FXAS21002C.cpp FXAS21002C.hpp + fxas21002c_i2c.cpp + fxas21002c_spi.cpp fxas21002c_main.cpp ) diff --git a/src/drivers/imu/fxas21002c/FXAS21002C.cpp b/src/drivers/imu/fxas21002c/FXAS21002C.cpp index 378509ec56d..742a4bf1faa 100644 --- a/src/drivers/imu/fxas21002c/FXAS21002C.cpp +++ b/src/drivers/imu/fxas21002c/FXAS21002C.cpp @@ -33,127 +33,6 @@ #include "FXAS21002C.hpp" -/* SPI protocol address bits */ -#define DIR_READ(a) ((a) | (1 << 7)) -#define DIR_WRITE(a) ((a) & 0x7f) -#define swap16(w) __builtin_bswap16((w)) - -#define FXAS21002C_STATUS 0x00 -#define FXAS21002C_OUT_X_MSB 0x01 -#define FXAS21002C_OUT_X_LSB 0x02 -#define FXAS21002C_OUT_Y_MSB 0x03 -#define FXAS21002C_OUT_Y_LSB 0x04 -#define FXAS21002C_OUT_Z_MSB 0x05 -#define FXAS21002C_OUT_Z_LSB 0x06 - -#define FXAS21002C_DR_STATUS 0x07 -# define DR_STATUS_ZYXOW (1 << 7) -# define DR_STATUS_ZOW (1 << 6) -# define DR_STATUS_YOW (1 << 5) -# define DR_STATUS_XOW (1 << 4) -# define DR_STATUS_ZYXDR (1 << 3) -# define DR_STATUS_ZDR (1 << 2) -# define DR_STATUS_YDR (1 << 1) -# define DR_STATUS_XDR (1 << 0) - -#define FXAS21002C_F_STATUS 0x08 -# define F_STATUS_F_OVF (1 << 7) -# define F_STATUS_F_WMKF (1 << 6) -# define F_STATUS_F_CNT_SHIFTS 0 -# define F_STATUS_F_CNT_MASK (0x3f << F_STATUS_F_CNT_SHIFTS) - -#define FXAS21002C_F_SETUP 0x09 -# define F_SETUP_F_MODE_SHIFTS 6 -# define F_SETUP_F_MODE_MASK (0x3 << F_SETUP_F_MODE_SHIFTS) -# define F_SETUP_F_WMRK_SHIFTS 0 -# define F_SETUP_F_WMRK_MASK (0x3f << F_SETUP_F_WMRK_SHIFTS) - -#define FXAS21002C_F_EVENT 0x0a -# define F_EVENT_F_EVENT (1 << 5) -# define F_EVENT_FE_TIME_SHIFTS 0 -# define F_EVENT_FE_TIME_MASK (0x1f << F_EVENT_FE_TIME_SHIFTS) - -#define FXAS21002C_INT_SRC_FLAG 0x0b -# define INT_SRC_FLAG_BOOTEND (1 << 3) -# define INT_SRC_FLAG_SRC_FIFO (1 << 2) -# define INT_SRC_FLAG_SRC_RT (1 << 1) -# define INT_SRC_FLAG_SRC_DRDY (1 << 0) - -#define FXAS21002C_WHO_AM_I 0x0c -#define WHO_AM_I 0xd7 - -#define FXAS21002C_CTRL_REG0 0x0d -# define CTRL_REG0_BW_SHIFTS 6 -# define CTRL_REG0_BW_MASK (0x3 << CTRL_REG0_BW_SHIFTS) -# define CTRL_REG0_BW(n) (((n) & 0x3) << CTRL_REG0_BW_SHIFTS) -# define CTRL_REG0_BW_HIGH CTRL_REG0_BW(0) -# define CTRL_REG0_BW_MED CTRL_REG0_BW(1) -# define CTRL_REG0_BW_LOW CTRL_REG0_BW(2) -# define CTRL_REG0_SPIW (1 << 6) -# define CTRL_REG0_SEL_SHIFTS 3 -# define CTRL_REG0_SEL_MASK (0x2 << CTRL_REG0_SEL_SHIFTS) -# define CTRL_REG0_HPF_EN (1 << 2) -# define CTRL_REG0_FS_SHIFTS 0 -# define CTRL_REG0_FS_MASK (0x3 << CTRL_REG0_FS_SHIFTS) -# define CTRL_REG0_FS_2000_DPS (0 << CTRL_REG0_FS_SHIFTS) -# define CTRL_REG0_FS_1000_DPS (1 << CTRL_REG0_FS_SHIFTS) -# define CTRL_REG0_FS_500_DPS (2 << CTRL_REG0_FS_SHIFTS) -# define CTRL_REG0_FS_250_DPS (3 << CTRL_REG0_FS_SHIFTS) - -#define FXAS21002C_RT_CFG 0x0e -# define RT_CFG_ELE (1 << 3) -# define RT_CFG_ZTEFE (1 << 2) -# define RT_CFG_YTEFE (1 << 1) -# define RT_CFG_XTEFE (1 << 0) - -#define FXAS21002C_RT_SRC 0x0f -# define RT_SRC_EA (1 << 6) -# define RT_SRC_ZRT (1 << 5) -# define RT_SRC_Z_RT_POL (1 << 4) -# define RT_SRC_YRT (1 << 3) -# define RT_SRC_Y_RT_POL (1 << 2) -# define RT_SRC_XRT (1 << 1) -# define RT_SRC_X_RT_POL (1 << 0) - -#define FXAS21002C_RT_THS 0x10 -# define RT_THS_DBCNTM (1 << 7) -# define RT_THS_THS_SHIFTS 0 -# define RT_THS_THS_MASK (0x7f << RT_THS_THS_SHIFTS) - -#define FXAS21002C_RT_COUNT 0x11 -#define FXAS21002C_TEMP 0x12 - -#define FXAS21002C_CTRL_REG1 0x13 -# define CTRL_REG1_RST (1 << 6) -# define CTRL_REG1_ST (1 << 5) -# define CTRL_REG1_DR_SHIFTS 2 -# define CTRL_REG1_DR_MASK (0x07 << CTRL_REG1_DR_SHIFTS) -# define CTRL_REG1_DR_12_5 (7 << CTRL_REG1_DR_SHIFTS) -# define CTRL_REG1_DR_12_5_1 (6 << CTRL_REG1_DR_SHIFTS) -# define CTRL_REG1_DR_25HZ (5 << CTRL_REG1_DR_SHIFTS) -# define CTRL_REG1_DR_50HZ (4 << CTRL_REG1_DR_SHIFTS) -# define CTRL_REG1_DR_100HZ (3 << CTRL_REG1_DR_SHIFTS) -# define CTRL_REG1_DR_200HZ (2 << CTRL_REG1_DR_SHIFTS) -# define CTRL_REG1_DR_400HZ (1 << CTRL_REG1_DR_SHIFTS) -# define CTRL_REG1_DR_800HZ (0 << CTRL_REG1_DR_SHIFTS) -# define CTRL_REG1_ACTIVE (1 << 1) -# define CTRL_REG1_READY (1 << 0) - -#define FXAS21002C_CTRL_REG2 0x14 -# define CTRL_REG2_INT_CFG_FIFO (1 << 7) -# define CTRL_REG2_INT_EN_FIFO (1 << 6) -# define CTRL_REG2_INT_CFG_RT (1 << 5) -# define CTRL_REG2_INT_EN_RT (1 << 4) -# define CTRL_REG2_INT_CFG_DRDY (1 << 3) -# define CTRL_REG2_INT_EN_DRDY (1 << 2) -# define CTRL_REG2_IPOL (1 << 1) -# define CTRL_REG2_PP_OD (1 << 0) - -#define FXAS21002C_CTRL_REG3 0x15 -# define CTRL_REG3_WRAPTOONE (1 << 3) -# define CTRL_REG3_EXTCTRLEN (1 << 2) -# define CTRL_REG3_FS_DOUBLE (1 << 0) - #define DEF_REG(r) {r, #r} /* default values for this device */ @@ -186,11 +65,11 @@ static constexpr uint8_t _checked_registers[] { using namespace time_literals; -FXAS21002C::FXAS21002C(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode) : - SPI(DRV_GYR_DEVTYPE_FXAS2100C, MODULE_NAME, bus, device, spi_mode, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_gyro(get_device_id(), (external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT), rotation), +FXAS21002C::FXAS21002C(device::Device *interface, I2CSPIBusOption bus_option, int bus, enum Rotation rotation, + int i2c_address) : + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus, i2c_address), + _interface(interface), + _px4_gyro(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT), rotation), _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), _errors(perf_alloc(PC_COUNT, MODULE_NAME": err")), _bad_registers(perf_alloc(PC_COUNT, MODULE_NAME": bad register")), @@ -209,12 +88,16 @@ FXAS21002C::~FXAS21002C() int FXAS21002C::init() { - /* do SPI init (and probe) first */ - if (SPI::init() != OK) { - PX4_ERR("SPI init failed"); + /* do SPI/I2C init (and probe) first */ + if (_interface->init() != OK) { + PX4_ERR("SPI/I2C interface init failed"); return PX4_ERROR; } + // passed SPI::probe or I2C::probe, which checked WHO_AM_I + // measurements will not start before registers are checked OK + _checked_values[0] = WHO_AM_I; + reset(); start(); @@ -253,44 +136,6 @@ FXAS21002C::reset() _read = 0; } -int -FXAS21002C::probe() -{ - /* verify that the device is attached and functioning */ - bool success = (read_reg(FXAS21002C_WHO_AM_I) == WHO_AM_I); - - if (success) { - _checked_values[0] = WHO_AM_I; - return OK; - } - - return -EIO; -} - -uint8_t -FXAS21002C::read_reg(unsigned reg) -{ - uint8_t cmd[2]; - - cmd[0] = DIR_READ(reg); - cmd[1] = 0; - - transfer(cmd, cmd, sizeof(cmd)); - - return cmd[1]; -} - -void -FXAS21002C::write_reg(unsigned reg, uint8_t value) -{ - uint8_t cmd[2]; - - cmd[0] = DIR_WRITE(reg); - cmd[1] = value; - - transfer(cmd, nullptr, sizeof(cmd)); -} - void FXAS21002C::write_checked_reg(unsigned reg, uint8_t value) { @@ -508,15 +353,7 @@ FXAS21002C::RunImpl() perf_begin(_sample_perf); /* status register and data as read back from the device */ -#pragma pack(push, 1) - struct { - uint8_t cmd; - uint8_t status; - int16_t x; - int16_t y; - int16_t z; - } raw_gyro_report{}; -#pragma pack(pop) + RawGyroReport raw_gyro_report{}; check_registers(); @@ -529,9 +366,9 @@ FXAS21002C::RunImpl() } /* fetch data from the sensor */ - raw_gyro_report.cmd = DIR_READ(FXAS21002C_STATUS); const hrt_abstime timestamp_sample = hrt_absolute_time(); - transfer((uint8_t *)&raw_gyro_report, (uint8_t *)&raw_gyro_report, sizeof(raw_gyro_report)); + + _interface->read(FXAS21002C_STATUS, (uint8_t *)&raw_gyro_report, sizeof(raw_gyro_report)); if (!(raw_gyro_report.status & DR_STATUS_ZYXDR)) { perf_end(_sample_perf); diff --git a/src/drivers/imu/fxas21002c/FXAS21002C.hpp b/src/drivers/imu/fxas21002c/FXAS21002C.hpp index 85d4def8a11..31111f63fb2 100644 --- a/src/drivers/imu/fxas21002c/FXAS21002C.hpp +++ b/src/drivers/imu/fxas21002c/FXAS21002C.hpp @@ -34,30 +34,171 @@ /** * @file FXAS21002C.hpp * Driver for the NXP FXAS21002C 3-Axis Digital Angular Rate Gyroscope - * connected via SPI + * connected via SPI or I2C */ #pragma once -#include +#include #include #include #include #include +#if defined(PX4_I2C_FXAS21002C_ADDR) && defined(PX4_I2C_BUS_EXPANSION) +# define FXAS21002C_USE_I2C +#endif -class FXAS21002C : public device::SPI, public I2CSPIDriver +/* SPI protocol address bits */ +#define DIR_READ(a) ((a) | (1 << 7)) +#define DIR_WRITE(a) ((a) & 0x7f) +#define swap16(w) __builtin_bswap16((w)) + +#define FXAS21002C_REG_MASK 0x00FF +#define FXAS21002C_REG(r) ((r) & FXAS21002C_REG_MASK) + +#define FXAS21002C_STATUS 0x00 +#define FXAS21002C_OUT_X_MSB 0x01 +#define FXAS21002C_OUT_X_LSB 0x02 +#define FXAS21002C_OUT_Y_MSB 0x03 +#define FXAS21002C_OUT_Y_LSB 0x04 +#define FXAS21002C_OUT_Z_MSB 0x05 +#define FXAS21002C_OUT_Z_LSB 0x06 + +#define FXAS21002C_DR_STATUS 0x07 +# define DR_STATUS_ZYXOW (1 << 7) +# define DR_STATUS_ZOW (1 << 6) +# define DR_STATUS_YOW (1 << 5) +# define DR_STATUS_XOW (1 << 4) +# define DR_STATUS_ZYXDR (1 << 3) +# define DR_STATUS_ZDR (1 << 2) +# define DR_STATUS_YDR (1 << 1) +# define DR_STATUS_XDR (1 << 0) + +#define FXAS21002C_F_STATUS 0x08 +# define F_STATUS_F_OVF (1 << 7) +# define F_STATUS_F_WMKF (1 << 6) +# define F_STATUS_F_CNT_SHIFTS 0 +# define F_STATUS_F_CNT_MASK (0x3f << F_STATUS_F_CNT_SHIFTS) + +#define FXAS21002C_F_SETUP 0x09 +# define F_SETUP_F_MODE_SHIFTS 6 +# define F_SETUP_F_MODE_MASK (0x3 << F_SETUP_F_MODE_SHIFTS) +# define F_SETUP_F_WMRK_SHIFTS 0 +# define F_SETUP_F_WMRK_MASK (0x3f << F_SETUP_F_WMRK_SHIFTS) + +#define FXAS21002C_F_EVENT 0x0a +# define F_EVENT_F_EVENT (1 << 5) +# define F_EVENT_FE_TIME_SHIFTS 0 +# define F_EVENT_FE_TIME_MASK (0x1f << F_EVENT_FE_TIME_SHIFTS) + +#define FXAS21002C_INT_SRC_FLAG 0x0b +# define INT_SRC_FLAG_BOOTEND (1 << 3) +# define INT_SRC_FLAG_SRC_FIFO (1 << 2) +# define INT_SRC_FLAG_SRC_RT (1 << 1) +# define INT_SRC_FLAG_SRC_DRDY (1 << 0) + +#define FXAS21002C_WHO_AM_I 0x0c +#define WHO_AM_I 0xd7 + +#define FXAS21002C_CTRL_REG0 0x0d +# define CTRL_REG0_BW_SHIFTS 6 +# define CTRL_REG0_BW_MASK (0x3 << CTRL_REG0_BW_SHIFTS) +# define CTRL_REG0_BW(n) (((n) & 0x3) << CTRL_REG0_BW_SHIFTS) +# define CTRL_REG0_BW_HIGH CTRL_REG0_BW(0) +# define CTRL_REG0_BW_MED CTRL_REG0_BW(1) +# define CTRL_REG0_BW_LOW CTRL_REG0_BW(2) +# define CTRL_REG0_SPIW (1 << 6) +# define CTRL_REG0_SEL_SHIFTS 3 +# define CTRL_REG0_SEL_MASK (0x2 << CTRL_REG0_SEL_SHIFTS) +# define CTRL_REG0_HPF_EN (1 << 2) +# define CTRL_REG0_FS_SHIFTS 0 +# define CTRL_REG0_FS_MASK (0x3 << CTRL_REG0_FS_SHIFTS) +# define CTRL_REG0_FS_2000_DPS (0 << CTRL_REG0_FS_SHIFTS) +# define CTRL_REG0_FS_1000_DPS (1 << CTRL_REG0_FS_SHIFTS) +# define CTRL_REG0_FS_500_DPS (2 << CTRL_REG0_FS_SHIFTS) +# define CTRL_REG0_FS_250_DPS (3 << CTRL_REG0_FS_SHIFTS) + +#define FXAS21002C_RT_CFG 0x0e +# define RT_CFG_ELE (1 << 3) +# define RT_CFG_ZTEFE (1 << 2) +# define RT_CFG_YTEFE (1 << 1) +# define RT_CFG_XTEFE (1 << 0) + +#define FXAS21002C_RT_SRC 0x0f +# define RT_SRC_EA (1 << 6) +# define RT_SRC_ZRT (1 << 5) +# define RT_SRC_Z_RT_POL (1 << 4) +# define RT_SRC_YRT (1 << 3) +# define RT_SRC_Y_RT_POL (1 << 2) +# define RT_SRC_XRT (1 << 1) +# define RT_SRC_X_RT_POL (1 << 0) + +#define FXAS21002C_RT_THS 0x10 +# define RT_THS_DBCNTM (1 << 7) +# define RT_THS_THS_SHIFTS 0 +# define RT_THS_THS_MASK (0x7f << RT_THS_THS_SHIFTS) + +#define FXAS21002C_RT_COUNT 0x11 +#define FXAS21002C_TEMP 0x12 + +#define FXAS21002C_CTRL_REG1 0x13 +# define CTRL_REG1_RST (1 << 6) +# define CTRL_REG1_ST (1 << 5) +# define CTRL_REG1_DR_SHIFTS 2 +# define CTRL_REG1_DR_MASK (0x07 << CTRL_REG1_DR_SHIFTS) +# define CTRL_REG1_DR_12_5 (7 << CTRL_REG1_DR_SHIFTS) +# define CTRL_REG1_DR_12_5_1 (6 << CTRL_REG1_DR_SHIFTS) +# define CTRL_REG1_DR_25HZ (5 << CTRL_REG1_DR_SHIFTS) +# define CTRL_REG1_DR_50HZ (4 << CTRL_REG1_DR_SHIFTS) +# define CTRL_REG1_DR_100HZ (3 << CTRL_REG1_DR_SHIFTS) +# define CTRL_REG1_DR_200HZ (2 << CTRL_REG1_DR_SHIFTS) +# define CTRL_REG1_DR_400HZ (1 << CTRL_REG1_DR_SHIFTS) +# define CTRL_REG1_DR_800HZ (0 << CTRL_REG1_DR_SHIFTS) +# define CTRL_REG1_ACTIVE (1 << 1) +# define CTRL_REG1_READY (1 << 0) + +#define FXAS21002C_CTRL_REG2 0x14 +# define CTRL_REG2_INT_CFG_FIFO (1 << 7) +# define CTRL_REG2_INT_EN_FIFO (1 << 6) +# define CTRL_REG2_INT_CFG_RT (1 << 5) +# define CTRL_REG2_INT_EN_RT (1 << 4) +# define CTRL_REG2_INT_CFG_DRDY (1 << 3) +# define CTRL_REG2_INT_EN_DRDY (1 << 2) +# define CTRL_REG2_IPOL (1 << 1) +# define CTRL_REG2_PP_OD (1 << 0) + +#define FXAS21002C_CTRL_REG3 0x15 +# define CTRL_REG3_WRAPTOONE (1 << 3) +# define CTRL_REG3_EXTCTRLEN (1 << 2) +# define CTRL_REG3_FS_DOUBLE (1 << 0) + +device::Device *FXAS21002C_SPI_interface(int bus, uint32_t chip_select, int bus_frequency, spi_mode_e spi_mode); +device::Device *FXAS21002C_I2C_interface(int bus, int bus_frequency, int i2c_address); + +/* status register and data as read back from the device */ +#pragma pack(push, 1) +struct RawGyroReport { + uint8_t cmd; + uint8_t status; + int16_t x; + int16_t y; + int16_t z; +}; +#pragma pack(pop) + + +class FXAS21002C : public I2CSPIDriver { public: - FXAS21002C(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode); + FXAS21002C(device::Device *interface, I2CSPIBusOption bus_option, int bus, enum Rotation rotation, int i2c_address); virtual ~FXAS21002C(); static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, int runtime_instance); static void print_usage(); - int init() override; + int init(); void print_status() override; @@ -67,10 +208,10 @@ public: void test_error(); protected: void custom_method(const BusCLIArguments &cli); - int probe() override; + int probe(); private: - + device::Device *_interface; PX4Gyroscope _px4_gyro; unsigned _current_rate{800}; @@ -121,7 +262,10 @@ private: * @param The register to read. * @return The value that was read. */ - uint8_t read_reg(unsigned reg); + inline uint8_t read_reg(unsigned reg) + { + return _interface->read_reg(reg); + } /** * Write a register in the FXAS21002C @@ -129,7 +273,10 @@ private: * @param reg The register to write. * @param value The new value to write. */ - void write_reg(unsigned reg, uint8_t value); + inline int write_reg(unsigned reg, uint8_t value) + { + return _interface->write_reg(reg, value); + } /** * Modify a register in the FXAS21002C diff --git a/src/drivers/imu/fxas21002c/fxas21002c_i2c.cpp b/src/drivers/imu/fxas21002c/fxas21002c_i2c.cpp new file mode 100644 index 00000000000..f2db92cedcc --- /dev/null +++ b/src/drivers/imu/fxas21002c/fxas21002c_i2c.cpp @@ -0,0 +1,202 @@ +/**************************************************************************** + * + * Copyright (c) 2016-2020 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. + * + ****************************************************************************/ + +/** + * @file fxas21002c_i2c.cpp + * + * Driver for the NXP FXAS21002C connected via I2C. + * + * @author Robert Fu + */ +#include +#include "FXAS21002C.hpp" + +device::Device *FXAS21002C_I2C_interface(int bus, int bus_frequenc, int i2c_addressy); + +class FXAS21002C_I2C : public device::I2C +{ +public: + FXAS21002C_I2C(int bus, int bus_frequency, int i2c_address); + ~FXAS21002C_I2C() override = default; + + /** + * Read directly from the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param reg The register address at which to start reading + * @param data The buffer into which the read values should be placed. + * @param count The number of items to read. + * @return The number of items read on success, negative errno otherwise. + */ + int read(unsigned reg, void *data, unsigned count) override; + + /** + * Write directly to the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param reg The register address at which to start writing. + * @param data The buffer from which values should be read. + * @param count The number of items to write. + * @return The number of items written on success, negative errno otherwise. + */ + int write(unsigned reg, void *data, unsigned count) override; + + /** + * Read a register from the device. + * + * @param The register to read. + * @return The value that was read. + */ + uint8_t read_reg(unsigned reg) override; + + /** + * Write a register in the device. + * + * @param reg The register to write. + * @param value The new value to write. + * @return OK on success, negative errno otherwise. + */ + int write_reg(unsigned reg, uint8_t value) override; + +protected: + int probe() override; +}; + +device::Device * +FXAS21002C_I2C_interface(int bus, int bus_frequency, int i2c_address) +{ + return new FXAS21002C_I2C(bus, bus_frequency, i2c_address); +} + +FXAS21002C_I2C::FXAS21002C_I2C(int bus, int bus_frequency, int i2c_address) : + I2C(DRV_GYR_DEVTYPE_FXAS2100C, MODULE_NAME, bus, i2c_address, bus_frequency) +{ +} + +int +FXAS21002C_I2C::probe() +{ + uint8_t whoami = read_reg(FXAS21002C_WHO_AM_I); + bool success = (whoami == WHO_AM_I); + + return success ? OK : -EIO; +} + +/** + * Read directly from the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param reg The register address at which to start reading + * @param data The buffer into which the read values should be placed. + * @param count The number of items to read. + * @return The number of items read on success, negative errno otherwise. + */ +int FXAS21002C_I2C::read(unsigned reg, void *data, unsigned count) +{ + /* Same as in mpu9250_i2c.cpp: + * We want to avoid copying the data of RawGyroReport: So if the caller + * supplies a buffer not RawGyroReport in size, it is assume to be a reg or + * reg 16 read + * Since RawGyroReport has a cmd at front, we must return the data + * after that. Foe anthing else we must return it + */ + uint32_t offset = count < sizeof(RawGyroReport) ? 0 : offsetof(RawGyroReport, status); + uint8_t cmd = FXAS21002C_REG(reg); + + return transfer(&cmd, 1, &((uint8_t *)data)[offset], count - offset); +} + +/** + * Write directly to the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param reg The register address at which to start writing. + * @param data The buffer from which values should be read. + * @param count The number of items to write. + * @return The number of items written on success, negative errno otherwise. + */ +int FXAS21002C_I2C::write(unsigned reg, void *data, unsigned count) +{ + uint8_t cmd[2]; + + if (sizeof(cmd) < (count + 1)) { + // same as in mpu9250_i2c.cpp + // This condition means only supportting the case of count == 1 + // so this API is the same as write_reg + return -EIO; + } + + cmd[0] = FXAS21002C_REG(reg); + cmd[1] = *(uint8_t *)data; + + return transfer(cmd, sizeof(cmd), nullptr, 0); +} + +/** + * Read a register from the device. + * + * @param The register to read. + * @return The value that was read. + */ +uint8_t FXAS21002C_I2C::read_reg(unsigned reg) +{ + uint8_t cmd[1]; + uint8_t data[1]; + + cmd[0] = reg & 0x00FF; + + transfer(cmd, 1, data, 1); + + return data[0]; +} + +/** + * Write a register in the device. + * + * @param reg The register to write. + * @param value The new value to write. + * @return OK on success, negative errno otherwise. + */ +int FXAS21002C_I2C::write_reg(unsigned reg, uint8_t value) +{ + uint8_t cmd[2]; + + cmd[0] = reg & 0x00FF; + cmd[1] = value; + + return transfer(cmd, 2, nullptr, 0); +} diff --git a/src/drivers/imu/fxas21002c/fxas21002c_main.cpp b/src/drivers/imu/fxas21002c/fxas21002c_main.cpp index 5f439d3d1b5..854641b0667 100644 --- a/src/drivers/imu/fxas21002c/fxas21002c_main.cpp +++ b/src/drivers/imu/fxas21002c/fxas21002c_main.cpp @@ -42,7 +42,8 @@ FXAS21002C::print_usage() PRINT_MODULE_USAGE_NAME("fxas21002c", "driver"); PRINT_MODULE_USAGE_SUBCATEGORY("imu"); PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x20); PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); PRINT_MODULE_USAGE_COMMAND("regdump"); PRINT_MODULE_USAGE_COMMAND("testerror"); @@ -52,20 +53,34 @@ FXAS21002C::print_usage() I2CSPIDriverBase *FXAS21002C::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, int runtime_instance) { - FXAS21002C *instance = new FXAS21002C(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, - cli.bus_frequency, cli.spi_mode); + device::Device *interface = nullptr; - if (!instance) { + if (iterator.busType() == BOARD_I2C_BUS) { + interface = FXAS21002C_I2C_interface(iterator.bus(), cli.bus_frequency, cli.i2c_address); + + } else if (iterator.busType() == BOARD_SPI_BUS) { + interface = FXAS21002C_SPI_interface(iterator.bus(), iterator.devid(), cli.bus_frequency, cli.spi_mode); + } + + if (interface == nullptr) { PX4_ERR("alloc failed"); return nullptr; } - if (OK != instance->init()) { - delete instance; + FXAS21002C *dev = new FXAS21002C(interface, iterator.configuredBusOption(), iterator.bus(), cli.rotation, + cli.i2c_address); + + if (dev == nullptr) { + delete interface; return nullptr; } - return instance; + if (OK != dev->init()) { + delete dev; + return nullptr; + } + + return dev; } void FXAS21002C::custom_method(const BusCLIArguments &cli) @@ -82,9 +97,11 @@ extern "C" int fxas21002c_main(int argc, char *argv[]) { int ch; using ThisDriver = FXAS21002C; - BusCLIArguments cli{false, true}; + BusCLIArguments cli{true, true}; + cli.default_i2c_frequency = 400 * 1000; cli.default_spi_frequency = 2 * 1000 * 1000; cli.spi_mode = SPIDEV_MODE0; + cli.i2c_address = 0x20; while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { switch (ch) { @@ -126,5 +143,5 @@ extern "C" int fxas21002c_main(int argc, char *argv[]) } ThisDriver::print_usage(); - return -1; + return PX4_ERROR; } diff --git a/src/drivers/imu/fxas21002c/fxas21002c_spi.cpp b/src/drivers/imu/fxas21002c/fxas21002c_spi.cpp new file mode 100644 index 00000000000..d5b4397f1eb --- /dev/null +++ b/src/drivers/imu/fxas21002c/fxas21002c_spi.cpp @@ -0,0 +1,223 @@ +/**************************************************************************** + * + * Copyright (c) 2016-2020 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. + * + ****************************************************************************/ + +/** + * @file fxas21002c_spi.cpp + * + * Driver for the NXP FXAS21002C connected via SPI. + * + * @author Robert Fu + */ +#include +#include "FXAS21002C.hpp" + +device::Device *FXAS21002C_SPI_interface(int bus, uint32_t chip_select, int bus_frequency, + spi_mode_e spi_mode); + +class FXAS21002C_SPI : public device::SPI +{ +public: + FXAS21002C_SPI(int bus, uint32_t chip_select, int bus_frequency, spi_mode_e spi_mode); + ~FXAS21002C_SPI() override = default; + + /** + * Read directly from the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param reg The register address at which to start reading + * @param data The buffer into which the read values should be placed. + * @param count The number of items to read. + * @return The number of items read on success, negative errno otherwise. + */ + int read(unsigned reg, void *data, unsigned count) override; + + /** + * Write directly to the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param reg The register address at which to start writing. + * @param data The buffer from which values should be read. + * @param count The number of items to write. + * @return The number of items written on success, negative errno otherwise. + */ + int write(unsigned reg, void *data, unsigned count) override; + + /** + * Read a register from the device. + * + * @param The register to read. + * @return The value that was read. + */ + uint8_t read_reg(unsigned reg) override; + + /** + * Write a register to the device. + * + * @param reg The register to write. + * @param value The new value to write. + * @return OK on success, negative errno otherwise. + */ + int write_reg(unsigned reg, uint8_t value) override; + +protected: + int probe() override; +}; + +device::Device * +FXAS21002C_SPI_interface(int bus, uint32_t chip_select, int bus_frequency, spi_mode_e spi_mode) +{ + return new FXAS21002C_SPI(bus, chip_select, bus_frequency, spi_mode); +} + +FXAS21002C_SPI::FXAS21002C_SPI(int bus, uint32_t chip_select, int bus_frequency, spi_mode_e spi_mode) : + SPI(DRV_GYR_DEVTYPE_FXAS2100C, MODULE_NAME, bus, chip_select, spi_mode, bus_frequency) +{ +} + +int +FXAS21002C_SPI::probe() +{ + uint8_t whoami = read_reg(FXAS21002C_WHO_AM_I); + bool success = (whoami == WHO_AM_I); + + DEVICE_DEBUG("FXAS21002C_SPI::probe: %s, whoami: 0x%02x", (success ? "Succeeded" : "failed"), whoami); + return success ? OK : -EIO; +} + +/** + * Read directly from the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param reg The register address at which to start reading + * @param data The buffer into which the read values should be placed. + * @param count The number of items to read. + * @return The number of items read on success, negative errno otherwise. + */ +int FXAS21002C_SPI::read(unsigned reg, void *data, unsigned count) +{ + /* Same as in mpu9250_spi.cpp: + * We want to avoid copying the data of RawGyroReport: So if the caller + * supplies a buffer not RawGyroReport in size, it is assume to be a reg or reg 16 read + * and we need to provied the buffer large enough for the callers data + * and our command. + */ + uint8_t cmd[3] {}; + + uint8_t *pBuf = count < sizeof(RawGyroReport) ? cmd : (uint8_t *) data ; + + if (count < sizeof(RawGyroReport)) { + /* add command */ + count++; + } + + /* Set command */ + pBuf[0] = DIR_READ(reg); + + /* Transfer the command and get the data */ + int ret = transfer(pBuf, pBuf, count); + + if (ret == OK && pBuf == &cmd[0]) { + /* Adjust the count back */ + count--; + + /* Return the data */ + memcpy(data, &cmd[1], count); + } + + return ret; +} + +/** + * Write directly to the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param reg The register address at which to start writing. + * @param data The buffer from which values should be read. + * @param count The number of items to write. + * @return The number of items written on success, negative errno otherwise. + */ +int FXAS21002C_SPI::write(unsigned reg, void *data, unsigned count) +{ + uint8_t cmd[2] {}; + + if (sizeof(cmd) < (count + 1)) { + // same as in mpu9250_spi.cpp + // This condition means only supportting the case of count == 1 + // so this API is the same as write_reg + return -EIO; + } + + cmd[0] = DIR_WRITE(reg); + cmd[1] = *(uint8_t *)data; + + return transfer(cmd, nullptr, sizeof(cmd)); +} + +/** + * Read a register from the device. + * + * @param The register to read. + * @return The value that was read. + */ +uint8_t FXAS21002C_SPI::read_reg(unsigned reg) +{ + uint8_t cmd[2]; + + cmd[0] = DIR_READ(reg); + cmd[1] = 0; + + transfer(cmd, cmd, sizeof(cmd)); + + return cmd[1]; +} + +/** + * Write a register to the device. + * + * @param reg The register to write. + * @param value The new value to write. + * @return OK on success, negative errno otherwise. + */ +int FXAS21002C_SPI::write_reg(unsigned reg, uint8_t value) +{ + uint8_t cmd[2]; + + cmd[0] = DIR_WRITE(reg); + cmd[1] = value; + + return transfer(cmd, nullptr, sizeof(cmd)); +} diff --git a/src/drivers/imu/fxos8701cq/CMakeLists.txt b/src/drivers/imu/fxos8701cq/CMakeLists.txt index 7ec14b5f46d..a8c1ca6c1e4 100644 --- a/src/drivers/imu/fxos8701cq/CMakeLists.txt +++ b/src/drivers/imu/fxos8701cq/CMakeLists.txt @@ -38,5 +38,7 @@ px4_add_module( SRCS FXOS8701CQ.cpp FXOS8701CQ.hpp + fxos8701cq_i2c.cpp + fxos8701cq_spi.cpp fxos8701cq_main.cpp ) diff --git a/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp b/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp index ac08fb5fff4..e56f61de671 100644 --- a/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp +++ b/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp @@ -53,13 +53,13 @@ const uint8_t FXOS8701CQ::_checked_registers[FXOS8701C_NUM_CHECKED_REGISTERS] = FXOS8701CQ_M_CTRL_REG2, }; -FXOS8701CQ::FXOS8701CQ(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode) : - SPI(DRV_ACC_DEVTYPE_FXOS8701C, MODULE_NAME, bus, device, spi_mode, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_accel(get_device_id(), ORB_PRIO_LOW, rotation), +FXOS8701CQ::FXOS8701CQ(device::Device *interface, I2CSPIBusOption bus_option, int bus, enum Rotation rotation, + int i2c_address) : + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus, i2c_address), + _interface(interface), + _px4_accel(interface->get_device_id(), ORB_PRIO_LOW, rotation), #if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) - _px4_mag(get_device_id(), ORB_PRIO_LOW, rotation), + _px4_mag(interface->get_device_id(), ORB_PRIO_LOW, rotation), _mag_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": mag read")), #endif _accel_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": acc read")), @@ -86,11 +86,20 @@ FXOS8701CQ::~FXOS8701CQ() int FXOS8701CQ::init() { - // do SPI init (and probe) first - int ret = SPI::init(); + // do SPI/I2C init (and probe) first + int ret = _interface->init(); if (ret != OK) { - PX4_ERR("SPI init failed"); + PX4_ERR("SPI/I2C interface init failed"); + return ret; + } + + // There are 2 possible WHOAMI return values, + // so probe here again to set proper _checked_values[0] + ret = probe(); + + if (ret != OK) { + PX4_ERR("FXOS8701CQ::probe() failed"); return ret; } @@ -136,32 +145,6 @@ FXOS8701CQ::probe() return -EIO; } -uint8_t -FXOS8701CQ::read_reg(unsigned reg) -{ - uint8_t cmd[3]; - - cmd[0] = DIR_READ(reg); - cmd[1] = ADDR_7(reg); - cmd[2] = 0; - - transfer(cmd, cmd, sizeof(cmd)); - - return cmd[2]; -} - -void -FXOS8701CQ::write_reg(unsigned reg, uint8_t value) -{ - uint8_t cmd[3]; - - cmd[0] = DIR_WRITE(reg); - cmd[1] = ADDR_7(reg); - cmd[2] = value; - - transfer(cmd, nullptr, sizeof(cmd)); -} - void FXOS8701CQ::write_checked_reg(unsigned reg, uint8_t value) { @@ -318,18 +301,7 @@ FXOS8701CQ::RunImpl() perf_begin(_accel_sample_perf); // status register and data as read back from the device -#pragma pack(push, 1) - struct { - uint8_t cmd[2]; - uint8_t status; - int16_t x; - int16_t y; - int16_t z; - int16_t mx; - int16_t my; - int16_t mz; - } raw_accel_mag_report{}; -#pragma pack(pop) + RawAccelMagReport raw_accel_mag_report{}; check_registers(); @@ -342,10 +314,9 @@ FXOS8701CQ::RunImpl() } /* fetch data from the sensor */ - raw_accel_mag_report.cmd[0] = DIR_READ(FXOS8701CQ_DR_STATUS); - raw_accel_mag_report.cmd[1] = ADDR_7(FXOS8701CQ_DR_STATUS); const hrt_abstime timestamp_sample = hrt_absolute_time(); - transfer((uint8_t *)&raw_accel_mag_report, (uint8_t *)&raw_accel_mag_report, sizeof(raw_accel_mag_report)); + + _interface->read(FXOS8701CQ_DR_STATUS, (uint8_t *)&raw_accel_mag_report, sizeof(raw_accel_mag_report)); if (!(raw_accel_mag_report.status & DR_STATUS_ZYXDR)) { perf_end(_accel_sample_perf); diff --git a/src/drivers/imu/fxos8701cq/FXOS8701CQ.hpp b/src/drivers/imu/fxos8701cq/FXOS8701CQ.hpp index 7793af77ceb..64e98d693c7 100644 --- a/src/drivers/imu/fxos8701cq/FXOS8701CQ.hpp +++ b/src/drivers/imu/fxos8701cq/FXOS8701CQ.hpp @@ -34,17 +34,22 @@ /** * @file FXOS8701CQ.hpp * Driver for the NXP FXOS8701CQ 6-axis sensor with integrated linear accelerometer and - * magnetometer connected via SPI. + * magnetometer connected via SPI or I2C. */ #pragma once +#include #include #include #include #include #include +#if defined(PX4_I2C_FXOS8701CQ_ADDR) && defined(PX4_I2C_BUS_EXPANSION) +# define FXOS8701CQ_USE_I2C +#endif + #if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) #include #endif @@ -56,6 +61,10 @@ #define swap16(w) __builtin_bswap16((w)) #define swap16RightJustify14(w) (((int16_t)swap16(w)) >> 2) +// I2C address +#define FXOS8701CQ_REG_MASK 0x00FF +#define FXOS8701CQ_REG(r) ((r) & FXOS8701CQ_REG_MASK) + #define FXOS8701CQ_DR_STATUS 0x00 # define DR_STATUS_ZYXDR (1 << 3) @@ -103,6 +112,28 @@ #define FXOS8701C_ACCEL_DEFAULT_RANGE_G 8 #define FXOS8701C_ACCEL_DEFAULT_RATE 400 /* ODR is 400 in Hybird mode (accel + mag) */ +#pragma pack(push, 1) +struct RawAccelMagReport { +# ifdef FXOS8701CQ_USE_I2C + uint8_t cmd; +# else + uint8_t cmd[2]; +# endif + + uint8_t status; + int16_t x; + int16_t y; + int16_t z; + int16_t mx; + int16_t my; + int16_t mz; +}; +#pragma pack(pop) + +extern device::Device *FXOS8701CQ_SPI_interface(int bus, uint32_t chip_select, int bus_frequency, spi_mode_e spi_mode); +extern device::Device *FXOS8701CQ_I2C_interface(int bus, int bus_frequency, int i2c_address); + + /* we set the timer interrupt to run a bit faster than the desired sample rate and then throw away duplicates using the data ready bit. @@ -111,18 +142,17 @@ */ #define FXOS8701C_TIMER_REDUCTION 240 -class FXOS8701CQ : public device::SPI, public I2CSPIDriver +class FXOS8701CQ : public I2CSPIDriver { public: - FXOS8701CQ(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode); + FXOS8701CQ(device::Device *interface, I2CSPIBusOption bus_option, int bus, enum Rotation rotation, int i2c_address); virtual ~FXOS8701CQ(); static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, int runtime_instance); static void print_usage(); - int init() override; + int init(); void print_status() override; @@ -131,10 +161,11 @@ public: void print_registers(); void test_error(); protected: - int probe() override; + int probe(); void custom_method(const BusCLIArguments &cli) override; private: + device::Device *_interface; void start(); void reset(); @@ -150,15 +181,22 @@ private: * @param The register to read. * @return The value that was read. */ - uint8_t read_reg(unsigned reg); + inline uint8_t read_reg(unsigned reg) + { + return _interface->read_reg(reg); + } /** * Write a register in the FXOS8701C * * @param reg The register to write. * @param value The new value to write. + * @return OK on success, negative errno otherwise. */ - void write_reg(unsigned reg, uint8_t value); + inline int write_reg(unsigned reg, uint8_t value) + { + return _interface->write_reg(reg, value); + } /** * Modify a register in the FXOS8701C diff --git a/src/drivers/imu/fxos8701cq/fxos8701cq_i2c.cpp b/src/drivers/imu/fxos8701cq/fxos8701cq_i2c.cpp new file mode 100644 index 00000000000..73c1f82f3bc --- /dev/null +++ b/src/drivers/imu/fxos8701cq/fxos8701cq_i2c.cpp @@ -0,0 +1,202 @@ +/**************************************************************************** + * + * Copyright (c) 2016-2020 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. + * + ****************************************************************************/ + +/** + * @file fxos8701cq_i2c.cpp + * + * Driver for the NXP FXOS8701CQ connected via I2C. + * + * @author Robert Fu + */ + +#include "FXOS8701CQ.hpp" + +device::Device *FXOS8701CQ_I2C_interface(int bus, int bus_frequency, int i2c_address); + +class FXOS8701CQ_I2C : public device::I2C +{ +public: + FXOS8701CQ_I2C(int bus, int bus_frequency, int i2c_address); + ~FXOS8701CQ_I2C() override = default; + + /** + * Read directly from the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param reg The register address at which to start reading + * @param data The buffer into which the read values should be placed. + * @param count The number of items to read. + * @return The number of items read on success, negative errno otherwise. + */ + int read(unsigned reg, void *data, unsigned count) override; + + /** + * Write directly to the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param reg The register address at which to start writing. + * @param data The buffer from which values should be read. + * @param count The number of items to write. + * @return The number of items written on success, negative errno otherwise. + */ + int write(unsigned reg, void *data, unsigned count) override; + + /** + * Read a register from the device. + * + * @param The register to read. + * @return The value that was read. + */ + uint8_t read_reg(unsigned reg) override; + + /** + * Write a register in the device. + * + * @param reg The register to write. + * @param value The new value to write. + * @return OK on success, negative errno otherwise. + */ + int write_reg(unsigned reg, uint8_t value) override; + +protected: + int probe() override; +}; + +device::Device * +FXOS8701CQ_I2C_interface(int bus, int bus_frequency, int i2c_address) +{ + return new FXOS8701CQ_I2C(bus, bus_frequency, i2c_address); +} + +FXOS8701CQ_I2C::FXOS8701CQ_I2C(int bus, int bus_frequency, int i2c_address) : + I2C(DRV_ACC_DEVTYPE_FXOS8701C, MODULE_NAME, bus, i2c_address, bus_frequency) +{ +} + +int +FXOS8701CQ_I2C::probe() +{ + uint8_t whoami = read_reg(FXOS8701CQ_WHOAMI); + bool success = (whoami == FXOS8700CQ_WHOAMI_VAL) || (whoami == FXOS8701CQ_WHOAMI_VAL); + + return success ? OK : -EIO; +} + +/** + * Read directly from the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param reg The register address at which to start reading + * @param data The buffer into which the read values should be placed. + * @param count The number of items to read. + * @return The number of items read on success, negative errno otherwise. + */ +int FXOS8701CQ_I2C::read(unsigned reg, void *data, unsigned count) +{ + /* Same as in mpu9250_i2c.cpp: + * We want to avoid copying the data of RawAccelMagReport: So if the caller + * supplies a buffer not RawAccelMagReport in size, it is assume to be a reg or + * reg 16 read + * Since RawAccelMagReport has a cmd at front, we must return the data + * after that. Foe anthing else we must return it + */ + uint32_t offset = count < sizeof(RawAccelMagReport) ? 0 : offsetof(RawAccelMagReport, status); + uint8_t cmd = FXOS8701CQ_REG(reg); + + return transfer(&cmd, 1, &((uint8_t *)data)[offset], count - offset); +} + +/** + * Write directly to the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param reg The register address at which to start writing. + * @param data The buffer from which values should be read. + * @param count The number of items to write. + * @return The number of items written on success, negative errno otherwise. + */ +int FXOS8701CQ_I2C::write(unsigned reg, void *data, unsigned count) +{ + uint8_t cmd[2] {}; + + if (sizeof(cmd) < (count + 1)) { + // same as in mpu9250_i2c.cpp + // This condition means only supportting the case of count == 1 + // so this API is the same as write_reg + return -EIO; + } + + cmd[0] = FXOS8701CQ_REG(reg); + cmd[1] = *(uint8_t *)data; + + return transfer(cmd, sizeof(cmd), nullptr, 0); +} + +/** + * Read a register from the device. + * + * @param The register to read. + * @return The value that was read. + */ +uint8_t FXOS8701CQ_I2C::read_reg(unsigned reg) +{ + uint8_t cmd[1]; + uint8_t data[1]; + + cmd[0] = reg & 0x00FF; + + transfer(cmd, 1, data, 1); + + return data[0]; +} + +/** + * Write a register in the device. + * + * @param reg The register to write. + * @param value The new value to write. + * @return OK on success, negative errno otherwise. + */ +int FXOS8701CQ_I2C::write_reg(unsigned reg, uint8_t value) +{ + uint8_t cmd[2]; + + cmd[0] = reg & 0x00FF; + cmd[1] = value; + + return transfer(cmd, 2, nullptr, 0); +} diff --git a/src/drivers/imu/fxos8701cq/fxos8701cq_main.cpp b/src/drivers/imu/fxos8701cq/fxos8701cq_main.cpp index f72afb21e33..50866f8b233 100644 --- a/src/drivers/imu/fxos8701cq/fxos8701cq_main.cpp +++ b/src/drivers/imu/fxos8701cq/fxos8701cq_main.cpp @@ -48,7 +48,8 @@ FXOS8701CQ::print_usage() PRINT_MODULE_USAGE_NAME("fxos8701cq", "driver"); PRINT_MODULE_USAGE_SUBCATEGORY("imu"); PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x1E); PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); PRINT_MODULE_USAGE_COMMAND("regdump"); PRINT_MODULE_USAGE_COMMAND("testerror"); @@ -58,20 +59,34 @@ FXOS8701CQ::print_usage() I2CSPIDriverBase *FXOS8701CQ::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, int runtime_instance) { - FXOS8701CQ *instance = new FXOS8701CQ(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, - cli.bus_frequency, cli.spi_mode); + device::Device *interface = nullptr; - if (!instance) { + if (iterator.busType() == BOARD_I2C_BUS) { + interface = FXOS8701CQ_I2C_interface(iterator.bus(), cli.bus_frequency, cli.i2c_address); + + } else if (iterator.busType() == BOARD_SPI_BUS) { + interface = FXOS8701CQ_SPI_interface(iterator.bus(), iterator.devid(), cli.bus_frequency, cli.spi_mode); + } + + if (interface == nullptr) { PX4_ERR("alloc failed"); return nullptr; } - if (OK != instance->init()) { - delete instance; + FXOS8701CQ *dev = new FXOS8701CQ(interface, iterator.configuredBusOption(), iterator.bus(), cli.rotation, + cli.i2c_address); + + if (dev == nullptr) { + delete interface; return nullptr; } - return instance; + if (OK != dev->init()) { + delete dev; + return nullptr; + } + + return dev; } void FXOS8701CQ::custom_method(const BusCLIArguments &cli) @@ -88,9 +103,11 @@ extern "C" int fxos8701cq_main(int argc, char *argv[]) { int ch; using ThisDriver = FXOS8701CQ; - BusCLIArguments cli{false, true}; + BusCLIArguments cli{true, true}; + cli.default_i2c_frequency = 400 * 1000; cli.default_spi_frequency = 1 * 1000 * 1000; cli.spi_mode = SPIDEV_MODE0; + cli.i2c_address = 0x1E; while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { switch (ch) { diff --git a/src/drivers/imu/fxos8701cq/fxos8701cq_spi.cpp b/src/drivers/imu/fxos8701cq/fxos8701cq_spi.cpp new file mode 100644 index 00000000000..28c12b3f652 --- /dev/null +++ b/src/drivers/imu/fxos8701cq/fxos8701cq_spi.cpp @@ -0,0 +1,216 @@ +/**************************************************************************** + * + * Copyright (c) 2016-2020 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. + * + ****************************************************************************/ + +/** + * @file fxos8701cq_spi.cpp + * + * Driver for the NXP FXOS8701CQ connected via SPI. + * + * @author Robert Fu + */ + +#include "FXOS8701CQ.hpp" + +device::Device *FXOS8701CQ_SPI_interface(int bus, uint32_t chip_select, int bus_frequency, spi_mode_e spi_mode); + +class FXOS8701CQ_SPI : public device::SPI +{ +public: + FXOS8701CQ_SPI(int bus, uint32_t chip_select, int bus_frequency, spi_mode_e spi_mode); + ~FXOS8701CQ_SPI() override = default; + + /** + * Read directly from the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param reg The register address at which to start reading + * @param data The buffer into which the read values should be placed. + * @param count The number of items to read. + * @return The number of items read on success, negative errno otherwise. + */ + int read(unsigned reg, void *data, unsigned count) override; + + /** + * Write directly to the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param reg The register address at which to start writing. + * @param data The buffer from which values should be read. + * @param count The number of items to write. + * @return The number of items written on success, negative errno otherwise. + */ + int write(unsigned reg, void *data, unsigned count) override; + + /** + * Read a register from FXOS8701CQ + * + * @param The register to read. + * @return The value that was read. + */ + uint8_t read_reg(unsigned reg) override; + + /** + * Write a register FXOS8701CQ. + * + * @param reg The register to write. + * @param value The new value to write. + * @return OK on success, negative errno otherwise. + */ + int write_reg(unsigned reg, uint8_t value) override; + +protected: + int probe() override; +}; + +device::Device * +FXOS8701CQ_SPI_interface(int bus, uint32_t chip_select, int bus_frequency, spi_mode_e spi_mode) +{ + return new FXOS8701CQ_SPI(bus, chip_select, bus_frequency, spi_mode); +} + +FXOS8701CQ_SPI::FXOS8701CQ_SPI(int bus, uint32_t chip_select, int bus_frequency, spi_mode_e spi_mode) : + SPI(DRV_ACC_DEVTYPE_FXOS8701C, MODULE_NAME, bus, chip_select, spi_mode, bus_frequency) +{ +} + +int +FXOS8701CQ_SPI::probe() +{ + uint8_t whoami = read_reg(FXOS8701CQ_WHOAMI); + bool success = (whoami == FXOS8700CQ_WHOAMI_VAL) || (whoami == FXOS8701CQ_WHOAMI_VAL); + + DEVICE_DEBUG("FXAS21002C_SPI::probe: %s, whoami: 0x%02x", (success ? "Succeeded" : "failed"), whoami); + return success ? OK : -EIO; +} + +uint8_t +FXOS8701CQ_SPI::read_reg(unsigned reg) +{ + uint8_t cmd[3]; + + cmd[0] = DIR_READ(reg); + cmd[1] = ADDR_7(reg); + cmd[2] = 0; + + transfer(cmd, cmd, sizeof(cmd)); + + return cmd[2]; +} + +int +FXOS8701CQ_SPI::write_reg(unsigned reg, uint8_t value) +{ + uint8_t cmd[3]; + + cmd[0] = DIR_WRITE(reg); + cmd[1] = ADDR_7(reg); + cmd[2] = value; + + return transfer(cmd, nullptr, sizeof(cmd)); +} + +/** + * Read directly from the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param reg The register address at which to start reading + * @param data The buffer into which the read values should be placed. + * @param count The number of items to read. + * @return The number of items read on success, negative errno otherwise. + */ +int FXOS8701CQ_SPI::read(unsigned reg, void *data, unsigned count) +{ + /* Same as in mpu9250_spi.cpp: + * We want to avoid copying the data of RawAccelMagReport: So if the caller + * supplies a buffer not RawAccelMagReport in size, it is assume to be a reg or reg 16 read + * and we need to provied the buffer large enough for the callers data + * and our command. + */ + uint8_t cmd[4] {}; + + uint8_t *pBuf = count < sizeof(RawAccelMagReport) ? cmd : (uint8_t *) data ; + + if (count < sizeof(RawAccelMagReport)) { + /* add command */ + count += 2; + } + + /* Set command */ + pBuf[0] = DIR_READ(reg); + pBuf[1] = ADDR_7(reg); + + /* Transfer the command and get the data */ + int ret = transfer(pBuf, pBuf, count); + + if (ret == OK && pBuf == &cmd[0]) { + /* Adjust the count back */ + count -= 2; + + /* Return the data */ + memcpy(data, &cmd[2], count); + } + + return ret; +} + +/** + * Write directly to the device. + * + * The actual size of each unit quantity is device-specific. + * + * @param reg The register address at which to start writing. + * @param data The buffer from which values should be read. + * @param count The number of items to write. + * @return The number of items written on success, negative errno otherwise. + */ +int FXOS8701CQ_SPI::write(unsigned reg, void *data, unsigned count) +{ + uint8_t cmd[3] {}; + + if (sizeof(cmd) < (count + 1)) { + // same as in mpu9250_spi.cpp + // This condition means only supportting the case of count == 1 + // so this API is the same as write_reg + return -EIO; + } + + cmd[0] = DIR_WRITE(reg); + cmd[1] = ADDR_7(reg); + cmd[2] = *(uint8_t *)data; + + return transfer(cmd, nullptr, sizeof(cmd)); +} + diff --git a/src/drivers/imu/icm20948/icm20948_i2c.cpp b/src/drivers/imu/icm20948/icm20948_i2c.cpp index 60205f700cc..ba52505079d 100644 --- a/src/drivers/imu/icm20948/icm20948_i2c.cpp +++ b/src/drivers/imu/icm20948/icm20948_i2c.cpp @@ -55,7 +55,7 @@ public: int write(unsigned address, void *data, unsigned count) override; protected: - virtual int probe(); + virtual int probe() override; private: diff --git a/src/drivers/imu/l3gd20/L3GD20.cpp b/src/drivers/imu/l3gd20/L3GD20.cpp index 7cabc4c7281..efc5dade3e3 100644 --- a/src/drivers/imu/l3gd20/L3GD20.cpp +++ b/src/drivers/imu/l3gd20/L3GD20.cpp @@ -117,7 +117,7 @@ L3GD20::read_reg(unsigned reg) return cmd[1]; } -void +int L3GD20::write_reg(unsigned reg, uint8_t value) { uint8_t cmd[2] {}; @@ -125,7 +125,7 @@ L3GD20::write_reg(unsigned reg, uint8_t value) cmd[0] = reg | DIR_WRITE; cmd[1] = value; - transfer(cmd, nullptr, sizeof(cmd)); + return transfer(cmd, nullptr, sizeof(cmd)); } void diff --git a/src/drivers/imu/l3gd20/L3GD20.hpp b/src/drivers/imu/l3gd20/L3GD20.hpp index 81e2505cd01..88ee5d67720 100644 --- a/src/drivers/imu/l3gd20/L3GD20.hpp +++ b/src/drivers/imu/l3gd20/L3GD20.hpp @@ -236,15 +236,16 @@ private: * @param The register to read. * @return The value that was read. */ - uint8_t read_reg(unsigned reg); + uint8_t read_reg(unsigned reg) override; /** * Write a register in the L3GD20 * * @param reg The register to write. * @param value The new value to write. + * @return OK on success, negative errno otherwise. */ - void write_reg(unsigned reg, uint8_t value); + int write_reg(unsigned reg, uint8_t value) override; /** * Modify a register in the L3GD20 diff --git a/src/drivers/imu/lsm303d/LSM303D.cpp b/src/drivers/imu/lsm303d/LSM303D.cpp index 38be5e40831..414686d2dd8 100644 --- a/src/drivers/imu/lsm303d/LSM303D.cpp +++ b/src/drivers/imu/lsm303d/LSM303D.cpp @@ -164,7 +164,7 @@ LSM303D::read_reg(unsigned reg) return cmd[1]; } -void +int LSM303D::write_reg(unsigned reg, uint8_t value) { uint8_t cmd[2] {}; @@ -172,7 +172,7 @@ LSM303D::write_reg(unsigned reg, uint8_t value) cmd[0] = reg | DIR_WRITE; cmd[1] = value; - transfer(cmd, nullptr, sizeof(cmd)); + return transfer(cmd, nullptr, sizeof(cmd)); } void diff --git a/src/drivers/imu/lsm303d/LSM303D.hpp b/src/drivers/imu/lsm303d/LSM303D.hpp index 20268d33105..3687d4fc6ea 100644 --- a/src/drivers/imu/lsm303d/LSM303D.hpp +++ b/src/drivers/imu/lsm303d/LSM303D.hpp @@ -188,15 +188,16 @@ private: * @param The register to read. * @return The value that was read. */ - uint8_t read_reg(unsigned reg); + uint8_t read_reg(unsigned reg) override; /** * Write a register in the LSM303D * * @param reg The register to write. * @param value The new value to write. + * @return OK on success, negative errno otherwise. */ - void write_reg(unsigned reg, uint8_t value); + int write_reg(unsigned reg, uint8_t value) override; /** * Modify a register in the LSM303D diff --git a/src/drivers/imu/mpu9250/mpu9250_spi.cpp b/src/drivers/imu/mpu9250/mpu9250_spi.cpp index e3bce98ac2d..e86594b735f 100644 --- a/src/drivers/imu/mpu9250/mpu9250_spi.cpp +++ b/src/drivers/imu/mpu9250/mpu9250_spi.cpp @@ -96,7 +96,7 @@ MPU9250_SPI::set_bus_frequency(unsigned ®_speed) /* Set the desired speed */ set_frequency(MPU9250_IS_HIGH_SPEED(reg_speed) ? MPU9250_HIGH_SPI_BUS_SPEED : MPU9250_LOW_SPI_BUS_SPEED); - /* Isoolate the register on return */ + /* Isolate the register on return */ reg_speed = MPU9250_REG(reg_speed); } diff --git a/src/drivers/magnetometer/ak09916/ak09916.cpp b/src/drivers/magnetometer/ak09916/ak09916.cpp index 28d9a6a6d06..a612e85bf93 100644 --- a/src/drivers/magnetometer/ak09916/ak09916.cpp +++ b/src/drivers/magnetometer/ak09916/ak09916.cpp @@ -170,11 +170,11 @@ AK09916::check_id() return (AK09916_DEVICE_ID_A == deviceid); } -void +int AK09916::write_reg(uint8_t reg, uint8_t value) { const uint8_t cmd[2] = { reg, value}; - transfer(cmd, 2, nullptr, 0); + return transfer(cmd, 2, nullptr, 0); } int diff --git a/src/drivers/magnetometer/ak09916/ak09916.hpp b/src/drivers/magnetometer/ak09916/ak09916.hpp index 26446c00b7c..3c453b08b78 100644 --- a/src/drivers/magnetometer/ak09916/ak09916.hpp +++ b/src/drivers/magnetometer/ak09916/ak09916.hpp @@ -105,7 +105,7 @@ protected: uint8_t read_reg(uint8_t reg); void read_block(uint8_t reg, uint8_t *val, uint8_t count); - void write_reg(uint8_t reg, uint8_t value); + int write_reg(uint8_t reg, uint8_t value); private: diff --git a/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp b/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp index 1d12aa927fc..6beafd64250 100644 --- a/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp +++ b/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp @@ -233,13 +233,13 @@ uint8_t LSM303AGR::read_reg(unsigned reg) return cmd[1]; } -void LSM303AGR::write_reg(unsigned reg, uint8_t value) +int LSM303AGR::write_reg(unsigned reg, uint8_t value) { uint8_t cmd[2]; cmd[0] = reg | DIR_WRITE; cmd[1] = value; - transfer(cmd, nullptr, sizeof(cmd)); + return transfer(cmd, nullptr, sizeof(cmd)); } void LSM303AGR::start() diff --git a/src/drivers/magnetometer/lsm303agr/LSM303AGR.hpp b/src/drivers/magnetometer/lsm303agr/LSM303AGR.hpp index be2cc38e4bf..857abc827a0 100644 --- a/src/drivers/magnetometer/lsm303agr/LSM303AGR.hpp +++ b/src/drivers/magnetometer/lsm303agr/LSM303AGR.hpp @@ -152,13 +152,14 @@ private: * @param The register to read. * @return The value that was read. */ - uint8_t read_reg(unsigned reg); + uint8_t read_reg(unsigned reg) override; /** * Write a register in the LSM303AGR * * @param reg The register to write. * @param value The new value to write. + * @return OK on success, negative errno otherwise. */ - void write_reg(unsigned reg, uint8_t value); + int write_reg(unsigned reg, uint8_t value) override; }; diff --git a/src/lib/drivers/device/Device.hpp b/src/lib/drivers/device/Device.hpp index 9245ff63b66..6179437e0d2 100644 --- a/src/lib/drivers/device/Device.hpp +++ b/src/lib/drivers/device/Device.hpp @@ -114,6 +114,23 @@ public: */ virtual int write(unsigned address, void *data, unsigned count) { return -ENODEV; } + /** + * Read a register from the device. + * + * @param The register to read. + * @return The value that was read. + */ + virtual uint8_t read_reg(unsigned reg) { return -ENODEV; } + + /** + * Write a register in the device. + * + * @param reg The register to write. + * @param value The new value to write. + * @return OK on success, negative errno otherwise. + */ + virtual int write_reg(unsigned reg, uint8_t value) { return -ENODEV; } + /** * Perform a device-specific operation. *