dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mpu9250: start building "NuttX" driver for Linux and QuRT

This commit is contained in:
Daniel Agar 2019-11-12 14:23:13 -05:00
parent 6bd4273b9c
commit c5520cbaca
36 changed files with 1159 additions and 869 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
boards/aerotenna/ocpoc/src/board_config.h
View File

@ -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 <system_config.h>
#include <px4_platform_common/board_common.h>

1
boards/aerotenna/ocpoc/ubuntu.cmake
View File

@ -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

1
boards/atlflight/eagle/qurt-default.cmake
View File

@ -46,6 +46,7 @@ px4_add_board(
DRIVERS
gps
imu/mpu9250
spektrum_rc
qshell/qurt
snapdragon_pwm_out

4
boards/atlflight/eagle/src/board_config.h
View File

@ -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 <system_config.h>
#include <px4_platform_common/board_common.h>

1
boards/atlflight/excelsior/qurt-default.cmake
View File

@ -46,6 +46,7 @@ px4_add_board(
DRIVERS
gps
imu/mpu9250
spektrum_rc
qshell/qurt
snapdragon_pwm_out

1
boards/beaglebone/blue/cross.cmake
View File

@ -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

1
boards/beaglebone/blue/native.cmake
View File

@ -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

1
boards/emlid/navio2/cross.cmake
View File

@ -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

1
boards/emlid/navio2/native.cmake
View File

@ -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

8
boards/emlid/navio2/src/board_config.h
View File

@ -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 <system_config.h>
#include <px4_platform_common/board_common.h>

22
platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp
View File

@ -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

4
platforms/common/px4_work_queue/WorkQueueManager.cpp
View File

@ -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;

158
src/drivers/imu/icm20948/ICM20948_mag.cpp
View File

@ -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;

86
src/drivers/imu/icm20948/ICM20948_mag.h
View File

@ -38,38 +38,31 @@
#include <drivers/device/Device.hpp>
/* 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 &);
};

241
src/drivers/imu/icm20948/icm20948.cpp
View File

@ -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 <px4_platform_common/px4_config.h>
@ -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();
}

131
src/drivers/imu/icm20948/icm20948.h
View File

@ -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 <stdint.h>
#include <perf/perf_counter.h>
#include <systemlib/conversions.h>
#include <drivers/drv_hrt.h>
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/conversion/rotation.h>
#include <systemlib/err.h>
#include <lib/ecl/geo/geo.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/uORB.h>
#include <lib/systemlib/conversions.h>
#include <lib/systemlib/px4_macros.h>
#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();
};

14
src/drivers/imu/icm20948/icm20948_i2c.cpp
View File

@ -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);
/*

144
src/drivers/imu/icm20948/icm20948_main.cpp
View File

@ -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 <px4_platform_common/px4_config.h>
@ -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();
}

40
src/drivers/imu/icm20948/icm20948_spi.cpp
View File

@ -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 <drivers/device/spi.h>
#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 &reg_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;

36
src/drivers/imu/icm20948/mag_i2c.cpp
View File

@ -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;
}

6
src/drivers/imu/mpu9250/AK8963_I2C.cpp
View File

@ -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;

42
src/drivers/imu/mpu9250/MPU9250_mag.cpp
View File

@ -40,6 +40,12 @@
*
*/
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/time.h>
#include <lib/perf/perf_counter.h>
#include <drivers/drv_hrt.h>
#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));
}
}

20
src/drivers/imu/mpu9250/MPU9250_mag.h
View File

@ -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 <cdev/CDev.hpp>
#include <perf/perf_counter.h>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <drivers/device/Device.hpp>
@ -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 &);
};

178
src/drivers/imu/mpu9250/mpu9250.cpp
View File

@ -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();
}

61
src/drivers/imu/mpu9250/mpu9250.h
View File

@ -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 <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/ecl/geo/geo.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <systemlib/conversions.h>
#include <systemlib/px4_macros.h>
#include <lib/systemlib/conversions.h>
#include <lib/systemlib/px4_macros.h>
#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();
};

8
src/drivers/imu/mpu9250/mpu9250_i2c.cpp
View File

@ -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;

147
src/drivers/imu/mpu9250/mpu9250_main.cpp
View File

@ -42,13 +42,10 @@
* based on the mpu6000 driver
*/
#include "mpu9250.h"
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/getopt.h>
#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();
}

19
src/drivers/imu/mpu9250/mpu9250_spi.cpp
View File

@ -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 &reg_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;

5
src/lib/drivers/device/CMakeLists.txt
View File

@ -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

4
src/lib/drivers/device/i2c.h
View File

@ -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]))
#define NUM_I2C_BUS_OPTIONS (sizeof(i2c_bus_options)/sizeof(i2c_bus_options[0]))

62
src/lib/drivers/device/posix/I2C.cpp
View File

@ -43,18 +43,9 @@
#include "I2C.hpp"
#ifdef __PX4_LINUX
#include <linux/i2c.h>
#include <linux/i2c-dev.h>
#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

118
src/lib/drivers/device/qurt/I2C.cpp Normal file
View File

@ -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

118
src/lib/drivers/device/qurt/I2C.hpp Normal file
View File

@ -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 <px4_platform_common/i2c.h>
#ifdef __PX4_LINUX
#include <linux/i2c.h>
#include <linux/i2c-dev.h>
#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 */

167
src/lib/drivers/device/qurt/SPI.cpp Normal file
View File

@ -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 <fcntl.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include "dev_fs_lib_spi.h"
#include <px4_platform_common/px4_config.h>
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

172
src/lib/drivers/device/qurt/SPI.hpp Normal file
View File

@ -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

2
src/lib/drivers/device/spi.h
View File

@ -34,6 +34,8 @@
#ifdef __PX4_NUTTX
#include "nuttx/SPI.hpp"
#elif __PX4_QURT
#include "qurt/SPI.hpp"
#else
#include "posix/SPI.hpp"
#endif