diff --git a/boards/aerotenna/ocpoc/src/board_config.h b/boards/aerotenna/ocpoc/src/board_config.h index 3bc62c4f0a..ca07365f2a 100644 --- a/boards/aerotenna/ocpoc/src/board_config.h +++ b/boards/aerotenna/ocpoc/src/board_config.h @@ -56,5 +56,9 @@ #define ADC_BATTERY_CURRENT_CHANNEL ((uint8_t)(-1)) #define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 +// SPI +#define PX4_SPI_BUS_SENSORS 1 +#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 1) + #include #include diff --git a/boards/aerotenna/ocpoc/ubuntu.cmake b/boards/aerotenna/ocpoc/ubuntu.cmake index f5a9b2bf2f..940c9c35fb 100644 --- a/boards/aerotenna/ocpoc/ubuntu.cmake +++ b/boards/aerotenna/ocpoc/ubuntu.cmake @@ -16,6 +16,7 @@ px4_add_board( distance_sensor # all available distance sensor drivers gps #imu # all available imu drivers + imu/mpu9250 lights/rgbled linux_pwm_out linux_sbus diff --git a/boards/atlflight/eagle/qurt-default.cmake b/boards/atlflight/eagle/qurt-default.cmake index 1b9ca0f585..49440e4bbf 100644 --- a/boards/atlflight/eagle/qurt-default.cmake +++ b/boards/atlflight/eagle/qurt-default.cmake @@ -46,6 +46,7 @@ px4_add_board( DRIVERS gps + imu/mpu9250 spektrum_rc qshell/qurt snapdragon_pwm_out diff --git a/boards/atlflight/eagle/src/board_config.h b/boards/atlflight/eagle/src/board_config.h index d2ea4312fb..59a7252c22 100644 --- a/boards/atlflight/eagle/src/board_config.h +++ b/boards/atlflight/eagle/src/board_config.h @@ -56,5 +56,9 @@ #define PX4_I2C_BUS_LED 3 #define PX4_NUMBER_I2C_BUSES 3 +// SPI +#define PX4_SPI_BUS_SENSORS 1 +#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 1) + #include #include diff --git a/boards/atlflight/excelsior/qurt-default.cmake b/boards/atlflight/excelsior/qurt-default.cmake index acbd5099b9..25bc791c79 100644 --- a/boards/atlflight/excelsior/qurt-default.cmake +++ b/boards/atlflight/excelsior/qurt-default.cmake @@ -46,6 +46,7 @@ px4_add_board( DRIVERS gps + imu/mpu9250 spektrum_rc qshell/qurt snapdragon_pwm_out diff --git a/boards/beaglebone/blue/cross.cmake b/boards/beaglebone/blue/cross.cmake index ae19a81487..6aa1b5dd90 100644 --- a/boards/beaglebone/blue/cross.cmake +++ b/boards/beaglebone/blue/cross.cmake @@ -16,6 +16,7 @@ px4_add_board( distance_sensor # all available distance sensor drivers gps #imu # all available imu drivers + imu/mpu9250 #magnetometer # all available magnetometer drivers pwm_out_sim #telemetry # all available telemetry drivers diff --git a/boards/beaglebone/blue/native.cmake b/boards/beaglebone/blue/native.cmake index 978de48660..ec4895ce65 100644 --- a/boards/beaglebone/blue/native.cmake +++ b/boards/beaglebone/blue/native.cmake @@ -14,6 +14,7 @@ px4_add_board( distance_sensor # all available distance sensor drivers gps #imu # all available imu drivers + imu/mpu9250 #magnetometer # all available magnetometer drivers pwm_out_sim #telemetry # all available telemetry drivers diff --git a/boards/emlid/navio2/cross.cmake b/boards/emlid/navio2/cross.cmake index a72074b59e..bd9c9ee19c 100644 --- a/boards/emlid/navio2/cross.cmake +++ b/boards/emlid/navio2/cross.cmake @@ -16,6 +16,7 @@ px4_add_board( distance_sensor # all available distance sensor drivers gps #imu # all available imu drivers + imu/mpu9250 #magnetometer # all available magnetometer drivers pwm_out_sim #telemetry # all available telemetry drivers diff --git a/boards/emlid/navio2/native.cmake b/boards/emlid/navio2/native.cmake index d001994211..de6a14d190 100644 --- a/boards/emlid/navio2/native.cmake +++ b/boards/emlid/navio2/native.cmake @@ -14,6 +14,7 @@ px4_add_board( distance_sensor # all available distance sensor drivers gps #imu # all available imu drivers + imu/mpu9250 #magnetometer # all available magnetometer drivers pwm_out_sim #telemetry # all available telemetry drivers diff --git a/boards/emlid/navio2/src/board_config.h b/boards/emlid/navio2/src/board_config.h index 6088a55f59..29e825fdb2 100644 --- a/boards/emlid/navio2/src/board_config.h +++ b/boards/emlid/navio2/src/board_config.h @@ -56,8 +56,12 @@ /* * I2C busses */ -#define PX4_I2C_BUS_EXPANSION 1 -#define PX4_NUMBER_I2C_BUSES 1 +#define PX4_I2C_BUS_EXPANSION 1 +#define PX4_NUMBER_I2C_BUSES 1 + +// SPI +#define PX4_SPI_BUS_SENSORS 0 +#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 1) #include #include diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp index a252e37f64..49f619d165 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp @@ -50,17 +50,19 @@ namespace wq_configurations { static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 1600, 0}; // PX4 inner loop highest priority -static constexpr wq_config_t SPI1{"wq:SPI1", 1400, -1}; -static constexpr wq_config_t SPI2{"wq:SPI2", 1400, -2}; -static constexpr wq_config_t SPI3{"wq:SPI3", 1400, -3}; -static constexpr wq_config_t SPI4{"wq:SPI4", 1400, -4}; -static constexpr wq_config_t SPI5{"wq:SPI5", 1400, -5}; -static constexpr wq_config_t SPI6{"wq:SPI6", 1400, -6}; +static constexpr wq_config_t SPI0{"wq:SPI0", 1400, -1}; +static constexpr wq_config_t SPI1{"wq:SPI1", 1400, -2}; +static constexpr wq_config_t SPI2{"wq:SPI2", 1400, -3}; +static constexpr wq_config_t SPI3{"wq:SPI3", 1400, -4}; +static constexpr wq_config_t SPI4{"wq:SPI4", 1400, -5}; +static constexpr wq_config_t SPI5{"wq:SPI5", 1400, -6}; +static constexpr wq_config_t SPI6{"wq:SPI6", 1400, -7}; -static constexpr wq_config_t I2C1{"wq:I2C1", 1250, -7}; -static constexpr wq_config_t I2C2{"wq:I2C2", 1250, -8}; -static constexpr wq_config_t I2C3{"wq:I2C3", 1250, -9}; -static constexpr wq_config_t I2C4{"wq:I2C4", 1250, -10}; +static constexpr wq_config_t I2C0{"wq:I2C0", 1400, -8}; +static constexpr wq_config_t I2C1{"wq:I2C1", 1400, -9}; +static constexpr wq_config_t I2C2{"wq:I2C2", 1400, -10}; +static constexpr wq_config_t I2C3{"wq:I2C3", 1400, -11}; +static constexpr wq_config_t I2C4{"wq:I2C4", 1400, -12}; static constexpr wq_config_t att_pos_ctrl{"wq:att_pos_ctrl", 6600, -11}; // PX4 att/pos controllers, highest priority after sensors diff --git a/platforms/common/px4_work_queue/WorkQueueManager.cpp b/platforms/common/px4_work_queue/WorkQueueManager.cpp index 77cc48103b..5f7c733aea 100644 --- a/platforms/common/px4_work_queue/WorkQueueManager.cpp +++ b/platforms/common/px4_work_queue/WorkQueueManager.cpp @@ -129,6 +129,8 @@ device_bus_to_wq(uint32_t device_id_int) if (bus_type == device::Device::DeviceBusType_I2C) { switch (bus) { + case 0: return wq_configurations::I2C0; + case 1: return wq_configurations::I2C1; case 2: return wq_configurations::I2C2; @@ -140,6 +142,8 @@ device_bus_to_wq(uint32_t device_id_int) } else if (bus_type == device::Device::DeviceBusType_SPI) { switch (bus) { + case 0: return wq_configurations::SPI0; + case 1: return wq_configurations::SPI1; case 2: return wq_configurations::SPI2; diff --git a/src/drivers/imu/icm20948/ICM20948_mag.cpp b/src/drivers/imu/icm20948/ICM20948_mag.cpp index 84690d54ad..240e9b11fa 100644 --- a/src/drivers/imu/icm20948/ICM20948_mag.cpp +++ b/src/drivers/imu/icm20948/ICM20948_mag.cpp @@ -34,7 +34,7 @@ /** * @file mag.cpp * - * Driver for the ak8963 magnetometer within the Invensense mpu9250 + * Driver for the ak09916 magnetometer within the Invensense icm20948 * * @author Robert Dickenson * @@ -56,80 +56,84 @@ ICM20948_mag::ICM20948_mag(ICM20948 *parent, device::Device *interface, enum Rot _px4_mag(parent->_interface->get_device_id(), (parent->_interface->external() ? ORB_PRIO_MAX : ORB_PRIO_HIGH), rotation), _parent(parent), - _mag_reads(perf_alloc(PC_COUNT, "icm20948: mag_reads")), - _mag_errors(perf_alloc(PC_COUNT, "icm20948: mag_errors")), - _mag_overruns(perf_alloc(PC_COUNT, "icm20948: mag_overruns")), - _mag_overflows(perf_alloc(PC_COUNT, "icm20948: mag_overflows")), - _mag_duplicates(perf_alloc(PC_COUNT, "icm20948: mag_duplicates")) + _mag_overruns(perf_alloc(PC_COUNT, MODULE_NAME": mag_overruns")), + _mag_overflows(perf_alloc(PC_COUNT, MODULE_NAME": mag_overflows")), + _mag_errors(perf_alloc(PC_COUNT, MODULE_NAME": mag_errors")) { _px4_mag.set_device_type(DRV_MAG_DEVTYPE_AK09916); - _px4_mag.set_scale(MPU9250_MAG_RANGE_GA); + _px4_mag.set_scale(ICM20948_MAG_RANGE_GA); } ICM20948_mag::~ICM20948_mag() { - perf_free(_mag_reads); - perf_free(_mag_errors); perf_free(_mag_overruns); perf_free(_mag_overflows); - perf_free(_mag_duplicates); -} - -bool ICM20948_mag::check_duplicate(uint8_t *mag_data) -{ - if (memcmp(mag_data, &_last_mag_data, sizeof(_last_mag_data)) == 0) { - // it isn't new data - wait for next timer - return true; - - } else { - memcpy(&_last_mag_data, mag_data, sizeof(_last_mag_data)); - return false; - } + perf_free(_mag_errors); } void ICM20948_mag::measure() { - union raw_data_t { - struct ak8963_regs ak8963_data; - struct ak09916_regs ak09916_data; - } raw_data; - - const hrt_abstime timestamp_sample = hrt_absolute_time(); - int ret = _interface->read(AK09916REG_ST1, &raw_data, sizeof(struct ak09916_regs)); - if (ret == OK) { - raw_data.ak8963_data.st2 = raw_data.ak09916_data.st2; + uint8_t st1 = 0; + int ret = _interface->read(AK09916REG_ST1, &st1, sizeof(st1)); - _measure(timestamp_sample, raw_data.ak8963_data); - } -} - -void -ICM20948_mag::_measure(hrt_abstime timestamp_sample, ak8963_regs data) -{ - if (check_duplicate((uint8_t *)&data.x) && !(data.st1 & 0x02)) { - perf_count(_mag_duplicates); + if (ret != OK) { + _px4_mag.set_error_count(perf_event_count(_mag_errors)); return; } - /* monitor for if data overrun flag is ever set */ - if (data.st1 & 0x02) { + /* Check if data ready is set. + * This is not described to be set in continuous mode according to the + * MPU9250 datasheet. However, the datasheet of the 8963 recommends to + * check data ready before doing the read and before triggering the + * next measurement by reading ST2. */ + if (!(st1 & AK09916_ST1_DRDY)) { + return; + } + + /* Monitor if data overrun flag is ever set. */ + if (st1 & 0x02) { perf_count(_mag_overruns); } - /* monitor for if magnetic sensor overflow flag is ever set noting that st2 - * is usually not even refreshed, but will always be in the same place in the - * mpu's buffers regardless, hence the actual count would be bogus - */ + ak09916_regs data{}; + ret = _interface->read(AK09916REG_ST1, &data, sizeof(data)); + + if (ret != OK) { + _px4_mag.set_error_count(perf_event_count(_mag_errors)); + return; + } + + /* Monitor magnetic sensor overflow flag. */ if (data.st2 & 0x08) { perf_count(_mag_overflows); } + _measure(timestamp_sample, data); +} + +void +ICM20948_mag::_measure(hrt_abstime timestamp_sample, ak09916_regs data) +{ + /* Check if data ready is set. + * This is not described to be set in continuous mode according to the + * MPU9250 datasheet. However, the datasheet of the 8963 recommends to + * check data ready before doing the read and before triggering the + * next measurement by reading ST2. + * + * If _measure is used in passthrough mode, all the data is already + * fetched, however, we should still not use the data if the data ready + * is not set. This has lead to intermittent spikes when the data was + * being updated while getting read. + */ + if (!(data.st1 & AK09916_ST1_DRDY)) { + return; + } + _px4_mag.set_external(_parent->is_external()); _px4_mag.set_temperature(_parent->_last_temperature); - _px4_mag.set_error_count(perf_event_count(_mag_errors)); /* * Align axes - Keeping consistent with the accel and gyro axes of the ICM20948 here, just aligning the magnetometer to them. @@ -146,10 +150,10 @@ ICM20948_mag::set_passthrough(uint8_t reg, uint8_t size, uint8_t *out) if (out) { _parent->write_reg(ICMREG_20948_I2C_SLV0_DO, *out); - addr = AK8963_I2C_ADDR; + addr = AK09916_I2C_ADDR; } else { - addr = AK8963_I2C_ADDR | BIT_I2C_READ_FLAG; + addr = AK09916_I2C_ADDR | BIT_I2C_READ_FLAG; } _parent->write_reg(ICMREG_20948_I2C_SLV0_ADDR, addr); @@ -188,11 +192,11 @@ ICM20948_mag::read_reg(unsigned int reg) } bool -ICM20948_mag::ak8963_check_id(uint8_t &deviceid) +ICM20948_mag::ak09916_check_id(uint8_t &deviceid) { - deviceid = read_reg(AK8963REG_WIA); + deviceid = read_reg(AK09916REG_WIA); - return (AK8963_DEVICE_ID == deviceid); + return (AK09916_DEVICE_ID == deviceid); } /* @@ -214,61 +218,61 @@ ICM20948_mag::write_reg(unsigned reg, uint8_t value) passthrough_write(reg, value); } else { - _interface->write(MPU9250_LOW_SPEED_OP(reg), &value, 1); + _interface->write(ICM20948_LOW_SPEED_OP(reg), &value, 1); } } int -ICM20948_mag::ak8963_reset(void) +ICM20948_mag::ak09916_reset() { // First initialize it to use the bus - int rv = ak8963_setup(); + int rv = ak09916_setup(); if (rv == OK) { // Now reset the mag - write_reg(AK09916REG_CNTL3, AK8963_RESET); + write_reg(AK09916REG_CNTL3, AK09916_RESET); // Then re-initialize the bus/mag - rv = ak8963_setup(); + rv = ak09916_setup(); } return rv; } bool -ICM20948_mag::ak8963_read_adjustments(void) +ICM20948_mag::ak09916_read_adjustments() { uint8_t response[3]; - float ak8963_ASA[3]; + float ak09916_ASA[3]; - write_reg(AK8963REG_CNTL1, AK8963_FUZE_MODE | AK8963_16BIT_ADC); + write_reg(AK09916REG_CNTL1, AK09916_FUZE_MODE | AK09916_16BIT_ADC); px4_usleep(50); if (_interface != nullptr) { - _interface->read(AK8963REG_ASAX, response, 3); + _interface->read(AK09916REG_ASAX, response, 3); } else { - passthrough_read(AK8963REG_ASAX, response, 3); + passthrough_read(AK09916REG_ASAX, response, 3); } - write_reg(AK8963REG_CNTL1, AK8963_POWERDOWN_MODE); + write_reg(AK09916REG_CNTL1, AK09916_POWERDOWN_MODE); for (int i = 0; i < 3; i++) { if (0 != response[i] && 0xff != response[i]) { - ak8963_ASA[i] = ((float)(response[i] - 128) / 256.0f) + 1.0f; + ak09916_ASA[i] = ((float)(response[i] - 128) / 256.0f) + 1.0f; } else { return false; } } - _px4_mag.set_sensitivity(ak8963_ASA[0], ak8963_ASA[1], ak8963_ASA[2]); + _px4_mag.set_sensitivity(ak09916_ASA[0], ak09916_ASA[1], ak09916_ASA[2]); return true; } int -ICM20948_mag::ak8963_setup_master_i2c(void) +ICM20948_mag::ak09916_setup_master_i2c() { /* When _interface is null we are using SPI and must * use the parent interface to configure the device to act @@ -288,29 +292,29 @@ ICM20948_mag::ak8963_setup_master_i2c(void) return OK; } int -ICM20948_mag::ak8963_setup(void) +ICM20948_mag::ak09916_setup(void) { int retries = 10; do { - ak8963_setup_master_i2c(); - write_reg(AK09916REG_CNTL3, AK8963_RESET); + ak09916_setup_master_i2c(); + write_reg(AK09916REG_CNTL3, AK09916_RESET); uint8_t id = 0; - if (ak8963_check_id(id)) { + if (ak09916_check_id(id)) { break; } retries--; - PX4_WARN("AK8963: bad id %d retries %d", id, retries); + PX4_WARN("AK09916: bad id %d retries %d", id, retries); _parent->modify_reg(ICMREG_20948_USER_CTRL, 0, BIT_I2C_MST_RST); up_udelay(100); } while (retries > 0); if (retries == 0) { - PX4_ERR("AK8963: failed to initialize, disabled!"); + PX4_ERR("AK09916: failed to initialize, disabled!"); _parent->modify_checked_reg(ICMREG_20948_USER_CTRL, BIT_I2C_MST_EN, 0); _parent->write_reg(ICMREG_20948_I2C_MST_CTRL, 0); return -EIO; @@ -318,13 +322,9 @@ ICM20948_mag::ak8963_setup(void) write_reg(AK09916REG_CNTL2, AK09916_CNTL2_CONTINOUS_MODE_100HZ); - if (_interface == NULL) { - - /* Configure mpu' I2c Master interface to read ak8963 data - * Into to fifo - */ - - set_passthrough(AK09916REG_ST1, sizeof(struct ak09916_regs)); + if (_interface == nullptr) { + // Configure mpu' I2c Master interface to read ak09916 data into to fifo + set_passthrough(AK09916REG_ST1, sizeof(ak09916_regs)); } return OK; diff --git a/src/drivers/imu/icm20948/ICM20948_mag.h b/src/drivers/imu/icm20948/ICM20948_mag.h index bfb580ed6d..7b3cdc9b1e 100644 --- a/src/drivers/imu/icm20948/ICM20948_mag.h +++ b/src/drivers/imu/icm20948/ICM20948_mag.h @@ -38,38 +38,31 @@ #include /* in 16-bit sampling mode the mag resolution is 1.5 milli Gauss per bit */ -static constexpr float MPU9250_MAG_RANGE_GA{1.5e-3f}; +static constexpr float ICM20948_MAG_RANGE_GA{1.5e-3f}; -/* we are using the continuous fixed sampling rate of 100Hz */ +#define ICM20948_AK09916_SAMPLE_RATE 100 -#define MPU9250_AK8963_SAMPLE_RATE 100 +#define AK09916_I2C_ADDR 0x0C +#define AK09916_DEVICE_ID 0x48 -/* ak8963 register address and bit definitions */ +#define AK09916REG_WIA 0x00 +#define AK09916REG_CNTL1 0x0A +#define AK09916REG_ASAX 0x10 -#define AK8963_I2C_ADDR 0x0C -#define AK8963_DEVICE_ID 0x48 - -#define AK8963REG_WIA 0x00 -#define AK8963REG_ST1 0x02 -#define AK8963REG_HXL 0x03 -#define AK8963REG_ASAX 0x10 -#define AK8963REG_CNTL1 0x0A -#define AK8963REG_CNTL2 0x0B - -#define AK8963_SINGLE_MEAS_MODE 0x01 -#define AK8963_CONTINUOUS_MODE1 0x02 -#define AK8963_CONTINUOUS_MODE2 0x06 -#define AK8963_POWERDOWN_MODE 0x00 -#define AK8963_SELFTEST_MODE 0x08 -#define AK8963_FUZE_MODE 0x0F -#define AK8963_16BIT_ADC 0x10 -#define AK8963_14BIT_ADC 0x00 -#define AK8963_RESET 0x01 -#define AK8963_HOFL 0x08 +#define AK09916_SINGLE_MEAS_MODE 0x01 +#define AK09916_CONTINUOUS_MODE1 0x02 +#define AK09916_CONTINUOUS_MODE2 0x06 +#define AK09916_POWERDOWN_MODE 0x00 +#define AK09916_SELFTEST_MODE 0x08 +#define AK09916_FUZE_MODE 0x0F +#define AK09916_16BIT_ADC 0x10 +#define AK09916_14BIT_ADC 0x00 +#define AK09916_RESET 0x01 +#define AK09916_HOFL 0x08 /* ak09916 deviating register addresses and bit definitions */ -#define AK09916_DEVICE_ID_A 0x48 // same as AK8963 +#define AK09916_DEVICE_ID_A 0x48 // same as AK09916 #define AK09916_DEVICE_ID_B 0x09 // additional ID byte ("INFO" on AK9063 without content specification.) #define AK09916REG_HXL 0x11 @@ -95,19 +88,8 @@ static constexpr float MPU9250_MAG_RANGE_GA{1.5e-3f}; #define AK09916_ST1_DRDY 0x01 #define AK09916_ST1_DOR 0x02 - class ICM20948; -#pragma pack(push, 1) -struct ak8963_regs { - uint8_t st1; - int16_t x; - int16_t y; - int16_t z; - uint8_t st2; -}; -#pragma pack(pop) - #pragma pack(push, 1) struct ak09916_regs { uint8_t st1; @@ -119,12 +101,10 @@ struct ak09916_regs { }; #pragma pack(pop) - -extern device::Device *AK8963_I2C_interface(int bus, bool external_bus); +extern device::Device *AK09916_I2C_interface(int bus); typedef device::Device *(*ICM20948_mag_constructor)(int, bool); - /** * Helper class implementing the magnetometer driver node. */ @@ -139,11 +119,11 @@ public: void passthrough_write(uint8_t reg, uint8_t val); void read_block(uint8_t reg, uint8_t *val, uint8_t count); - int ak8963_reset(void); - int ak8963_setup(void); - int ak8963_setup_master_i2c(void); - bool ak8963_check_id(uint8_t &id); - bool ak8963_read_adjustments(void); + int ak09916_reset(); + int ak09916_setup(); + int ak09916_setup_master_i2c(); + bool ak09916_check_id(uint8_t &id); + bool ak09916_read_adjustments(); void print_status() { _px4_mag.print_status(); } @@ -152,11 +132,8 @@ protected: friend class ICM20948; - /* Directly measure from the _interface if possible */ void measure(); - - /* Update the state with prefetched data (internally called by the regular measure() )*/ - void _measure(hrt_abstime timestamp, struct ak8963_regs data); + void _measure(hrt_abstime timestamp_sample, ak09916_regs data); uint8_t read_reg(unsigned reg); void write_reg(unsigned reg, uint8_t value); @@ -164,25 +141,14 @@ protected: bool is_passthrough() { return _interface == nullptr; } private: - PX4Magnetometer _px4_mag; ICM20948 *_parent; bool _mag_reading_data{false}; - perf_counter_t _mag_reads; - perf_counter_t _mag_errors; perf_counter_t _mag_overruns; perf_counter_t _mag_overflows; - perf_counter_t _mag_duplicates; + perf_counter_t _mag_errors; - bool check_duplicate(uint8_t *mag_data); - - // keep last mag reading for duplicate detection - uint8_t _last_mag_data[6] {}; - - /* do not allow to copy this class due to pointer data members */ - ICM20948_mag(const ICM20948_mag &); - ICM20948_mag operator=(const ICM20948_mag &); }; diff --git a/src/drivers/imu/icm20948/icm20948.cpp b/src/drivers/imu/icm20948/icm20948.cpp index 7314acfe51..81df4f663f 100644 --- a/src/drivers/imu/icm20948/icm20948.cpp +++ b/src/drivers/imu/icm20948/icm20948.cpp @@ -32,12 +32,12 @@ ****************************************************************************/ /** - * @file mpu9250.cpp + * @file icm20948.cpp * * Driver for the Invensense ICM20948 connected via I2C or SPI. * * - * based on the mpu9250 driver + * based on the icm20948 driver */ #include @@ -59,7 +59,7 @@ accelerometer values. This time reduction is enough to cope with worst case timing jitter due to other timers */ -#define MPU9250_TIMER_REDUCTION 200 +#define ICM20948_TIMER_REDUCTION 200 /* Set accel range used */ #define ACCEL_RANGE_G 16 @@ -84,26 +84,21 @@ const uint16_t ICM20948::_icm20948_checked_registers[ICM20948_NUM_CHECKED_REGIST ICMREG_20948_ACCEL_CONFIG_2 }; -ICM20948::ICM20948(device::Device *interface, device::Device *mag_interface, const char *path, enum Rotation rotation, - bool magnetometer_only) : +ICM20948::ICM20948(device::Device *interface, device::Device *mag_interface, enum Rotation rotation) : ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())), _interface(interface), _px4_accel(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_DEFAULT : ORB_PRIO_HIGH), rotation), _px4_gyro(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_DEFAULT : ORB_PRIO_HIGH), rotation), _mag(this, mag_interface, rotation), _selected_bank(0xFF), // invalid/improbable bank value, will be set on first read/write - _magnetometer_only(magnetometer_only), - _dlpf_freq(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ), - _dlpf_freq_icm_gyro(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ), - _dlpf_freq_icm_accel(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ), - _accel_reads(perf_alloc(PC_COUNT, "icm20948: acc_read")), - _gyro_reads(perf_alloc(PC_COUNT, "icm20948: gyro_read")), - _sample_perf(perf_alloc(PC_ELAPSED, "icm20948: read")), - _bad_transfers(perf_alloc(PC_COUNT, "icm20948: bad_trans")), - _bad_registers(perf_alloc(PC_COUNT, "icm20948: bad_reg")), - _good_transfers(perf_alloc(PC_COUNT, "icm20948: good_trans")), - _reset_retries(perf_alloc(PC_COUNT, "icm20948: reset")), - _duplicates(perf_alloc(PC_COUNT, "icm20948: dupe")) + _dlpf_freq(ICM20948_DEFAULT_ONCHIP_FILTER_FREQ), + _dlpf_freq_icm_gyro(ICM20948_DEFAULT_ONCHIP_FILTER_FREQ), + _dlpf_freq_icm_accel(ICM20948_DEFAULT_ONCHIP_FILTER_FREQ), + _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), + _bad_transfers(perf_alloc(PC_COUNT, MODULE_NAME": bad_trans")), + _bad_registers(perf_alloc(PC_COUNT, MODULE_NAME": bad_reg")), + _good_transfers(perf_alloc(PC_COUNT, MODULE_NAME": good_trans")), + _duplicates(perf_alloc(PC_COUNT, MODULE_NAME": dupe")) { _px4_accel.set_device_type(DRV_DEVTYPE_ICM20948); _px4_gyro.set_device_type(DRV_DEVTYPE_ICM20948); @@ -116,12 +111,10 @@ ICM20948::~ICM20948() // delete the perf counter perf_free(_sample_perf); - perf_free(_accel_reads); - perf_free(_gyro_reads); + perf_free(_interval_perf); perf_free(_bad_transfers); perf_free(_bad_registers); perf_free(_good_transfers); - perf_free(_reset_retries); perf_free(_duplicates); } @@ -135,7 +128,7 @@ ICM20948::init() */ const bool is_i2c = (_interface->get_device_bus_type() == device::Device::DeviceBusType_I2C); - if (is_i2c && !_magnetometer_only) { + if (is_i2c) { _sample_rate = 200; } @@ -159,12 +152,12 @@ ICM20948::init() px4_usleep(100); if (!_mag.is_passthrough() && _mag._interface->init() != PX4_OK) { - PX4_ERR("failed to setup ak8963 interface"); + PX4_ERR("failed to setup ak09916 interface"); } #endif /* USE_I2C */ - ret = _mag.ak8963_reset(); + ret = _mag.ak09916_reset(); if (ret != OK) { PX4_DEBUG("mag reset failed"); @@ -178,13 +171,12 @@ ICM20948::init() int ICM20948::reset() { - /* When the mpu9250 starts from 0V the internal power on circuit + /* When the icm20948 starts from 0V the internal power on circuit * per the data sheet will require: * * Start-up time for register read/write From power-up Typ:11 max:100 ms * */ - px4_usleep(110000); // Hold off sampling until done (100 MS will be shortened) @@ -193,7 +185,7 @@ int ICM20948::reset() int ret = reset_mpu(); if (ret == OK && (_whoami == ICM_WHOAMI_20948)) { - ret = _mag.ak8963_reset(); + ret = _mag.ak09916_reset(); } _reset_wait = hrt_absolute_time() + 10; @@ -201,17 +193,16 @@ int ICM20948::reset() return ret; } -int ICM20948::reset_mpu() +int +ICM20948::reset_mpu() { - uint8_t retries; - switch (_whoami) { case ICM_WHOAMI_20948: write_reg(ICMREG_20948_PWR_MGMT_1, BIT_H_RESET); - usleep(1000); + px4_usleep(1000); write_checked_reg(ICMREG_20948_PWR_MGMT_1, MPU_CLK_SEL_AUTO); - usleep(200); + px4_usleep(200); write_checked_reg(ICMREG_20948_PWR_MGMT_2, 0); break; } @@ -223,7 +214,7 @@ int ICM20948::reset_mpu() // SAMPLE RATE _set_sample_rate(_sample_rate); - _set_dlpf_filter(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ); + _set_dlpf_filter(ICM20948_DEFAULT_ONCHIP_FILTER_FREQ); // Gyro scale 2000 deg/s () switch (_whoami) { @@ -232,7 +223,6 @@ int ICM20948::reset_mpu() break; } - // correct gyro scale factors // scale to rad/s in SI units // 2000 deg/s = (2000/180)*PI = 34.906585 rad/s @@ -264,21 +254,19 @@ int ICM20948::reset_mpu() write_checked_reg(ICMREG_20948_ACCEL_CONFIG_2, ICM_BITS_DEC3_CFG_32); - retries = 3; + uint8_t retries = 3; bool all_ok = false; while (!all_ok && retries--) { // Assume all checked values are as expected all_ok = true; - uint8_t reg; + uint8_t reg = 0; uint8_t bankcheck = 0; for (uint8_t i = 0; i < _num_checked_registers; i++) { if ((reg = read_reg(_checked_registers[i])) != _checked_values[i]) { - if (_whoami == ICM_WHOAMI_20948) { - _interface->read(MPU9250_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &bankcheck, 1); - } + _interface->read(ICM20948_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &bankcheck, 1); write_reg(_checked_registers[i], _checked_values[i]); PX4_ERR("Reg %d is:%d s/b:%d Tries:%d - bank s/b %d, is %d", _checked_registers[i], reg, _checked_values[i], retries, @@ -296,7 +284,7 @@ ICM20948::probe() { int ret = PX4_ERROR; - // Try first for mpu9250/6500 + // Try first for icm20948/6500 _whoami = read_reg(MPUREG_WHOAMI); // must be an ICM @@ -332,7 +320,7 @@ ICM20948::_set_sample_rate(unsigned desired_sample_rate_hz) uint8_t div = 1; if (desired_sample_rate_hz == 0) { - desired_sample_rate_hz = MPU9250_GYRO_DEFAULT_RATE; + desired_sample_rate_hz = ICM20948_GYRO_DEFAULT_RATE; } switch (_whoami) { @@ -459,7 +447,7 @@ ICM20948::select_register_bank(uint8_t bank) uint8_t retries = 3; if (_selected_bank != bank) { - ret = _interface->write(MPU9250_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &bank, 1); + ret = _interface->write(ICM20948_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &bank, 1); if (ret != OK) { return ret; @@ -470,11 +458,11 @@ ICM20948::select_register_bank(uint8_t bank) * Making sure the right register bank is selected (even if it should be). Observed some * unexpected changes to this, don't risk writing to the wrong register bank. */ - _interface->read(MPU9250_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &buf, 1); + _interface->read(ICM20948_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &buf, 1); while (bank != buf && retries > 0) { //PX4_WARN("user bank: expected %d got %d",bank,buf); - ret = _interface->write(MPU9250_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &bank, 1); + ret = _interface->write(ICM20948_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &bank, 1); if (ret != OK) { return ret; @@ -483,7 +471,7 @@ ICM20948::select_register_bank(uint8_t bank) retries--; //PX4_WARN("BANK retries: %d", 4-retries); - _interface->read(MPU9250_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &buf, 1); + _interface->read(ICM20948_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &buf, 1); } @@ -504,7 +492,7 @@ ICM20948::read_reg(unsigned reg, uint32_t speed) uint8_t buf{}; select_register_bank(REG_BANK(reg)); - _interface->read(MPU9250_SET_SPEED(REG_ADDRESS(reg), speed), &buf, 1); + _interface->read(ICM20948_SET_SPEED(REG_ADDRESS(reg), speed), &buf, 1); return buf; } @@ -517,7 +505,7 @@ ICM20948::read_reg_range(unsigned start_reg, uint32_t speed, uint8_t *buf, uint1 } select_register_bank(REG_BANK(start_reg)); - return _interface->read(MPU9250_SET_SPEED(REG_ADDRESS(start_reg), speed), buf, count); + return _interface->read(ICM20948_SET_SPEED(REG_ADDRESS(start_reg), speed), buf, count); } uint16_t @@ -527,7 +515,7 @@ ICM20948::read_reg16(unsigned reg) // general register transfer at low clock speed select_register_bank(REG_BANK(reg)); - _interface->read(MPU9250_LOW_SPEED_OP(REG_ADDRESS(reg)), &buf, arraySize(buf)); + _interface->read(ICM20948_LOW_SPEED_OP(REG_ADDRESS(reg)), &buf, arraySize(buf)); return (uint16_t)(buf[0] << 8) | buf[1]; } @@ -537,7 +525,7 @@ ICM20948::write_reg(unsigned reg, uint8_t value) { // general register transfer at low clock speed select_register_bank(REG_BANK(reg)); - _interface->write(MPU9250_LOW_SPEED_OP(REG_ADDRESS(reg)), &value, 1); + _interface->write(ICM20948_LOW_SPEED_OP(REG_ADDRESS(reg)), &value, 1); } void @@ -617,7 +605,7 @@ ICM20948::start() /* make sure we are stopped first */ stop(); - ScheduleOnInterval(_call_interval - MPU9250_TIMER_REDUCTION, 1000); + ScheduleOnInterval(_call_interval - ICM20948_TIMER_REDUCTION, 1000); } void @@ -647,8 +635,9 @@ ICM20948::check_registers(void) */ uint8_t v; - if ((v = read_reg(_checked_registers[_checked_next], MPU9250_HIGH_BUS_SPEED)) != + if ((v = read_reg(_checked_registers[_checked_next], ICM20948_HIGH_BUS_SPEED)) != _checked_values[_checked_next]) { + _checked_bad[_checked_next] = v; /* @@ -667,12 +656,11 @@ ICM20948::check_registers(void) if (_register_wait == 0 || _checked_next == 0) { // if the product_id is wrong then reset the // sensor completely - reset_mpu(); // after doing a reset we need to wait a long // time before we do any other register writes - // or we will end up with the mpu9250 in a + // or we will end up with the icm20948 in a // bizarre state where it has all correct // register values but large offsets on the // accel axes @@ -681,6 +669,7 @@ ICM20948::check_registers(void) } 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 @@ -693,7 +682,8 @@ ICM20948::check_registers(void) _checked_next = (_checked_next + 1) % _num_checked_registers; } -bool ICM20948::check_null_data(uint16_t *data, uint8_t size) +bool +ICM20948::check_null_data(uint16_t *data, uint8_t size) { while (size--) { if (*data++) { @@ -707,12 +697,13 @@ bool ICM20948::check_null_data(uint16_t *data, uint8_t size) perf_end(_sample_perf); // note that we don't call reset() here as a reset() // costs 20ms with interrupts disabled. That means if - // the mpu9250 does go bad it would cause a FMU failure, + // the icm20948 does go bad it would cause a FMU failure, // regardless of whether another sensor is available, return true; } -bool ICM20948::check_duplicate(uint8_t *accel_data) +bool +ICM20948::check_duplicate(uint8_t *accel_data) { /* see if this is duplicate accelerometer data. Note that we @@ -739,14 +730,17 @@ bool ICM20948::check_duplicate(uint8_t *accel_data) void ICM20948::measure() { + perf_begin(_sample_perf); + perf_count(_interval_perf); + if (hrt_absolute_time() < _reset_wait) { // we're waiting for a reset to complete + perf_end(_sample_perf); return; } - struct MPUReport mpu_report; - - struct ICMReport icm_report; + MPUReport mpu_report{}; + ICMReport icm_report{}; struct Report { int16_t accel_x; @@ -756,21 +750,16 @@ ICM20948::measure() int16_t gyro_x; int16_t gyro_y; int16_t gyro_z; - } report; - - /* start measuring */ - perf_begin(_sample_perf); + } report{}; const hrt_abstime timestamp_sample = hrt_absolute_time(); - /* - * Fetch the full set of measurements from the MPU9250 in one pass - */ - if ((!_magnetometer_only || _mag.is_passthrough()) && _register_wait == 0) { + // Fetch the full set of measurements from the ICM20948 in one pass + if (_mag.is_passthrough() && _register_wait == 0) { select_register_bank(REG_BANK(ICMREG_20948_ACCEL_XOUT_H)); - if (OK != read_reg_range(ICMREG_20948_ACCEL_XOUT_H, MPU9250_HIGH_BUS_SPEED, (uint8_t *)&icm_report, + if (OK != read_reg_range(ICMREG_20948_ACCEL_XOUT_H, ICM20948_HIGH_BUS_SPEED, (uint8_t *)&icm_report, sizeof(icm_report))) { perf_end(_sample_perf); return; @@ -803,32 +792,16 @@ ICM20948::measure() # endif - /* - * Continue evaluating gyro and accelerometer results - */ - if (!_magnetometer_only && _register_wait == 0) { - - /* - * Convert from big to little endian - */ - if (_whoami == ICM_WHOAMI_20948) { - report.accel_x = int16_t_from_bytes(icm_report.accel_x); - report.accel_y = int16_t_from_bytes(icm_report.accel_y); - report.accel_z = int16_t_from_bytes(icm_report.accel_z); - report.temp = int16_t_from_bytes(icm_report.temp); - report.gyro_x = int16_t_from_bytes(icm_report.gyro_x); - report.gyro_y = int16_t_from_bytes(icm_report.gyro_y); - report.gyro_z = int16_t_from_bytes(icm_report.gyro_z); - - } else { // MPU9250/MPU6500 - report.accel_x = int16_t_from_bytes(mpu_report.accel_x); - report.accel_y = int16_t_from_bytes(mpu_report.accel_y); - report.accel_z = int16_t_from_bytes(mpu_report.accel_z); - report.temp = int16_t_from_bytes(mpu_report.temp); - report.gyro_x = int16_t_from_bytes(mpu_report.gyro_x); - report.gyro_y = int16_t_from_bytes(mpu_report.gyro_y); - report.gyro_z = int16_t_from_bytes(mpu_report.gyro_z); - } + // Continue evaluating gyro and accelerometer results + if (_register_wait == 0) { + // Convert from big to little endian + report.accel_x = int16_t_from_bytes(icm_report.accel_x); + report.accel_y = int16_t_from_bytes(icm_report.accel_y); + report.accel_z = int16_t_from_bytes(icm_report.accel_z); + report.temp = int16_t_from_bytes(icm_report.temp); + report.gyro_x = int16_t_from_bytes(icm_report.gyro_x); + report.gyro_y = int16_t_from_bytes(icm_report.gyro_y); + report.gyro_z = int16_t_from_bytes(icm_report.gyro_z); if (check_null_data((uint16_t *)&report, sizeof(report) / 2)) { return; @@ -839,72 +812,42 @@ ICM20948::measure() /* * 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; } - /* - * Get sensor temperature - */ + // Get sensor temperature _last_temperature = (report.temp) / 333.87f + 21.0f; _px4_accel.set_temperature(_last_temperature); _px4_gyro.set_temperature(_last_temperature); - /* - * Convert and publish accelerometer and gyrometer data. - */ - if (!_magnetometer_only) { + // Swap axes and negate y + int16_t accel_xt = report.accel_y; + int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x); - /* - * Keeping the axes as they are for ICM20948 so orientation will match the actual chip orientation - * Swap axes and negate y - */ + int16_t gyro_xt = report.gyro_y; + int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x); - int16_t accel_xt = report.accel_y; - int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x); + // Apply the swap + report.accel_x = accel_xt; + report.accel_y = accel_yt; + report.gyro_x = gyro_xt; + report.gyro_y = gyro_yt; - int16_t gyro_xt = report.gyro_y; - int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x); + // 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 + const uint64_t error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); + _px4_accel.set_error_count(error_count); + _px4_gyro.set_error_count(error_count); - /* - * Apply the swap - */ - report.accel_x = accel_xt; - report.accel_y = accel_yt; - report.gyro_x = gyro_xt; - report.gyro_y = gyro_yt; - - // 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 - const uint64_t error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); - _px4_accel.set_error_count(error_count); - _px4_gyro.set_error_count(error_count); - - /* - * 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. - */ - - /* NOTE: Axes have been swapped to match the board a few lines above. */ - _px4_accel.update(timestamp_sample, report.accel_x, report.accel_y, report.accel_z); - _px4_gyro.update(timestamp_sample, report.gyro_x, report.gyro_y, report.gyro_z); - } + /* NOTE: Axes have been swapped to match the board a few lines above. */ + _px4_accel.update(timestamp_sample, report.accel_x, report.accel_y, report.accel_z); + _px4_gyro.update(timestamp_sample, report.gyro_x, report.gyro_y, report.gyro_z); /* stop measuring */ perf_end(_sample_perf); @@ -914,18 +857,12 @@ void ICM20948::print_info() { perf_print_counter(_sample_perf); - perf_print_counter(_accel_reads); - 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); - if (!_magnetometer_only) { - _px4_accel.print_status(); - _px4_gyro.print_status(); - } - + _px4_accel.print_status(); + _px4_gyro.print_status(); _mag.print_status(); } diff --git a/src/drivers/imu/icm20948/icm20948.h b/src/drivers/imu/icm20948/icm20948.h index abeead1d58..682d48283b 100644 --- a/src/drivers/imu/icm20948/icm20948.h +++ b/src/drivers/imu/icm20948/icm20948.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2019 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 @@ -33,29 +33,22 @@ #pragma once -#include - -#include -#include - -#include - #include #include -#include -#include +#include +#include #include - -#include +#include +#include #include "ICM20948_mag.h" -#if defined(PX4_I2C_OBDEV_MPU9250) || defined(PX4_I2C_BUS_EXPANSION) +#if defined(PX4_I2C_OBDEV_ICM20948) || defined(PX4_I2C_BUS_EXPANSION) # define USE_I2C #endif -// MPU 9250 registers +// ICM20948 registers #define MPUREG_WHOAMI 0x75 #define MPUREG_SMPLRT_DIV 0x19 #define MPUREG_CONFIG 0x1A @@ -116,7 +109,7 @@ #define MPUREG_FIFO_COUNTL 0x73 #define MPUREG_FIFO_R_W 0x74 -// Configuration bits MPU 9250 +// Configuration bits ICM20948 #define BIT_SLEEP 0x40 #define BIT_H_RESET 0x80 #define MPU_CLK_SEL_AUTO 0x01 @@ -175,18 +168,15 @@ #define ICM_WHOAMI_20948 0xEA -#define MPU9250_ACCEL_DEFAULT_RATE 1000 -#define MPU9250_ACCEL_MAX_OUTPUT_RATE 280 -#define MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 -#define MPU9250_GYRO_DEFAULT_RATE 1000 +#define ICM20948_ACCEL_DEFAULT_RATE 1000 +#define ICM20948_ACCEL_MAX_OUTPUT_RATE 280 +#define ICM20948_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 +#define ICM20948_GYRO_DEFAULT_RATE 1000 /* rates need to be the same between accel and gyro */ -#define MPU9250_GYRO_MAX_OUTPUT_RATE MPU9250_ACCEL_MAX_OUTPUT_RATE -#define MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30 - -#define MPU9250_DEFAULT_ONCHIP_FILTER_FREQ 92 - -#define MPUIOCGIS_I2C (unsigned)(DEVIOCGDEVICEID+100) +#define ICM20948_GYRO_MAX_OUTPUT_RATE ICM20948_ACCEL_MAX_OUTPUT_RATE +#define ICM20948_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30 +#define ICM20948_DEFAULT_ONCHIP_FILTER_FREQ 92 // ICM20948 registers and data @@ -245,9 +235,9 @@ /* * ICM20948 register bits -* Most of the regiser set values from MPU9250 have the same +* Most of the regiser set values from ICM20948 have the same * meaning on ICM20948. The exceptions and values not already -* defined for MPU9250 are defined below +* defined for ICM20948 are defined below */ #define ICM_BIT_PWR_MGMT_1_ENABLE 0x00 #define ICM_BIT_USER_CTRL_I2C_MST_DISABLE 0x00 @@ -293,10 +283,8 @@ #define ICM_BITS_I2C_MST_CLOCK_400HZ 0x07 // recommended by datasheet for 400kHz target clock - #define MPU_OR_ICM(m,i) ((_whoami==ICM_WHOAMI_20948) ? i : m) - #pragma pack(push, 1) /** * Report conversation within the mpu, including command byte and @@ -310,13 +298,11 @@ struct ICMReport { uint8_t gyro_y[2]; uint8_t gyro_z[2]; uint8_t temp[2]; - struct ak8963_regs mag; + struct ak09916_regs mag; }; #pragma pack(pop) - - #pragma pack(push, 1) /** * Report conversation within the mpu, including command byte and @@ -332,44 +318,39 @@ struct MPUReport { uint8_t gyro_x[2]; uint8_t gyro_y[2]; uint8_t gyro_z[2]; - struct ak8963_regs mag; + struct ak09916_regs mag; }; #pragma pack(pop) -#define MPU_MAX_WRITE_BUFFER_SIZE (2) - - /* - The MPU9250 can only handle high bus speeds on the sensor and + The ICM20948 can only handle high bus speeds on the sensor and interrupt status registers. All other registers have a maximum 1MHz Communication with all registers of the device is performed using either I2C at 400kHz or SPI at 1MHz. For applications requiring faster communications, the sensor and interrupt registers may be read using SPI at 20MHz */ -#define MPU9250_LOW_BUS_SPEED 0 -#define MPU9250_HIGH_BUS_SPEED 0x8000 -#define MPU9250_REG_MASK 0x00FF -# define MPU9250_IS_HIGH_SPEED(r) ((r) & MPU9250_HIGH_BUS_SPEED) -# define MPU9250_REG(r) ((r) & MPU9250_REG_MASK) -# define MPU9250_SET_SPEED(r, s) ((r)|(s)) -# define MPU9250_HIGH_SPEED_OP(r) MPU9250_SET_SPEED((r), MPU9250_HIGH_BUS_SPEED) -# define MPU9250_LOW_SPEED_OP(r) ((r) &~MPU9250_HIGH_BUS_SPEED) +#define ICM20948_LOW_BUS_SPEED 0 +#define ICM20948_HIGH_BUS_SPEED 0x8000 +#define ICM20948_REG_MASK 0x00FF +# define ICM20948_IS_HIGH_SPEED(r) ((r) & ICM20948_HIGH_BUS_SPEED) +# define ICM20948_REG(r) ((r) & ICM20948_REG_MASK) +# define ICM20948_SET_SPEED(r, s) ((r)|(s)) +# define ICM20948_HIGH_SPEED_OP(r) ICM20948_SET_SPEED((r), ICM20948_HIGH_BUS_SPEED) +# define ICM20948_LOW_SPEED_OP(r) ((r) &~ICM20948_HIGH_BUS_SPEED) /* interface factories */ -extern device::Device *ICM20948_SPI_interface(int bus, uint32_t cs, bool external_bus); -extern device::Device *ICM20948_I2C_interface(int bus, uint32_t address, bool external_bus); -extern int MPU9250_probe(device::Device *dev); +extern device::Device *ICM20948_SPI_interface(int bus, uint32_t cs); +extern device::Device *ICM20948_I2C_interface(int bus, uint32_t address); +extern int ICM20948_probe(device::Device *dev); -typedef device::Device *(*ICM20948_constructor)(int, uint32_t, bool); +typedef device::Device *(*ICM20948_constructor)(int, uint32_t); class ICM20948_mag; class ICM20948 : public px4::ScheduledWorkItem { public: - ICM20948(device::Device *interface, device::Device *mag_interface, const char *path, enum Rotation rotation, - bool magnetometer_only); - + ICM20948(device::Device *interface, device::Device *mag_interface, enum Rotation rotation); virtual ~ICM20948(); virtual int init(); @@ -382,7 +363,7 @@ public: protected: device::Device *_interface; - uint8_t _whoami; /** whoami result */ + uint8_t _whoami{0}; /** whoami result */ virtual int probe(); @@ -396,25 +377,21 @@ private: PX4Gyroscope _px4_gyro; ICM20948_mag _mag; - uint8_t _selected_bank; /* Remember selected memory bank to avoid polling / setting on each read/write */ - bool - _magnetometer_only; /* To disable accel and gyro reporting if only magnetometer is used (e.g. as external magnetometer) */ + uint8_t _selected_bank{0}; /* Remember selected memory bank to avoid polling / setting on each read/write */ unsigned _call_interval{1000}; - unsigned _dlpf_freq; - unsigned _dlpf_freq_icm_gyro; - unsigned _dlpf_freq_icm_accel; + unsigned _dlpf_freq{0}; + unsigned _dlpf_freq_icm_gyro{0}; + unsigned _dlpf_freq_icm_accel{0}; unsigned _sample_rate{1000}; - perf_counter_t _accel_reads; - perf_counter_t _gyro_reads; perf_counter_t _sample_perf; + perf_counter_t _interval_perf; perf_counter_t _bad_transfers; perf_counter_t _bad_registers; perf_counter_t _good_transfers; - perf_counter_t _reset_retries; perf_counter_t _duplicates; uint8_t _register_wait{0}; @@ -429,8 +406,8 @@ private: const uint16_t *_checked_registers{nullptr}; - uint8_t _checked_values[ICM20948_NUM_CHECKED_REGISTERS]; - uint8_t _checked_bad[ICM20948_NUM_CHECKED_REGISTERS]; + uint8_t _checked_values[ICM20948_NUM_CHECKED_REGISTERS] {}; + uint8_t _checked_bad[ICM20948_NUM_CHECKED_REGISTERS] {}; unsigned _checked_next{0}; unsigned _num_checked_registers{0}; @@ -440,28 +417,15 @@ private: bool check_null_data(uint16_t *data, uint8_t size); bool check_duplicate(uint8_t *accel_data); + // keep last accel reading for duplicate detection uint8_t _last_accel_data[6] {}; bool _got_duplicate{false}; - /** - * 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(); - /** * Resets the main chip (excluding the magnetometer if any). */ @@ -483,7 +447,6 @@ private: */ int select_register_bank(uint8_t bank); - /** * Read a register from the mpu * @@ -491,7 +454,7 @@ private: * @param The bus speed to read with. * @return The value that was read. */ - uint8_t read_reg(unsigned reg, uint32_t speed = MPU9250_LOW_BUS_SPEED); + uint8_t read_reg(unsigned reg, uint32_t speed = ICM20948_LOW_BUS_SPEED); uint16_t read_reg16(unsigned reg); @@ -574,14 +537,8 @@ private: */ void _set_sample_rate(unsigned desired_sample_rate_hz); - /* - set poll rate - */ - int _set_pollrate(unsigned long rate); - /* check that key registers still have the right value */ - void check_registers(void); - + void check_registers(); }; diff --git a/src/drivers/imu/icm20948/icm20948_i2c.cpp b/src/drivers/imu/icm20948/icm20948_i2c.cpp index 62cc8139f7..938ebf8d16 100644 --- a/src/drivers/imu/icm20948/icm20948_i2c.cpp +++ b/src/drivers/imu/icm20948/icm20948_i2c.cpp @@ -45,7 +45,7 @@ #ifdef USE_I2C -device::Device *ICM20948_I2C_interface(int bus, uint32_t address, bool external_bus); +device::Device *ICM20948_I2C_interface(int bus, uint32_t address); class ICM20948_I2C : public device::I2C { @@ -64,7 +64,7 @@ private: }; device::Device * -ICM20948_I2C_interface(int bus, uint32_t address, bool external_bus) +ICM20948_I2C_interface(int bus, uint32_t address) { return new ICM20948_I2C(bus, address); } @@ -72,19 +72,19 @@ ICM20948_I2C_interface(int bus, uint32_t address, bool external_bus) ICM20948_I2C::ICM20948_I2C(int bus, uint32_t address) : I2C("ICM20948_I2C", nullptr, bus, address, 400000) { - _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU9250; + _device_id.devid_s.devtype = DRV_DEVTYPE_ICM20948; } int ICM20948_I2C::write(unsigned reg_speed, void *data, unsigned count) { - uint8_t cmd[MPU_MAX_WRITE_BUFFER_SIZE]; + uint8_t cmd[2] {}; if (sizeof(cmd) < (count + 1)) { return -EIO; } - cmd[0] = MPU9250_REG(reg_speed); + cmd[0] = ICM20948_REG(reg_speed); cmd[1] = *(uint8_t *)data; return transfer(&cmd[0], count + 1, nullptr, 0); } @@ -99,7 +99,7 @@ ICM20948_I2C::read(unsigned reg_speed, void *data, unsigned count) * after that. Foe anthing else we must return it */ uint32_t offset = count < sizeof(MPUReport) ? 0 : offsetof(MPUReport, status); - uint8_t cmd = MPU9250_REG(reg_speed); + uint8_t cmd = ICM20948_REG(reg_speed); return transfer(&cmd, 1, &((uint8_t *)data)[offset], count); } @@ -109,7 +109,7 @@ ICM20948_I2C::probe() uint8_t whoami = 0; uint8_t register_select = REG_BANK(BANK0); // register bank containing WHOAMI for ICM20948 - // Try first for mpu9250/6500 + // Try first for icm20948/6500 read(MPUREG_WHOAMI, &whoami, 1); /* diff --git a/src/drivers/imu/icm20948/icm20948_main.cpp b/src/drivers/imu/icm20948/icm20948_main.cpp index 9b522b7f9c..6963bde785 100644 --- a/src/drivers/imu/icm20948/icm20948_main.cpp +++ b/src/drivers/imu/icm20948/icm20948_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2019 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 @@ -36,7 +36,7 @@ * * Driver for the Invensense icm20948 connected via I2C or SPI. * - * based on the mpu9250 driver + * based on the icm20948 driver */ #include @@ -50,8 +50,6 @@ #include "icm20948.h" -#define ICM_DEVICE_PATH_EXT "/dev/icm20948_ext" - /** driver 'main' command */ extern "C" { __EXPORT int icm20948_main(int argc, char *argv[]); } @@ -60,7 +58,6 @@ enum ICM20948_BUS { ICM20948_BUS_I2C_INTERNAL, ICM20948_BUS_I2C_EXTERNAL, ICM20948_BUS_SPI_INTERNAL, -// ICM20948_BUS_SPI_INTERNAL2, ICM20948_BUS_SPI_EXTERNAL }; @@ -70,13 +67,9 @@ enum ICM20948_BUS { namespace icm20948 { -/* - list of supported bus configurations - */ - +// list of supported bus configurations struct icm20948_bus_option { enum ICM20948_BUS busid; - const char *path; ICM20948_constructor interface_constructor; bool magpassthrough; uint8_t busnum; @@ -85,67 +78,66 @@ struct icm20948_bus_option { } bus_options[] = { #if defined(PX4_SPIDEV_ICM_20948) && defined(PX4_SPI_BUS_1) - { ICM20948_BUS_SPI_INTERNAL, ICM_DEVICE_PATH_EXT, &ICM20948_SPI_interface, true, PX4_SPI_BUS_1, PX4_SPIDEV_ICM_20948, nullptr }, + { ICM20948_BUS_SPI_INTERNAL, &ICM20948_SPI_interface, true, PX4_SPI_BUS_1, PX4_SPIDEV_ICM_20948, nullptr }, #endif #if defined (USE_I2C) - # if defined(PX4_I2C_BUS_EXPANSION) - { ICM20948_BUS_I2C_EXTERNAL, ICM_DEVICE_PATH_EXT, &ICM20948_I2C_interface, false, PX4_I2C_BUS_EXPANSION, PX4_I2C_EXT_ICM20948_1, nullptr }, + { ICM20948_BUS_I2C_EXTERNAL, &ICM20948_I2C_interface, false, PX4_I2C_BUS_EXPANSION, PX4_I2C_EXT_ICM20948_1, nullptr }, # endif - #endif - }; #define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) -void start(enum ICM20948_BUS busid, enum Rotation rotation, bool external_bus, bool magnetometer_only); -bool start_bus(struct icm20948_bus_option &bus, enum Rotation rotation, bool external_bus, bool magnetometer_only); -struct icm20948_bus_option &find_bus(enum ICM20948_BUS busid); -void stop(enum ICM20948_BUS busid); -void info(enum ICM20948_BUS busid); -void usage(); +bool start_bus(icm20948_bus_option &bus, enum Rotation rotation); +icm20948_bus_option *find_bus(enum ICM20948_BUS busid); + +int start(enum ICM20948_BUS busid, enum Rotation rotation); +int stop(enum ICM20948_BUS busid); +int info(enum ICM20948_BUS busid); +int usage(); /** * find a bus structure for a busid */ -struct icm20948_bus_option &find_bus(enum ICM20948_BUS busid) +struct icm20948_bus_option *find_bus(enum ICM20948_BUS busid) { for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) { if ((busid == ICM20948_BUS_ALL || busid == bus_options[i].busid) && bus_options[i].dev != nullptr) { - return bus_options[i]; + return &bus_options[i]; } } - errx(1, "bus %u not started", (unsigned)busid); + PX4_ERR("bus %u not started", (unsigned)busid); + return nullptr; } /** * start driver for a specific bus option */ bool -start_bus(struct icm20948_bus_option &bus, enum Rotation rotation, bool external, bool magnetometer_only) +start_bus(icm20948_bus_option &bus, enum Rotation rotation) { PX4_INFO("Bus probed: %d", bus.busid); if (bus.dev != nullptr) { - warnx("%s SPI not available", external ? "External" : "Internal"); + PX4_ERR("SPI %d not available", bus.busid); return false; } - device::Device *interface = bus.interface_constructor(bus.busnum, bus.address, external); + device::Device *interface = bus.interface_constructor(bus.busnum, bus.address); if (interface == nullptr) { - warnx("no device on bus %u", (unsigned)bus.busid); + PX4_WARN("no device on bus %u", (unsigned)bus.busid); return false; } if (interface->init() != OK) { delete interface; - warnx("no device on bus %u", (unsigned)bus.busid); + PX4_WARN("no device on bus %u", (unsigned)bus.busid); return false; } @@ -153,15 +145,15 @@ start_bus(struct icm20948_bus_option &bus, enum Rotation rotation, bool external #ifdef USE_I2C /* For i2c interfaces, connect to the magnetomer directly */ - bool is_i2c = bus.busid == ICM20948_BUS_I2C_INTERNAL || bus.busid == ICM20948_BUS_I2C_EXTERNAL; + const bool is_i2c = bus.busid == ICM20948_BUS_I2C_INTERNAL || bus.busid == ICM20948_BUS_I2C_EXTERNAL; if (is_i2c) { - mag_interface = AK8963_I2C_interface(bus.busnum, external); + mag_interface = AK09916_I2C_interface(bus.busnum); } #endif - bus.dev = new ICM20948(interface, mag_interface, bus.path, rotation, magnetometer_only); + bus.dev = new ICM20948(interface, mag_interface, rotation); if (bus.dev == nullptr) { delete interface; @@ -186,7 +178,9 @@ fail: bus.dev = nullptr; } - errx(1, "driver start failed"); + PX4_ERR("driver start failed"); + + return false; } /** @@ -195,10 +189,9 @@ fail: * This function only returns if the driver is up and running * or failed to detect the sensor. */ -void -start(enum ICM20948_BUS busid, enum Rotation rotation, bool external, bool magnetometer_only) +int +start(enum ICM20948_BUS busid, enum Rotation rotation) { - bool started = false; for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) { @@ -212,56 +205,53 @@ start(enum ICM20948_BUS busid, enum Rotation rotation, bool external, bool magne continue; } - started |= start_bus(bus_options[i], rotation, external, magnetometer_only); + started |= start_bus(bus_options[i], rotation); if (started) { break; } } - exit(started ? 0 : 1); - + return PX4_OK; } -void +int stop(enum ICM20948_BUS busid) { - struct icm20948_bus_option &bus = find_bus(busid); + icm20948_bus_option *bus = find_bus(busid); - - if (bus.dev != nullptr) { - delete bus.dev; - bus.dev = nullptr; + if (bus != nullptr && bus->dev != nullptr) { + delete bus->dev; + bus->dev = nullptr; } else { /* warn, but not an error */ - warnx("already stopped."); + PX4_WARN("already stopped."); } - exit(0); + return PX4_OK; } /** * Print a little info about the driver. */ -void +int info(enum ICM20948_BUS busid) { - struct icm20948_bus_option &bus = find_bus(busid); + icm20948_bus_option *bus = find_bus(busid); - - if (bus.dev == nullptr) { - errx(1, "driver not running"); + if (bus != nullptr && bus->dev != nullptr) { + PX4_WARN("driver not running"); + return PX4_ERROR; } - printf("state @ %p\n", bus.dev); - bus.dev->print_info(); + bus->dev->print_info(); - exit(0); + return PX4_OK; } -void +int usage() { - PX4_INFO("missing command: try 'start', 'info', 'stop',\n 'regdump', 'testerror'"); + PX4_INFO("missing command: try 'start', 'stop', 'info'"); PX4_INFO("options:"); PX4_INFO(" -X (i2c external bus)"); PX4_INFO(" -I (i2c internal bus)"); @@ -269,7 +259,8 @@ usage() PX4_INFO(" -S (spi external bus)"); PX4_INFO(" -t (spi internal bus, 2nd instance)"); PX4_INFO(" -R rotation"); - PX4_INFO(" -M only enable magnetometer, accel/gyro disabled - not av. on MPU6500"); + + return PX4_OK; } } // namespace icm20948 @@ -283,7 +274,6 @@ icm20948_main(int argc, char *argv[]) enum ICM20948_BUS busid = ICM20948_BUS_ALL; enum Rotation rotation = ROTATION_NONE; - bool magnetometer_only = false; while ((ch = px4_getopt(argc, argv, "XISstMR:", &myoptind, &myoptarg)) != EOF) { switch (ch) { @@ -291,62 +281,38 @@ icm20948_main(int argc, char *argv[]) busid = ICM20948_BUS_I2C_EXTERNAL; break; -// case 'I': -// busid = ICM20948_BUS_I2C_INTERNAL; -// break; - -// case 'S': -// busid = ICM20948_BUS_SPI_EXTERNAL; -// break; -// -// case 's': -// busid = ICM20948_BUS_SPI_INTERNAL; -// break; -// -// case 't': -// busid = ICM20948_BUS_SPI_INTERNAL2; -// break; - case 'R': rotation = (enum Rotation)atoi(myoptarg); break; - case 'M': - magnetometer_only = true; - break; - default: - icm20948::usage(); - return 0; + return icm20948::usage(); } } if (myoptind >= argc) { - icm20948::usage(); - return -1; + return icm20948::usage(); } - bool external = busid == ICM20948_BUS_I2C_EXTERNAL || busid == ICM20948_BUS_SPI_EXTERNAL; const char *verb = argv[myoptind]; /* * Start/load the driver. */ if (!strcmp(verb, "start")) { - icm20948::start(busid, rotation, external, magnetometer_only); + return icm20948::start(busid, rotation); } if (!strcmp(verb, "stop")) { - icm20948::stop(busid); + return icm20948::stop(busid); } /* * Print driver information. */ if (!strcmp(verb, "info")) { - icm20948::info(busid); + return icm20948::info(busid); } - icm20948::usage(); - return 0; + return icm20948::usage(); } diff --git a/src/drivers/imu/icm20948/icm20948_spi.cpp b/src/drivers/imu/icm20948/icm20948_spi.cpp index 52de202ed1..94b714e0d4 100644 --- a/src/drivers/imu/icm20948/icm20948_spi.cpp +++ b/src/drivers/imu/icm20948/icm20948_spi.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2019 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 @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file mpu9250_spi.cpp + * @file icm20948_spi.cpp * * Driver for the Invensense ICM20948 connected via SPI. * @@ -42,7 +42,6 @@ */ #include - #include "icm20948.h" #define DIR_READ 0x80 @@ -57,10 +56,10 @@ * for a 168Mhz CPU this will be 10.5 Mhz and for a 180 Mhz CPU * it will be 11.250 Mhz */ -#define MPU9250_LOW_SPI_BUS_SPEED 1000*1000 -#define MPU9250_HIGH_SPI_BUS_SPEED 20*1000*1000 +#define ICM20948_LOW_SPI_BUS_SPEED 1000*1000 +#define ICM20948_HIGH_SPI_BUS_SPEED 20*1000*1000 -device::Device *ICM20948_SPI_interface(int bus, uint32_t cs, bool external_bus); +device::Device *ICM20948_SPI_interface(int bus, uint32_t cs); class ICM20948_SPI : public device::SPI { @@ -81,50 +80,41 @@ private: }; device::Device * -ICM20948_SPI_interface(int bus, uint32_t cs, bool external_bus) +ICM20948_SPI_interface(int bus, uint32_t cs) { device::Device *interface = nullptr; - if (external_bus) { -#if !(defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_MPU)) - errx(0, "External SPI not available"); -#endif - } - - if (cs != SPIDEV_NONE(0)) { - interface = new ICM20948_SPI(bus, cs); - } + interface = new ICM20948_SPI(bus, cs); return interface; } ICM20948_SPI::ICM20948_SPI(int bus, uint32_t device) : - SPI("ICM20948", nullptr, bus, device, SPIDEV_MODE3, MPU9250_LOW_SPI_BUS_SPEED) + SPI("ICM20948", nullptr, bus, device, SPIDEV_MODE3, ICM20948_LOW_SPI_BUS_SPEED) { - _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU9250; + _device_id.devid_s.devtype = DRV_DEVTYPE_ICM20948; } void ICM20948_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); + set_frequency(ICM20948_IS_HIGH_SPEED(reg_speed) ? ICM20948_HIGH_SPI_BUS_SPEED : ICM20948_LOW_SPI_BUS_SPEED); /* Isoolate the register on return */ - reg_speed = MPU9250_REG(reg_speed); + reg_speed = ICM20948_REG(reg_speed); } int ICM20948_SPI::write(unsigned reg_speed, void *data, unsigned count) { - uint8_t cmd[MPU_MAX_WRITE_BUFFER_SIZE]; + uint8_t cmd[2] {}; if (sizeof(cmd) < (count + 1)) { return -EIO; } /* Set the desired speed and isolate the register */ - set_bus_frequency(reg_speed); cmd[0] = reg_speed | DIR_WRITE; @@ -141,7 +131,7 @@ ICM20948_SPI::read(unsigned reg_speed, void *data, unsigned count) * and we need to provied the buffer large enough for the callers data * and our command. */ - uint8_t cmd[3] = {0, 0, 0}; + uint8_t cmd[3] {}; uint8_t *pbuff = count < sizeof(MPUReport) ? cmd : (uint8_t *) data ; @@ -181,6 +171,10 @@ ICM20948_SPI::probe() } switch (whoami) { + case ICM_WHOAMI_20948: + ret = 0; + break; + default: PX4_WARN("probe failed! %u", whoami); ret = -EIO; diff --git a/src/drivers/imu/icm20948/mag_i2c.cpp b/src/drivers/imu/icm20948/mag_i2c.cpp index be38bf1547..39827b6231 100644 --- a/src/drivers/imu/icm20948/mag_i2c.cpp +++ b/src/drivers/imu/icm20948/mag_i2c.cpp @@ -34,7 +34,7 @@ /** * @file mag_i2c.cpp * - * I2C interface for AK8963 + * I2C interface for AK09916 */ #include "icm20948.h" @@ -44,13 +44,13 @@ #ifdef USE_I2C -device::Device *AK8963_I2C_interface(int bus, bool external_bus); +device::Device *AK09916_I2C_interface(int bus); -class AK8963_I2C : public device::I2C +class AK09916_I2C : public device::I2C { public: - AK8963_I2C(int bus); - ~AK8963_I2C() override = default; + AK09916_I2C(int bus); + ~AK09916_I2C() override = default; int read(unsigned address, void *data, unsigned count) override; int write(unsigned address, void *data, unsigned count) override; @@ -61,45 +61,45 @@ protected: }; device::Device * -AK8963_I2C_interface(int bus, bool external_bus) +AK09916_I2C_interface(int bus) { - return new AK8963_I2C(bus); + return new AK09916_I2C(bus); } -AK8963_I2C::AK8963_I2C(int bus) : - I2C("AK8963_I2C", nullptr, bus, AK8963_I2C_ADDR, 400000) +AK09916_I2C::AK09916_I2C(int bus) : + I2C("AK09916_I2C", nullptr, bus, AK09916_I2C_ADDR, 400000) { - _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_MPU9250; + _device_id.devid_s.devtype = DRV_DEVTYPE_ICM20948; } int -AK8963_I2C::write(unsigned reg_speed, void *data, unsigned count) +AK09916_I2C::write(unsigned reg_speed, void *data, unsigned count) { - uint8_t cmd[MPU_MAX_WRITE_BUFFER_SIZE] {}; + uint8_t cmd[2] {}; if (sizeof(cmd) < (count + 1)) { return -EIO; } - cmd[0] = MPU9250_REG(reg_speed); + cmd[0] = ICM20948_REG(reg_speed); cmd[1] = *(uint8_t *)data; return transfer(&cmd[0], count + 1, nullptr, 0); } int -AK8963_I2C::read(unsigned reg_speed, void *data, unsigned count) +AK09916_I2C::read(unsigned reg_speed, void *data, unsigned count) { - uint8_t cmd = MPU9250_REG(reg_speed); + uint8_t cmd = ICM20948_REG(reg_speed); return transfer(&cmd, 1, (uint8_t *)data, count); } int -AK8963_I2C::probe() +AK09916_I2C::probe() { uint8_t whoami = 0; - uint8_t expected = AK8963_DEVICE_ID; + uint8_t expected = AK09916_DEVICE_ID; - if (PX4_OK != read(AK8963REG_WIA, &whoami, 1)) { + if (PX4_OK != read(AK09916REG_WIA, &whoami, 1)) { return -EIO; } diff --git a/src/drivers/imu/mpu9250/AK8963_I2C.cpp b/src/drivers/imu/mpu9250/AK8963_I2C.cpp index 829dec00d8..877f8d6629 100644 --- a/src/drivers/imu/mpu9250/AK8963_I2C.cpp +++ b/src/drivers/imu/mpu9250/AK8963_I2C.cpp @@ -46,7 +46,7 @@ #ifdef USE_I2C -device::Device *AK8963_I2C_interface(int bus, bool external_bus); +device::Device *AK8963_I2C_interface(int bus); class AK8963_I2C : public device::I2C { @@ -63,7 +63,7 @@ protected: }; device::Device * -AK8963_I2C_interface(int bus, bool external_bus) +AK8963_I2C_interface(int bus) { return new AK8963_I2C(bus); } @@ -76,7 +76,7 @@ AK8963_I2C::AK8963_I2C(int bus) : I2C("AK8963_I2C", nullptr, bus, AK8963_I2C_ADD int AK8963_I2C::write(unsigned reg_speed, void *data, unsigned count) { - uint8_t cmd[MPU_MAX_WRITE_BUFFER_SIZE]; + uint8_t cmd[2] {}; if (sizeof(cmd) < (count + 1)) { return -EIO; diff --git a/src/drivers/imu/mpu9250/MPU9250_mag.cpp b/src/drivers/imu/mpu9250/MPU9250_mag.cpp index 791e944fae..b834908e7a 100644 --- a/src/drivers/imu/mpu9250/MPU9250_mag.cpp +++ b/src/drivers/imu/mpu9250/MPU9250_mag.cpp @@ -40,6 +40,12 @@ * */ +#include +#include +#include +#include +#include + #include "MPU9250_mag.h" #include "mpu9250.h" @@ -50,9 +56,9 @@ MPU9250_mag::MPU9250_mag(MPU9250 *parent, device::Device *interface, enum Rotati _px4_mag(parent->_interface->get_device_id(), (parent->_interface->external() ? ORB_PRIO_MAX : ORB_PRIO_HIGH), rotation), _parent(parent), - _mag_overruns(perf_alloc(PC_COUNT, "mpu9250_mag_overruns")), - _mag_overflows(perf_alloc(PC_COUNT, "mpu9250_mag_overflows")), - _mag_errors(perf_alloc(PC_COUNT, "mpu9250_mag_errors")) + _mag_overruns(perf_alloc(PC_COUNT, MODULE_NAME": mag_overruns")), + _mag_overflows(perf_alloc(PC_COUNT, MODULE_NAME": mag_overflows")), + _mag_errors(perf_alloc(PC_COUNT, MODULE_NAME": mag_errors")) { _px4_mag.set_device_type(DRV_MAG_DEVTYPE_MPU9250); _px4_mag.set_scale(MPU9250_MAG_RANGE_GA); @@ -70,7 +76,7 @@ MPU9250_mag::measure() { const hrt_abstime timestamp_sample = hrt_absolute_time(); - uint8_t st1; + uint8_t st1 = 0; int ret = _interface->read(AK8963REG_ST1, &st1, sizeof(st1)); if (ret != OK) { @@ -92,7 +98,7 @@ MPU9250_mag::measure() perf_count(_mag_overruns); } - ak8963_regs data; + ak8963_regs data{}; ret = _interface->read(AK8963REG_ST1, &data, sizeof(data)); if (ret != OK) { @@ -218,15 +224,15 @@ MPU9250_mag::write_reg(unsigned reg, uint8_t value) } int -MPU9250_mag::ak8963_reset(void) +MPU9250_mag::ak8963_reset() { // First initialize it to use the bus int rv = ak8963_setup(); if (rv == OK) { - // Now reset the mag write_reg(AK8963REG_CNTL2, AK8963_RESET); + // Then re-initialize the bus/mag rv = ak8963_setup(); } @@ -235,7 +241,7 @@ MPU9250_mag::ak8963_reset(void) } bool -MPU9250_mag::ak8963_read_adjustments(void) +MPU9250_mag::ak8963_read_adjustments() { uint8_t response[3]; float ak8963_ASA[3]; @@ -267,7 +273,7 @@ MPU9250_mag::ak8963_read_adjustments(void) } int -MPU9250_mag::ak8963_setup_master_i2c(void) +MPU9250_mag::ak8963_setup_master_i2c() { /* When _interface is null we are using SPI and must * use the parent interface to configure the device to act @@ -286,12 +292,11 @@ MPU9250_mag::ak8963_setup_master_i2c(void) return OK; } int -MPU9250_mag::ak8963_setup(void) +MPU9250_mag::ak8963_setup() { int retries = 10; do { - ak8963_setup_master_i2c(); write_reg(AK8963REG_CNTL2, AK8963_RESET); @@ -304,7 +309,7 @@ MPU9250_mag::ak8963_setup(void) retries--; PX4_WARN("AK8963: bad id %d retries %d", id, retries); _parent->modify_reg(MPUREG_USER_CTRL, 0, BIT_I2C_MST_RST); - up_udelay(100); + px4_usleep(100); } while (retries > 0); if (retries > 0) { @@ -315,7 +320,7 @@ MPU9250_mag::ak8963_setup(void) PX4_ERR("AK8963: failed to read adjustment data. Retries %d", retries); _parent->modify_reg(MPUREG_USER_CTRL, 0, BIT_I2C_MST_RST); - up_udelay(100); + px4_usleep(100); ak8963_setup_master_i2c(); write_reg(AK8963REG_CNTL2, AK8963_RESET); } @@ -325,21 +330,18 @@ MPU9250_mag::ak8963_setup(void) PX4_ERR("AK8963: failed to initialize, disabled!"); _parent->modify_checked_reg(MPUREG_USER_CTRL, BIT_I2C_MST_EN, 0); _parent->write_reg(MPUREG_I2C_MST_CTRL, 0); + return -EIO; } if (_parent->_whoami == MPU_WHOAMI_9250) { write_reg(AK8963REG_CNTL1, AK8963_CONTINUOUS_MODE2 | AK8963_16BIT_ADC); - } - if (_interface == NULL) { - - /* Configure mpu' I2c Master interface to read ak8963 data - * Into to fifo - */ + if (_interface == nullptr) { + // Configure mpu' I2C Master interface to read ak8963 data into to fifo if (_parent->_whoami == MPU_WHOAMI_9250) { - set_passthrough(AK8963REG_ST1, sizeof(struct ak8963_regs)); + set_passthrough(AK8963REG_ST1, sizeof(ak8963_regs)); } } diff --git a/src/drivers/imu/mpu9250/MPU9250_mag.h b/src/drivers/imu/mpu9250/MPU9250_mag.h index 31529b78a6..05c1ad8ce3 100644 --- a/src/drivers/imu/mpu9250/MPU9250_mag.h +++ b/src/drivers/imu/mpu9250/MPU9250_mag.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-2019 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 @@ -33,7 +33,6 @@ #pragma once -#include #include #include #include @@ -41,9 +40,6 @@ /* in 16-bit sampling mode the mag resolution is 1.5 milli Gauss per bit */ static constexpr float MPU9250_MAG_RANGE_GA{1.5e-3f}; -/* we are using the continuous fixed sampling rate of 100Hz */ -#define MPU9250_AK8963_SAMPLE_RATE 100 - /* ak8963 register address and bit definitions */ #define AK8963_I2C_ADDR 0x0C #define AK8963_DEVICE_ID 0x48 @@ -106,11 +102,10 @@ struct ak8963_regs { }; #pragma pack(pop) -extern device::Device *AK8963_I2C_interface(int bus, bool external_bus); +extern device::Device *AK8963_I2C_interface(int bus); typedef device::Device *(*MPU9250_mag_constructor)(int, bool); - /** * Helper class implementing the magnetometer driver node. */ @@ -125,11 +120,11 @@ public: void passthrough_write(uint8_t reg, uint8_t val); void read_block(uint8_t reg, uint8_t *val, uint8_t count); - int ak8963_reset(void); - int ak8963_setup(void); - int ak8963_setup_master_i2c(void); + int ak8963_reset(); + int ak8963_setup(); + int ak8963_setup_master_i2c(); bool ak8963_check_id(uint8_t &id); - bool ak8963_read_adjustments(void); + bool ak8963_read_adjustments(); void print_status() { _px4_mag.print_status(); } @@ -157,7 +152,4 @@ private: perf_counter_t _mag_overflows; perf_counter_t _mag_errors; - /* do not allow to copy this class due to pointer data members */ - MPU9250_mag(const MPU9250_mag &); - MPU9250_mag operator=(const MPU9250_mag &); }; diff --git a/src/drivers/imu/mpu9250/mpu9250.cpp b/src/drivers/imu/mpu9250/mpu9250.cpp index d839804b31..9990a7fb86 100644 --- a/src/drivers/imu/mpu9250/mpu9250.cpp +++ b/src/drivers/imu/mpu9250/mpu9250.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-2019 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 @@ -70,21 +70,18 @@ const uint16_t MPU9250::_mpu9250_checked_registers[MPU9250_NUM_CHECKED_REGISTERS MPUREG_INT_PIN_CFG }; -MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const char *path, enum Rotation rotation, - bool magnetometer_only) : +MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, enum Rotation rotation) : ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())), _interface(interface), _px4_accel(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_MAX : ORB_PRIO_HIGH), rotation), _px4_gyro(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_MAX : ORB_PRIO_HIGH), rotation), _mag(this, mag_interface, rotation), - _selected_bank(0xFF), // invalid/improbable bank value, will be set on first read/write - _magnetometer_only(magnetometer_only), _dlpf_freq(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ), - _sample_perf(perf_alloc(PC_ELAPSED, "mpu9250_read")), - _bad_transfers(perf_alloc(PC_COUNT, "mpu9250_bad_trans")), - _bad_registers(perf_alloc(PC_COUNT, "mpu9250_bad_reg")), - _good_transfers(perf_alloc(PC_COUNT, "mpu9250_good_trans")), - _duplicates(perf_alloc(PC_COUNT, "mpu9250_dupe")) + _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), + _bad_transfers(perf_alloc(PC_COUNT, MODULE_NAME": bad_trans")), + _bad_registers(perf_alloc(PC_COUNT, MODULE_NAME": bad_reg")), + _good_transfers(perf_alloc(PC_COUNT, MODULE_NAME": good_trans")), + _duplicates(perf_alloc(PC_COUNT, MODULE_NAME": dupe")) { _px4_accel.set_device_type(DRV_ACC_DEVTYPE_MPU9250); _px4_gyro.set_device_type(DRV_GYR_DEVTYPE_MPU9250); @@ -92,11 +89,12 @@ MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const MPU9250::~MPU9250() { - /* make sure we are truly inactive */ + // make sure we are truly inactive stop(); - /* delete the perf counter */ + // delete the perf counter perf_free(_sample_perf); + perf_free(_interval_perf); perf_free(_bad_transfers); perf_free(_bad_registers); perf_free(_good_transfers); @@ -113,7 +111,7 @@ MPU9250::init() */ const bool is_i2c = (_interface->get_device_bus_type() == device::Device::DeviceBusType_I2C); - if (is_i2c && !_magnetometer_only) { + if (is_i2c) { _sample_rate = 200; } @@ -135,8 +133,7 @@ MPU9250::init() if (_whoami == MPU_WHOAMI_9250) { #ifdef USE_I2C - - up_udelay(100); + px4_usleep(100); if (!_mag.is_passthrough() && _mag._interface->init() != PX4_OK) { PX4_ERR("failed to setup ak8963 interface"); @@ -157,7 +154,8 @@ MPU9250::init() return ret; } -int MPU9250::reset() +int +MPU9250::reset() { /* When the mpu9250 starts from 0V the internal power on circuit * per the data sheet will require: @@ -165,7 +163,6 @@ int MPU9250::reset() * Start-up time for register read/write From power-up Typ:11 max:100 ms * */ - px4_usleep(110000); // Hold off sampling until done (100 MS will be shortened) @@ -182,17 +179,16 @@ int MPU9250::reset() return ret; } -int MPU9250::reset_mpu() +int +MPU9250::reset_mpu() { - uint8_t retries; - switch (_whoami) { case MPU_WHOAMI_9250: case MPU_WHOAMI_6500: write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_AUTO); write_checked_reg(MPUREG_PWR_MGMT_2, 0); - usleep(1000); + px4_usleep(1000); break; } @@ -213,7 +209,6 @@ int MPU9250::reset_mpu() break; } - // correct gyro scale factors // scale to rad/s in SI units // 2000 deg/s = (2000/180)*PI = 34.906585 rad/s @@ -245,22 +240,20 @@ int MPU9250::reset_mpu() write_checked_reg(MPUREG_ACCEL_CONFIG2, BITS_ACCEL_CONFIG2_41HZ); - retries = 3; + uint8_t retries = 3; bool all_ok = false; while (!all_ok && retries--) { // Assume all checked values are as expected all_ok = true; - uint8_t reg; - uint8_t bankcheck = 0; + uint8_t reg = 0; for (uint8_t i = 0; i < _num_checked_registers; i++) { if ((reg = read_reg(_checked_registers[i])) != _checked_values[i]) { write_reg(_checked_registers[i], _checked_values[i]); - PX4_ERR("Reg %d is:%d s/b:%d Tries:%d - bank s/b %d, is %d", _checked_registers[i], reg, _checked_values[i], retries, - REG_BANK(_checked_registers[i]), bankcheck); + PX4_ERR("Reg %d is:%d s/b:%d Tries:%d", _checked_registers[i], reg, _checked_values[i], retries); all_ok = false; } } @@ -391,7 +384,7 @@ MPU9250::read_reg(unsigned reg, uint32_t speed) { uint8_t buf{}; - _interface->read(MPU9250_SET_SPEED(REG_ADDRESS(reg), speed), &buf, 1); + _interface->read(MPU9250_SET_SPEED(reg, speed), &buf, 1); return buf; } @@ -403,7 +396,7 @@ MPU9250::read_reg_range(unsigned start_reg, uint32_t speed, uint8_t *buf, uint16 return PX4_ERROR; } - return _interface->read(MPU9250_SET_SPEED(REG_ADDRESS(start_reg), speed), buf, count); + return _interface->read(MPU9250_SET_SPEED(start_reg, speed), buf, count); } uint16_t @@ -519,7 +512,7 @@ MPU9250::Run() } void -MPU9250::check_registers(void) +MPU9250::check_registers() { /* we read the register at full speed, even though it isn't @@ -530,10 +523,10 @@ MPU9250::check_registers(void) test of SPI bus health to read at the same speed as we read the data registers. */ - uint8_t v; + uint8_t v = 0; + + if ((v = read_reg(_checked_registers[_checked_next], MPU9250_HIGH_BUS_SPEED)) != _checked_values[_checked_next]) { - if ((v = read_reg(_checked_registers[_checked_next], MPU9250_HIGH_BUS_SPEED)) != - _checked_values[_checked_next]) { _checked_bad[_checked_next] = v; /* @@ -550,9 +543,7 @@ MPU9250::check_registers(void) bus. */ if (_register_wait == 0 || _checked_next == 0) { - // if the product_id is wrong then reset the - // sensor completely - + // if the product_id is wrong then reset the sensor completely write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); write_reg(MPUREG_PWR_MGMT_2, MPU_CLK_SEL_AUTO); @@ -567,6 +558,7 @@ MPU9250::check_registers(void) } 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 @@ -579,7 +571,8 @@ MPU9250::check_registers(void) _checked_next = (_checked_next + 1) % _num_checked_registers; } -bool MPU9250::check_null_data(uint16_t *data, uint8_t size) +bool +MPU9250::check_null_data(uint16_t *data, uint8_t size) { while (size--) { if (*data++) { @@ -598,7 +591,8 @@ bool MPU9250::check_null_data(uint16_t *data, uint8_t size) return true; } -bool MPU9250::check_duplicate(uint8_t *accel_data) +bool +MPU9250::check_duplicate(uint8_t *accel_data) { /* see if this is duplicate accelerometer data. Note that we @@ -625,12 +619,16 @@ bool MPU9250::check_duplicate(uint8_t *accel_data) void MPU9250::measure() { + perf_begin(_sample_perf); + perf_count(_interval_perf); + if (hrt_absolute_time() < _reset_wait) { // we're waiting for a reset to complete + perf_end(_sample_perf); return; } - struct MPUReport mpu_report; + MPUReport mpu_report{}; struct Report { int16_t accel_x; @@ -640,24 +638,17 @@ MPU9250::measure() int16_t gyro_x; int16_t gyro_y; int16_t gyro_z; - } report; - - /* start measuring */ - perf_begin(_sample_perf); + } report{}; const hrt_abstime timestamp_sample = hrt_absolute_time(); - /* - * Fetch the full set of measurements from the MPU9250 in one pass - */ - - if ((!_magnetometer_only || _mag.is_passthrough()) && _register_wait == 0) { + // Fetch the full set of measurements from the ICM20948 in one pass + if (_mag.is_passthrough() && _register_wait == 0) { if (_whoami == MPU_WHOAMI_9250 || _whoami == MPU_WHOAMI_6500) { if (OK != read_reg_range(MPUREG_INT_STATUS, MPU9250_HIGH_BUS_SPEED, (uint8_t *)&mpu_report, sizeof(mpu_report))) { perf_end(_sample_perf); return; } - } check_registers(); @@ -689,14 +680,9 @@ MPU9250::measure() # endif - /* - * Continue evaluating gyro and accelerometer results - */ - if (!_magnetometer_only && _register_wait == 0) { - - /* - * Convert from big to little endian - */ + // Continue evaluating gyro and accelerometer results + if (_register_wait == 0) { + // Convert from big to little endian report.accel_x = int16_t_from_bytes(mpu_report.accel_x); report.accel_y = int16_t_from_bytes(mpu_report.accel_y); report.accel_z = int16_t_from_bytes(mpu_report.accel_z); @@ -714,71 +700,42 @@ MPU9250::measure() /* * 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; } - /* - * Get sensor temperature - */ + // Get sensor temperature _last_temperature = (report.temp) / 333.87f + 21.0f; _px4_accel.set_temperature(_last_temperature); _px4_gyro.set_temperature(_last_temperature); - /* - * Convert and publish accelerometer and gyrometer data. - */ - if (!_magnetometer_only) { + // Swap axes and negate y + int16_t accel_xt = report.accel_y; + int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x); - /* - * Swap axes and negate y - */ + int16_t gyro_xt = report.gyro_y; + int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x); - int16_t accel_xt = report.accel_y; - int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x); + // Apply the swap + report.accel_x = accel_xt; + report.accel_y = accel_yt; + report.gyro_x = gyro_xt; + report.gyro_y = gyro_yt; - int16_t gyro_xt = report.gyro_y; - int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x); + // 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 + const uint64_t error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); + _px4_accel.set_error_count(error_count); + _px4_gyro.set_error_count(error_count); - /* - * Apply the swap - */ - report.accel_x = accel_xt; - report.accel_y = accel_yt; - report.gyro_x = gyro_xt; - report.gyro_y = gyro_yt; - - // 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 - const uint64_t error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); - _px4_accel.set_error_count(error_count); - _px4_gyro.set_error_count(error_count); - - /* - * 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. - */ - - /* NOTE: Axes have been swapped to match the board a few lines above. */ - _px4_accel.update(timestamp_sample, report.accel_x, report.accel_y, report.accel_z); - _px4_gyro.update(timestamp_sample, report.gyro_x, report.gyro_y, report.gyro_z); - } + /* NOTE: Axes have been swapped to match the board a few lines above. */ + _px4_accel.update(timestamp_sample, report.accel_x, report.accel_y, report.accel_z); + _px4_gyro.update(timestamp_sample, report.gyro_x, report.gyro_y, report.gyro_z); /* stop measuring */ perf_end(_sample_perf); @@ -793,10 +750,7 @@ MPU9250::print_info() perf_print_counter(_good_transfers); perf_print_counter(_duplicates); - if (!_magnetometer_only) { - _px4_accel.print_status(); - _px4_gyro.print_status(); - } - + _px4_accel.print_status(); + _px4_gyro.print_status(); _mag.print_status(); } diff --git a/src/drivers/imu/mpu9250/mpu9250.h b/src/drivers/imu/mpu9250/mpu9250.h index 2709c1def2..aa139d3980 100644 --- a/src/drivers/imu/mpu9250/mpu9250.h +++ b/src/drivers/imu/mpu9250/mpu9250.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-2019 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 @@ -31,17 +31,18 @@ * ****************************************************************************/ +#pragma once + #include #include #include #include #include -#include -#include +#include +#include #include "MPU9250_mag.h" - #if defined(PX4_I2C_OBDEV_MPU9250) || defined(PX4_I2C_BUS_EXPANSION) # define USE_I2C #endif @@ -178,16 +179,6 @@ #define MPU9250_DEFAULT_ONCHIP_FILTER_FREQ 92 - -#define BANK0 0x0000 -#define BANK1 0x0100 -#define BANK2 0x0200 -#define BANK3 0x0300 - -#define BANK_REG_MASK 0x0300 -#define REG_BANK(r) (((r) & BANK_REG_MASK)>>4) -#define REG_ADDRESS(r) ((r) & ~BANK_REG_MASK) - #pragma pack(push, 1) /** * Report conversation within the mpu, including command byte and @@ -207,9 +198,6 @@ struct MPUReport { }; #pragma pack(pop) -#define MPU_MAX_WRITE_BUFFER_SIZE (2) - - /* The MPU9250 can only handle high bus speeds on the sensor and interrupt status registers. All other registers have a maximum 1MHz @@ -227,20 +215,18 @@ struct MPUReport { # define MPU9250_LOW_SPEED_OP(r) ((r) &~MPU9250_HIGH_BUS_SPEED) /* interface factories */ -extern device::Device *MPU9250_SPI_interface(int bus, uint32_t cs, bool external_bus); -extern device::Device *MPU9250_I2C_interface(int bus, uint32_t address, bool external_bus); +extern device::Device *MPU9250_SPI_interface(int bus, uint32_t cs); +extern device::Device *MPU9250_I2C_interface(int bus, uint32_t address); extern int MPU9250_probe(device::Device *dev); -typedef device::Device *(*MPU9250_constructor)(int, uint32_t, bool); +typedef device::Device *(*MPU9250_constructor)(int, uint32_t); class MPU9250_mag; class MPU9250 : public px4::ScheduledWorkItem { public: - MPU9250(device::Device *interface, device::Device *mag_interface, const char *path, enum Rotation rotation, - bool magnetometer_only); - + MPU9250(device::Device *interface, device::Device *mag_interface, enum Rotation rotation); virtual ~MPU9250(); virtual int init(); @@ -267,17 +253,15 @@ private: PX4Gyroscope _px4_gyro; MPU9250_mag _mag; - uint8_t _selected_bank; /* Remember selected memory bank to avoid polling / setting on each read/write */ - bool - _magnetometer_only; /* To disable accel and gyro reporting if only magnetometer is used (e.g. as external magnetometer) */ unsigned _call_interval{1000}; - unsigned _dlpf_freq; + unsigned _dlpf_freq{0}; unsigned _sample_rate{1000}; perf_counter_t _sample_perf; + perf_counter_t _interval_perf; perf_counter_t _bad_transfers; perf_counter_t _bad_registers; perf_counter_t _good_transfers; @@ -291,7 +275,7 @@ private: // reset static constexpr int MPU9250_NUM_CHECKED_REGISTERS{11}; - static const uint16_t _mpu9250_checked_registers[MPU9250_NUM_CHECKED_REGISTERS]; + static const uint16_t _mpu9250_checked_registers[MPU9250_NUM_CHECKED_REGISTERS]; const uint16_t *_checked_registers{nullptr}; @@ -306,28 +290,15 @@ private: bool check_null_data(uint16_t *data, uint8_t size); bool check_duplicate(uint8_t *accel_data); + // keep last accel reading for duplicate detection uint8_t _last_accel_data[6] {}; bool _got_duplicate{false}; - /** - * 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(); - /** * Resets the main chip (excluding the magnetometer if any). */ @@ -431,9 +402,5 @@ private: /* 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 */ - MPU9250(const MPU9250 &); - MPU9250 operator=(const MPU9250 &); + void check_registers(); }; diff --git a/src/drivers/imu/mpu9250/mpu9250_i2c.cpp b/src/drivers/imu/mpu9250/mpu9250_i2c.cpp index e3f5a8079b..b47419e8d3 100644 --- a/src/drivers/imu/mpu9250/mpu9250_i2c.cpp +++ b/src/drivers/imu/mpu9250/mpu9250_i2c.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-2019 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 @@ -43,7 +43,7 @@ #ifdef USE_I2C -device::Device *MPU9250_I2C_interface(int bus, uint32_t address, bool external_bus); +device::Device *MPU9250_I2C_interface(int bus, uint32_t address); class MPU9250_I2C : public device::I2C { @@ -62,7 +62,7 @@ private: }; device::Device * -MPU9250_I2C_interface(int bus, uint32_t address, bool external_bus) +MPU9250_I2C_interface(int bus, uint32_t address) { return new MPU9250_I2C(bus, address); } @@ -76,7 +76,7 @@ MPU9250_I2C::MPU9250_I2C(int bus, uint32_t address) : int MPU9250_I2C::write(unsigned reg_speed, void *data, unsigned count) { - uint8_t cmd[MPU_MAX_WRITE_BUFFER_SIZE] {}; + uint8_t cmd[2] {}; if (sizeof(cmd) < (count + 1)) { return -EIO; diff --git a/src/drivers/imu/mpu9250/mpu9250_main.cpp b/src/drivers/imu/mpu9250/mpu9250_main.cpp index e1e50262f8..53e04b9a2d 100644 --- a/src/drivers/imu/mpu9250/mpu9250_main.cpp +++ b/src/drivers/imu/mpu9250/mpu9250_main.cpp @@ -42,13 +42,10 @@ * based on the mpu6000 driver */ -#include "mpu9250.h" +#include +#include -#define MPU_DEVICE_PATH "/dev/mpu9250" -#define MPU_DEVICE_PATH_1 "/dev/mpu9250_1" -#define MPU_DEVICE_PATH_EXT "/dev/mpu9250_ext" -#define MPU_DEVICE_PATH_EXT_1 "/dev/mpu9250_ext_1" -#define MPU_DEVICE_PATH_EXT_2 "/dev/mpu9250_ext_2" +#include "mpu9250.h" /** driver 'main' command */ extern "C" { __EXPORT int mpu9250_main(int argc, char *argv[]); } @@ -68,94 +65,90 @@ enum MPU9250_BUS { namespace mpu9250 { -/* - list of supported bus configurations - */ - +// list of supported bus configurations struct mpu9250_bus_option { enum MPU9250_BUS busid; - const char *path; MPU9250_constructor interface_constructor; bool magpassthrough; uint8_t busnum; uint32_t address; MPU9250 *dev; } bus_options[] = { -#if defined (USE_I2C) +#if defined(USE_I2C) # if defined(PX4_I2C_BUS_ONBOARD) && defined(PX4_I2C_OBDEV_MPU9250) - { MPU9250_BUS_I2C_INTERNAL, MPU_DEVICE_PATH, &MPU9250_I2C_interface, false, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_MPU9250, nullptr }, + { MPU9250_BUS_I2C_INTERNAL, &MPU9250_I2C_interface, false, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_MPU9250, nullptr }, # endif -# if defined(PX4_I2C_BUS_EXPANSION) -# if defined(PX4_I2C_OBDEV_MPU9250) - { MPU9250_BUS_I2C_EXTERNAL, MPU_DEVICE_PATH_EXT, &MPU9250_I2C_interface, false, PX4_I2C_BUS_EXPANSION, PX4_I2C_OBDEV_MPU9250, nullptr }, +# if defined(PX4_I2C_BUS_EXPANSION) && defined(PX4_I2C_OBDEV_MPU9250) + { MPU9250_BUS_I2C_EXTERNAL, &MPU9250_I2C_interface, false, PX4_I2C_BUS_EXPANSION, PX4_I2C_OBDEV_MPU9250, nullptr }, # endif -#endif # if defined(PX4_I2C_BUS_EXPANSION1) && defined(PX4_I2C_OBDEV_MPU9250) - { MPU9250_BUS_I2C_EXTERNAL, MPU_DEVICE_PATH_EXT_1, &MPU9250_I2C_interface, false, PX4_I2C_BUS_EXPANSION1, PX4_I2C_OBDEV_MPU9250, nullptr }, + { MPU9250_BUS_I2C_EXTERNAL, &MPU9250_I2C_interface, false, PX4_I2C_BUS_EXPANSION1, PX4_I2C_OBDEV_MPU9250, nullptr }, # endif # if defined(PX4_I2C_BUS_EXPANSION2) && defined(PX4_I2C_OBDEV_MPU9250) - { MPU9250_BUS_I2C_EXTERNAL, MPU_DEVICE_PATH_EXT_2, &MPU9250_I2C_interface, false, PX4_I2C_BUS_EXPANSION2, PX4_I2C_OBDEV_MPU9250, nullptr }, + { MPU9250_BUS_I2C_EXTERNAL, &MPU9250_I2C_interface, false, PX4_I2C_BUS_EXPANSION2, PX4_I2C_OBDEV_MPU9250, nullptr }, # endif #endif -#ifdef PX4_SPIDEV_MPU - { MPU9250_BUS_SPI_INTERNAL, MPU_DEVICE_PATH, &MPU9250_SPI_interface, true, PX4_SPI_BUS_SENSORS, PX4_SPIDEV_MPU, nullptr }, +#if defined(PX4_SPI_BUS_SENSORS) && defined(PX4_SPIDEV_MPU) + { MPU9250_BUS_SPI_INTERNAL, &MPU9250_SPI_interface, true, PX4_SPI_BUS_SENSORS, PX4_SPIDEV_MPU, nullptr }, #endif -#ifdef PX4_SPIDEV_MPU2 - { MPU9250_BUS_SPI_INTERNAL2, MPU_DEVICE_PATH_1, &MPU9250_SPI_interface, true, PX4_SPI_BUS_SENSORS, PX4_SPIDEV_MPU2, nullptr }, +#if defined(PX4_SPI_BUS_SENSORS) && defined(PX4_SPIDEV_MPU2) + { MPU9250_BUS_SPI_INTERNAL2, &MPU9250_SPI_interface, true, PX4_SPI_BUS_SENSORS, PX4_SPIDEV_MPU2, nullptr }, #endif #if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_MPU) - { MPU9250_BUS_SPI_EXTERNAL, MPU_DEVICE_PATH_EXT, &MPU9250_SPI_interface, true, PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_MPU, nullptr }, + { MPU9250_BUS_SPI_EXTERNAL, &MPU9250_SPI_interface, true, PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_MPU, nullptr }, #endif }; #define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) -void start(enum MPU9250_BUS busid, enum Rotation rotation, bool external_bus, bool magnetometer_only); -bool start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, bool external_bus, bool magnetometer_only); -struct mpu9250_bus_option &find_bus(enum MPU9250_BUS busid); -void stop(enum MPU9250_BUS busid); -void info(enum MPU9250_BUS busid); -void usage(); +bool start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation); +mpu9250_bus_option *find_bus(enum MPU9250_BUS busid); + +int start(enum MPU9250_BUS busid, enum Rotation rotation); +int stop(enum MPU9250_BUS busid); +int info(enum MPU9250_BUS busid); +int usage(); /** * find a bus structure for a busid */ -struct mpu9250_bus_option &find_bus(enum MPU9250_BUS busid) +struct mpu9250_bus_option *find_bus(enum MPU9250_BUS busid) { for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) { if ((busid == MPU9250_BUS_ALL || busid == bus_options[i].busid) && bus_options[i].dev != nullptr) { - return bus_options[i]; + return &bus_options[i]; } } - errx(1, "bus %u not started", (unsigned)busid); + PX4_ERR("bus %u not started", (unsigned)busid); + return nullptr; } /** * start driver for a specific bus option */ bool -start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, bool external, bool magnetometer_only) +start_bus(mpu9250_bus_option &bus, enum Rotation rotation) { PX4_INFO("Bus probed: %d", bus.busid); if (bus.dev != nullptr) { - warnx("%s SPI not available", external ? "External" : "Internal"); + PX4_ERR("SPI %d not available", bus.busid); return false; } - device::Device *interface = bus.interface_constructor(bus.busnum, bus.address, external); + device::Device *interface = bus.interface_constructor(bus.busnum, bus.address); if (interface == nullptr) { - warnx("no device on bus %u", (unsigned)bus.busid); + PX4_WARN("no device on bus %u", (unsigned)bus.busid); return false; } if (interface->init() != OK) { delete interface; - warnx("no device on bus %u", (unsigned)bus.busid); + PX4_WARN("no device on bus %u", (unsigned)bus.busid); return false; } @@ -163,15 +156,15 @@ start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, bool external, #ifdef USE_I2C /* For i2c interfaces, connect to the magnetomer directly */ - bool is_i2c = bus.busid == MPU9250_BUS_I2C_INTERNAL || bus.busid == MPU9250_BUS_I2C_EXTERNAL; + const bool is_i2c = bus.busid == MPU9250_BUS_I2C_INTERNAL || bus.busid == MPU9250_BUS_I2C_EXTERNAL; if (is_i2c) { - mag_interface = AK8963_I2C_interface(bus.busnum, external); + mag_interface = AK8963_I2C_interface(bus.busnum); } #endif - bus.dev = new MPU9250(interface, mag_interface, bus.path, rotation, magnetometer_only); + bus.dev = new MPU9250(interface, mag_interface, rotation); if (bus.dev == nullptr) { delete interface; @@ -196,7 +189,9 @@ fail: bus.dev = nullptr; } - errx(1, "driver start failed"); + PX4_ERR("driver start failed"); + + return false; } /** @@ -205,10 +200,9 @@ fail: * This function only returns if the driver is up and running * or failed to detect the sensor. */ -void -start(enum MPU9250_BUS busid, enum Rotation rotation, bool external, bool magnetometer_only) +int +start(enum MPU9250_BUS busid, enum Rotation rotation) { - bool started = false; for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) { @@ -222,56 +216,53 @@ start(enum MPU9250_BUS busid, enum Rotation rotation, bool external, bool magnet continue; } - started |= start_bus(bus_options[i], rotation, external, magnetometer_only); + started |= start_bus(bus_options[i], rotation); if (started) { break; } } - exit(started ? 0 : 1); - + return PX4_OK; } -void +int stop(enum MPU9250_BUS busid) { - struct mpu9250_bus_option &bus = find_bus(busid); + mpu9250_bus_option *bus = find_bus(busid); - - if (bus.dev != nullptr) { - delete bus.dev; - bus.dev = nullptr; + if (bus != nullptr && bus->dev != nullptr) { + delete bus->dev; + bus->dev = nullptr; } else { /* warn, but not an error */ - warnx("already stopped."); + PX4_WARN("already stopped."); } - exit(0); + return PX4_OK; } /** * Print a little info about the driver. */ -void +int info(enum MPU9250_BUS busid) { - struct mpu9250_bus_option &bus = find_bus(busid); + mpu9250_bus_option *bus = find_bus(busid); - - if (bus.dev == nullptr) { - errx(1, "driver not running"); + if (bus != nullptr && bus->dev != nullptr) { + PX4_WARN("driver not running"); + return PX4_ERROR; } - printf("state @ %p\n", bus.dev); - bus.dev->print_info(); + bus->dev->print_info(); - exit(0); + return PX4_OK; } -void +int usage() { - PX4_INFO("missing command: try 'start', 'info', 'test', 'stop',\n 'regdump', 'testerror'"); + PX4_INFO("missing command: try 'start', 'stop', 'info'"); PX4_INFO("options:"); PX4_INFO(" -X (i2c external bus)"); PX4_INFO(" -I (i2c internal bus)"); @@ -279,7 +270,8 @@ usage() PX4_INFO(" -S (spi external bus)"); PX4_INFO(" -t (spi internal bus, 2nd instance)"); PX4_INFO(" -R rotation"); - PX4_INFO(" -M only enable magnetometer, accel/gyro disabled - not av. on MPU6500"); + + return PX4_OK; } } // namespace @@ -293,7 +285,6 @@ mpu9250_main(int argc, char *argv[]) enum MPU9250_BUS busid = MPU9250_BUS_ALL; enum Rotation rotation = ROTATION_NONE; - bool magnetometer_only = false; while ((ch = px4_getopt(argc, argv, "XISstMR:", &myoptind, &myoptarg)) != EOF) { switch (ch) { @@ -321,42 +312,34 @@ mpu9250_main(int argc, char *argv[]) rotation = (enum Rotation)atoi(myoptarg); break; - case 'M': - magnetometer_only = true; - break; - default: - mpu9250::usage(); - return 0; + return mpu9250::usage(); } } if (myoptind >= argc) { - mpu9250::usage(); - return -1; + return mpu9250::usage(); } - bool external = busid == MPU9250_BUS_I2C_EXTERNAL || busid == MPU9250_BUS_SPI_EXTERNAL; const char *verb = argv[myoptind]; /* * Start/load the driver. */ if (!strcmp(verb, "start")) { - mpu9250::start(busid, rotation, external, magnetometer_only); + return mpu9250::start(busid, rotation); } if (!strcmp(verb, "stop")) { - mpu9250::stop(busid); + return mpu9250::stop(busid); } /* * Print driver information. */ if (!strcmp(verb, "info")) { - mpu9250::info(busid); + return mpu9250::info(busid); } - mpu9250::usage(); - return 0; + return mpu9250::usage(); } diff --git a/src/drivers/imu/mpu9250/mpu9250_spi.cpp b/src/drivers/imu/mpu9250/mpu9250_spi.cpp index 122c2a7c53..1d0f49408b 100644 --- a/src/drivers/imu/mpu9250/mpu9250_spi.cpp +++ b/src/drivers/imu/mpu9250/mpu9250_spi.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-2019 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 @@ -59,7 +59,7 @@ #define MPU9250_LOW_SPI_BUS_SPEED 1000*1000 #define MPU9250_HIGH_SPI_BUS_SPEED 20*1000*1000 -device::Device *MPU9250_SPI_interface(int bus, uint32_t cs, bool external_bus); +device::Device *MPU9250_SPI_interface(int bus, uint32_t cs); class MPU9250_SPI : public device::SPI { @@ -80,19 +80,11 @@ private: }; device::Device * -MPU9250_SPI_interface(int bus, uint32_t cs, bool external_bus) +MPU9250_SPI_interface(int bus, uint32_t cs) { device::Device *interface = nullptr; - if (external_bus) { -#if !(defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_MPU)) - errx(0, "External SPI not available"); -#endif - } - - if (cs != SPIDEV_NONE(0)) { - interface = new MPU9250_SPI(bus, cs); - } + interface = new MPU9250_SPI(bus, cs); return interface; } @@ -116,14 +108,13 @@ MPU9250_SPI::set_bus_frequency(unsigned ®_speed) int MPU9250_SPI::write(unsigned reg_speed, void *data, unsigned count) { - uint8_t cmd[MPU_MAX_WRITE_BUFFER_SIZE] {}; + uint8_t cmd[2] {}; if (sizeof(cmd) < (count + 1)) { return -EIO; } /* Set the desired speed and isolate the register */ - set_bus_frequency(reg_speed); cmd[0] = reg_speed | DIR_WRITE; diff --git a/src/lib/drivers/device/CMakeLists.txt b/src/lib/drivers/device/CMakeLists.txt index 5162cb39ab..d1830466fc 100644 --- a/src/lib/drivers/device/CMakeLists.txt +++ b/src/lib/drivers/device/CMakeLists.txt @@ -40,6 +40,11 @@ if (${PX4_PLATFORM} STREQUAL "nuttx") if ("${CONFIG_SPI}" STREQUAL "y") list(APPEND SRCS_PLATFORM nuttx/SPI.cpp) endif() +elseif((${PX4_PLATFORM} MATCHES "qurt")) + list(APPEND SRCS_PLATFORM + qurt/I2C.cpp + qurt/SPI.cpp + ) elseif(UNIX AND NOT APPLE AND NOT (${PX4_PLATFORM} MATCHES "qurt")) #TODO: add linux PX4 platform type # Linux I2Cdev and SPIdev list(APPEND SRCS_PLATFORM diff --git a/src/lib/drivers/device/i2c.h b/src/lib/drivers/device/i2c.h index 9a0f7391a5..ce47516e65 100644 --- a/src/lib/drivers/device/i2c.h +++ b/src/lib/drivers/device/i2c.h @@ -34,6 +34,8 @@ #ifdef __PX4_NUTTX #include "nuttx/I2C.hpp" +#elif __PX4_QURT +#include "qurt/I2C.hpp" #else #include "posix/I2C.hpp" #endif @@ -56,4 +58,4 @@ static const int i2c_bus_options[] = { #endif }; -#define NUM_I2C_BUS_OPTIONS (sizeof(i2c_bus_options)/sizeof(i2c_bus_options[0])) \ No newline at end of file +#define NUM_I2C_BUS_OPTIONS (sizeof(i2c_bus_options)/sizeof(i2c_bus_options[0])) diff --git a/src/lib/drivers/device/posix/I2C.cpp b/src/lib/drivers/device/posix/I2C.cpp index 266fd93d29..ef36311cf1 100644 --- a/src/lib/drivers/device/posix/I2C.cpp +++ b/src/lib/drivers/device/posix/I2C.cpp @@ -43,18 +43,9 @@ #include "I2C.hpp" #ifdef __PX4_LINUX + #include #include -#endif - -#ifdef __PX4_QURT -#define PX4_SIMULATE_I2C 1 -#else -#define PX4_SIMULATE_I2C 0 -#endif - -static constexpr const int simulate = PX4_SIMULATE_I2C; - namespace device { @@ -74,9 +65,7 @@ I2C::I2C(const char *name, const char *devname, const int bus, const uint16_t ad I2C::~I2C() { if (_fd >= 0) { -#ifndef __PX4_QURT ::close(_fd); -#endif /* !__PX4_QURT */ _fd = -1; } } @@ -95,24 +84,15 @@ I2C::init() return ret; } - if (simulate) { - _fd = 10000; + // Open the actual I2C device + char dev_path[16]; + snprintf(dev_path, sizeof(dev_path), "/dev/i2c-%i", get_device_bus()); + _fd = ::open(dev_path, O_RDWR); - } else { -#ifndef __PX4_QURT - - // Open the actual I2C device - char dev_path[16]; - snprintf(dev_path, sizeof(dev_path), "/dev/i2c-%i", get_device_bus()); - _fd = ::open(dev_path, O_RDWR); - - if (_fd < 0) { - PX4_ERR("could not open %s", dev_path); - px4_errno = errno; - return PX4_ERROR; - } - -#endif /* !__PX4_QURT */ + if (_fd < 0) { + PX4_ERR("could not open %s", dev_path); + px4_errno = errno; + return PX4_ERROR; } return ret; @@ -121,9 +101,6 @@ I2C::init() int I2C::transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len) { -#ifndef __PX4_LINUX - return PX4_ERROR; -#else struct i2c_msg msgv[2]; unsigned msgs; int ret = PX4_ERROR; @@ -164,20 +141,14 @@ I2C::transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const packets.nmsgs = msgs; - if (simulate) { - DEVICE_DEBUG("I2C SIM: transfer_4 on %s", get_devname()); - ret = PX4_OK; + ret = ::ioctl(_fd, I2C_RDWR, (unsigned long)&packets); + + if (ret == -1) { + DEVICE_DEBUG("I2C transfer failed"); + ret = PX4_ERROR; } else { - ret = ::ioctl(_fd, I2C_RDWR, (unsigned long)&packets); - - if (ret == -1) { - DEVICE_DEBUG("I2C transfer failed"); - ret = PX4_ERROR; - - } else { - ret = PX4_OK; - } + ret = PX4_OK; } /* success */ @@ -188,7 +159,8 @@ I2C::transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const } while (retry_count++ < _retries); return ret; -#endif } } // namespace device + +#endif // __PX4_LINUX diff --git a/src/lib/drivers/device/qurt/I2C.cpp b/src/lib/drivers/device/qurt/I2C.cpp new file mode 100644 index 0000000000..abe50772ae --- /dev/null +++ b/src/lib/drivers/device/qurt/I2C.cpp @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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. + * + ****************************************************************************/ + +/** + * @file i2c.cpp + * + * Base class for devices attached via the I2C bus. + * + * @todo Bus frequency changes; currently we do nothing with the value + * that is supplied. Should we just depend on the bus knowing? + */ + +#include "I2C.hpp" + +#include "dev_fs_lib_i2c.h" + +namespace device +{ + +I2C::I2C(const char *name, const char *devname, const int bus, const uint16_t address, const uint32_t frequency) : + CDev(name, devname) +{ + DEVICE_DEBUG("I2C::I2C name = %s devname = %s", name, devname); + // fill in _device_id fields for a I2C device + _device_id.devid_s.bus_type = DeviceBusType_I2C; + _device_id.devid_s.bus = bus; + _device_id.devid_s.address = address; + // devtype needs to be filled in by the driver + _device_id.devid_s.devtype = 0; +} + +I2C::~I2C() +{ + if (_fd >= 0) { + ::close(_fd); + _fd = -1; + } +} + +int +I2C::init() +{ + // Assume the driver set the desired bus frequency. There is no standard + // way to set it from user space. + + // do base class init, which will create device node, etc + int ret = CDev::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("CDev::init failed"); + return ret; + } + + // Open the actual I2C device + char dev_path[16]; + snprintf(dev_path, sizeof(dev_path), "/dev/iic-%i", get_device_bus()); + _fd = ::open(dev_path, O_RDWR); + + if (_fd < 0) { + PX4_ERR("could not open %s", dev_path); + px4_errno = errno; + return PX4_ERROR; + } + + return ret; +} + +int +I2C::transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len) +{ + dspal_i2c_ioctl_combined_write_read ioctl_write_read{}; + + ioctl_write_read.write_buf = (uint8_t *)send; + ioctl_write_read.write_buf_len = send_len; + ioctl_write_read.read_buf = recv; + ioctl_write_read.read_buf_len = recv_len; + + int bytes_read = ::ioctl(_fd, I2C_IOCTL_RDWR, &ioctl_write_read); + + if (bytes_read != (ssize_t)recv_len) { + PX4_ERR("read register reports a read of %d bytes, but attempted to read %d bytes", bytes_read, recv_len); + return -1; + } + + return 0; +} + +} // namespace device diff --git a/src/lib/drivers/device/qurt/I2C.hpp b/src/lib/drivers/device/qurt/I2C.hpp new file mode 100644 index 0000000000..cc1f36ecef --- /dev/null +++ b/src/lib/drivers/device/qurt/I2C.hpp @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 i2c.h + * + * Base class for devices connected via I2C. + */ + +#ifndef _DEVICE_I2C_H +#define _DEVICE_I2C_H + +#include "../CDev.hpp" + +#include + +#ifdef __PX4_LINUX +#include +#include +#endif + +namespace device __EXPORT +{ + +/** + * Abstract class for character device on I2C + */ +class __EXPORT I2C : public CDev +{ + +public: + + // no copy, assignment, move, move assignment + I2C(const I2C &) = delete; + I2C &operator=(const I2C &) = delete; + I2C(I2C &&) = delete; + I2C &operator=(I2C &&) = delete; + + virtual int init() override; + +protected: + /** + * The number of times a read or write operation will be retried on + * error. + */ + uint8_t _retries{0}; + + /** + * @ Constructor + * + * @param name Driver name + * @param devname Device node name + * @param bus I2C bus on which the device lives + * @param address I2C bus address, or zero if set_address will be used + * @param frequency I2C bus frequency for the device (currently not used) + */ + I2C(const char *name, const char *devname, const int bus, const uint16_t address, const uint32_t frequency); + virtual ~I2C(); + + /** + * Check for the presence of the device on the bus. + */ + virtual int probe() { return PX4_OK; } + + /** + * Perform an I2C transaction to the device. + * + * At least one of send_len and recv_len must be non-zero. + * + * @param send Pointer to bytes to send. + * @param send_len Number of bytes to send. + * @param recv Pointer to buffer for bytes received. + * @param recv_len Number of bytes to receive. + * @return OK if the transfer was successful, -errno + * otherwise. + */ + int transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len); + + virtual bool external() const override { return px4_i2c_bus_external(_device_id.devid_s.bus); } + +private: + int _fd{-1}; + +}; + +} // namespace device + +#endif /* _DEVICE_I2C_H */ diff --git a/src/lib/drivers/device/qurt/SPI.cpp b/src/lib/drivers/device/qurt/SPI.cpp new file mode 100644 index 0000000000..28b00931d9 --- /dev/null +++ b/src/lib/drivers/device/qurt/SPI.cpp @@ -0,0 +1,167 @@ +/**************************************************************************** + * + * Copyright (C) 2019 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 SPI.cpp + * + * Base class for devices connected via SPI. + * + */ + +#include "SPI.hpp" + +#include +#include +#include +#include "dev_fs_lib_spi.h" + +#include + +namespace device +{ + +SPI::SPI(const char *name, const char *devname, int bus, uint32_t device, enum spi_mode_e mode, uint32_t frequency) : + CDev(name, devname), + _device(device), + _mode(mode), + _frequency(frequency) +{ + DEVICE_DEBUG("SPI::SPI name = %s devname = %s", name, devname); + + // fill in _device_id fields for a SPI device + _device_id.devid_s.bus_type = DeviceBusType_SPI; + _device_id.devid_s.bus = bus; + _device_id.devid_s.address = (uint8_t)device; + // devtype needs to be filled in by the driver + _device_id.devid_s.devtype = 0; +} + +SPI::~SPI() +{ + if (_fd >= 0) { + ::close(_fd); + _fd = -1; + } +} + +int +SPI::init() +{ + // Open the actual SPI device + char dev_path[16]; + snprintf(dev_path, sizeof(dev_path), "dev/spi-%lu", PX4_SPI_DEV_ID(_device)); + DEVICE_DEBUG("%s", dev_path); + _fd = ::open(dev_path, O_RDWR); + + if (_fd < 0) { + PX4_ERR("could not open %s", dev_path); + return PX4_ERROR; + } + + /* call the probe function to check whether the device is present */ + int ret = probe(); + + if (ret != OK) { + DEVICE_DEBUG("probe failed"); + return ret; + } + + /* do base class init, which will create the device node, etc. */ + ret = CDev::init(); + + if (ret != OK) { + DEVICE_DEBUG("cdev init failed"); + return ret; + } + + /* tell the workd where we are */ + DEVICE_LOG("on SPI bus %d at %d (%u KHz)", get_device_bus(), PX4_SPI_DEV_ID(_device), _frequency / 1000); + + return PX4_OK; +} + +int +SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) +{ + if ((send == nullptr) && (recv == nullptr)) { + return -EINVAL; + } + + dspal_spi_ioctl_read_write ioctl_write_read{}; + ioctl_write_read.read_buffer = send; + ioctl_write_read.read_buffer_length = len; + ioctl_write_read.write_buffer = recv; + ioctl_write_read.write_buffer_length = len; + + int result = ::ioctl(_fd, SPI_IOCTL_RDWR, &ioctl_write_read); + + if (result < 0) { + PX4_ERR("transfer error %d", result); + return result; + } + + return result; +} + +int +SPI::transferhword(uint16_t *send, uint16_t *recv, unsigned len) +{ + if ((send == nullptr) && (recv == nullptr)) { + return -EINVAL; + } + + // int bits = 16; + // result = ::ioctl(_fd, SPI_IOC_WR_BITS_PER_WORD, &bits); + + // if (result == -1) { + // PX4_ERR("can’t set 16 bit spi mode"); + // return PX4_ERROR; + // } + + dspal_spi_ioctl_read_write ioctl_write_read{}; + ioctl_write_read.read_buffer = send; + ioctl_write_read.read_buffer_length = len * 2; + ioctl_write_read.write_buffer = recv; + ioctl_write_read.write_buffer_length = len * 2; + + int result = ::ioctl(_fd, SPI_IOCTL_RDWR, &ioctl_write_read); + + if (result < 0) { + PX4_ERR("transfer error %d", result); + return result; + } + + return result; +} + +} // namespace device diff --git a/src/lib/drivers/device/qurt/SPI.hpp b/src/lib/drivers/device/qurt/SPI.hpp new file mode 100644 index 0000000000..93c307581c --- /dev/null +++ b/src/lib/drivers/device/qurt/SPI.hpp @@ -0,0 +1,172 @@ +/**************************************************************************** + * + * Copyright (C) 2019 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 SPI.hpp + * + * Base class for devices connected via SPI. + */ + +#pragma once + +#include "../CDev.hpp" + +#include "dev_fs_lib_spi.h" + +enum spi_mode_e { + SPIDEV_MODE0 = 0, /* CPOL=0 CHPHA=0 */ + SPIDEV_MODE1 = 1, /* CPOL=0 CHPHA=1 */ + SPIDEV_MODE2 = 2, /* CPOL=1 CHPHA=0 */ + SPIDEV_MODE3 = 3 /* CPOL=1 CHPHA=1 */ +}; + +namespace device __EXPORT +{ + +/** + * Abstract class for character device on SPI + */ +class __EXPORT SPI : public CDev +{ +protected: + /** + * Constructor + * + * @param name Driver name + * @param devname Device node name + * @param bus SPI bus on which the device lives + * @param device Device handle (used by SPI_SELECT) + * @param mode SPI clock/data mode + * @param frequency SPI clock frequency + */ + SPI(const char *name, const char *devname, int bus, uint32_t device, enum spi_mode_e mode, uint32_t frequency); + virtual ~SPI(); + + /** + * Locking modes supported by the driver. + */ + enum LockMode { + LOCK_PREEMPTION, /**< the default; lock against all forms of preemption. */ + LOCK_THREADS, /**< lock only against other threads, using SPI_LOCK */ + LOCK_NONE /**< perform no locking, only safe if the bus is entirely private */ + }; + + virtual int init(); + + /** + * Check for the presence of the device on the bus. + */ + virtual int probe() { return PX4_OK; } + + /** + * Perform a SPI transfer. + * + * If called from interrupt context, this interface does not lock + * the bus and may interfere with non-interrupt-context callers. + * + * Clients in a mixed interrupt/non-interrupt configuration must + * ensure appropriate interlocking. + * + * At least one of send or recv must be non-null. + * + * @param send Bytes to send to the device, or nullptr if + * no data is to be sent. + * @param recv Buffer for receiving bytes from the device, + * or nullptr if no bytes are to be received. + * @param len Number of bytes to transfer. + * @return OK if the exchange was successful, -errno + * otherwise. + */ + int transfer(uint8_t *send, uint8_t *recv, unsigned len); + + /** + * Perform a SPI 16 bit transfer. + * + * If called from interrupt context, this interface does not lock + * the bus and may interfere with non-interrupt-context callers. + * + * Clients in a mixed interrupt/non-interrupt configuration must + * ensure appropriate interlocking. + * + * At least one of send or recv must be non-null. + * + * @param send Words to send to the device, or nullptr if + * no data is to be sent. + * @param recv Words for receiving bytes from the device, + * or nullptr if no bytes are to be received. + * @param len Number of words to transfer. + * @return OK if the exchange was successful, -errno + * otherwise. + */ + int transferhword(uint16_t *send, uint16_t *recv, unsigned len); + + /** + * Set the SPI bus frequency + * This is used to change frequency on the fly. Some sensors + * (such as the MPU6000) need a lower frequency for setup + * registers and can handle higher frequency for sensor + * value registers + * + * @param frequency Frequency to set (Hz) + */ + void set_frequency(uint32_t frequency) { _frequency = frequency; } + uint32_t get_frequency() { return _frequency; } + + /** + * Set the SPI bus locking mode + * + * This set the SPI locking mode. For devices competing with NuttX SPI + * drivers on a bus the right lock mode is LOCK_THREADS. + * + * @param mode Locking mode + */ + void set_lockmode(enum LockMode mode) {} + +private: + int _fd{-1}; + + uint32_t _device; + enum spi_mode_e _mode; + uint32_t _frequency; + + /* this class does not allow copying */ + SPI(const SPI &); + SPI operator=(const SPI &); + +protected: + + bool external() { return px4_spi_bus_external(get_device_bus()); } + +}; + +} // namespace device diff --git a/src/lib/drivers/device/spi.h b/src/lib/drivers/device/spi.h index 9ca739d180..5a3f4d4d37 100644 --- a/src/lib/drivers/device/spi.h +++ b/src/lib/drivers/device/spi.h @@ -34,6 +34,8 @@ #ifdef __PX4_NUTTX #include "nuttx/SPI.hpp" +#elif __PX4_QURT +#include "qurt/SPI.hpp" #else #include "posix/SPI.hpp" #endif