Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 01:35:35 +03:00
Code Issues Wiki Activity

Added runtime setting of gyro SPI pin

Browse source
This commit is contained in:
Martin Budden 2017-03-13 16:27:58 +00:00
parent a4e345ca54
commit b3ee1409e8
20 changed files with 205 additions and 162 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/drivers/accgyro.h
View file

@ -48,6 +48,7 @@ typedef struct gyroDev_s {
sensorGyroInterruptStatusFuncPtr intStatus; sensorGyroInterruptStatusFuncPtr intStatus;
sensorGyroUpdateFuncPtr update; sensorGyroUpdateFuncPtr update;
extiCallbackRec_t exti; extiCallbackRec_t exti;
busDevice_t bus;
float scale; // scalefactor float scale; // scalefactor
int16_t gyroADCRaw[XYZ_AXIS_COUNT]; int16_t gyroADCRaw[XYZ_AXIS_COUNT];
int16_t gyroZero[XYZ_AXIS_COUNT]; int16_t gyroZero[XYZ_AXIS_COUNT];
@ -64,6 +65,7 @@ typedef struct gyroDev_s {
typedef struct accDev_s { typedef struct accDev_s {
sensorAccInitFuncPtr init; // initialize function sensorAccInitFuncPtr init; // initialize function
sensorAccReadFuncPtr read; // read 3 axis data function sensorAccReadFuncPtr read; // read 3 axis data function
busDevice_t bus;
uint16_t acc_1G; uint16_t acc_1G;
int16_t ADCRaw[XYZ_AXIS_COUNT]; int16_t ADCRaw[XYZ_AXIS_COUNT];
char revisionCode; // a revision code for the sensor, if known char revisionCode; // a revision code for the sensor, if known

50
src/main/drivers/accgyro_mpu.c
View file

@ -75,7 +75,7 @@ static void mpu6050FindRevision(gyroDev_t *gyro)
// See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c // See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c
// determine product ID and accel revision // determine product ID and accel revision
ack = gyro->mpuConfiguration.readFn(MPU_RA_XA_OFFS_H, 6, readBuffer); ack = gyro->mpuConfiguration.readFn(&gyro->bus, MPU_RA_XA_OFFS_H, 6, readBuffer);
revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01); revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01);
if (revision) { if (revision) {
/* Congrats, these parts are better. */ /* Congrats, these parts are better. */
@ -89,7 +89,7 @@ static void mpu6050FindRevision(gyroDev_t *gyro)
failureMode(FAILURE_ACC_INCOMPATIBLE); failureMode(FAILURE_ACC_INCOMPATIBLE);
} }
} else { } else {
ack = gyro->mpuConfiguration.readFn(MPU_RA_PRODUCT_ID, 1, &productId); ack = gyro->mpuConfiguration.readFn(&gyro->bus, MPU_RA_PRODUCT_ID, 1, &productId);
revision = productId & 0x0F; revision = productId & 0x0F;
if (!revision) { if (!revision) {
failureMode(FAILURE_ACC_INCOMPATIBLE); failureMode(FAILURE_ACC_INCOMPATIBLE);
@ -158,14 +158,16 @@ static void mpuIntExtiInit(gyroDev_t *gyro)
#endif #endif
} }
bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data) bool mpuReadRegisterI2C(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t* data)
{ {
UNUSED(bus);
bool ack = i2cRead(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, length, data); bool ack = i2cRead(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, length, data);
return ack; return ack;
} }
bool mpuWriteRegisterI2C(uint8_t reg, uint8_t data) bool mpuWriteRegisterI2C(const busDevice_t *bus, uint8_t reg, uint8_t data)
{ {
UNUSED(bus);
bool ack = i2cWrite(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, data); bool ack = i2cWrite(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, data);
return ack; return ack;
} }
@ -174,7 +176,7 @@ bool mpuAccRead(accDev_t *acc)
{ {
uint8_t data[6]; uint8_t data[6];
bool ack = acc->mpuConfiguration.readFn(MPU_RA_ACCEL_XOUT_H, 6, data); bool ack = acc->mpuConfiguration.readFn(&acc->bus, MPU_RA_ACCEL_XOUT_H, 6, data);
if (!ack) { if (!ack) {
return false; return false;
} }
@ -197,7 +199,7 @@ bool mpuGyroRead(gyroDev_t *gyro)
{ {
uint8_t data[6]; uint8_t data[6];
const bool ack = gyro->mpuConfiguration.readFn(gyro->mpuConfiguration.gyroReadXRegister, 6, data); const bool ack = gyro->mpuConfiguration.readFn(&gyro->bus, gyro->mpuConfiguration.gyroReadXRegister, 6, data);
if (!ack) { if (!ack) {
return false; return false;
} }
@ -224,8 +226,12 @@ bool mpuCheckDataReady(gyroDev_t* gyro)
#ifdef USE_SPI #ifdef USE_SPI
static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
{ {
UNUSED(gyro); // since there are FCs which have gyro on I2C but other devices on SPI
#ifdef USE_GYRO_SPI_MPU6000 #ifdef USE_GYRO_SPI_MPU6000
if (mpu6000SpiDetect()) { #ifdef MPU6000_CS_PIN
gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU6000_CS_PIN)) : gyro->bus.spi.csnPin;
#endif
if (mpu6000SpiDetect(&gyro->bus)) {
gyro->mpuDetectionResult.sensor = MPU_60x0_SPI; gyro->mpuDetectionResult.sensor = MPU_60x0_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = mpu6000SpiReadRegister; gyro->mpuConfiguration.readFn = mpu6000SpiReadRegister;
@ -235,8 +241,11 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
#endif #endif
#ifdef USE_GYRO_SPI_MPU6500 #ifdef USE_GYRO_SPI_MPU6500
if (mpu6500SpiDetect()) { gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU6500_CS_PIN)) : gyro->bus.spi.csnPin;
gyro->mpuDetectionResult.sensor = MPU_65xx_SPI; const uint8_t mpu6500Sensor = mpu6500SpiDetect(&gyro->bus);
// some targets using MPU_9250_SPI, ICM_20608_SPI or ICM_20602_SPI state sensor is MPU_65xx_SPI
if (mpu6500Sensor != MPU_NONE) {
gyro->mpuDetectionResult.sensor = mpu6500Sensor;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = mpu6500SpiReadRegister; gyro->mpuConfiguration.readFn = mpu6500SpiReadRegister;
gyro->mpuConfiguration.writeFn = mpu6500SpiWriteRegister; gyro->mpuConfiguration.writeFn = mpu6500SpiWriteRegister;
@ -245,7 +254,8 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
#endif #endif
#ifdef USE_GYRO_SPI_MPU9250 #ifdef USE_GYRO_SPI_MPU9250
if (mpu9250SpiDetect()) { gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(MPU9250_CS_PIN)) : gyro->bus.spi.csnPin;
if (mpu9250SpiDetect(&gyro->bus)) {
gyro->mpuDetectionResult.sensor = MPU_9250_SPI; gyro->mpuDetectionResult.sensor = MPU_9250_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = mpu9250SpiReadRegister; gyro->mpuConfiguration.readFn = mpu9250SpiReadRegister;
@ -258,7 +268,8 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
#endif #endif
#ifdef USE_GYRO_SPI_ICM20608 #ifdef USE_GYRO_SPI_ICM20608
if (icm20608SpiDetect()) { gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(ICM20608_CS_PIN)) : gyro->bus.spi.csnPin;
if (icm20608SpiDetect(&gyro->bus)) {
mpuDetectionResult.sensor = ICM_20608_SPI; mpuDetectionResult.sensor = ICM_20608_SPI;
mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
mpuConfiguration.readFn = icm20608SpiReadRegister; mpuConfiguration.readFn = icm20608SpiReadRegister;
@ -268,7 +279,8 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
#endif #endif
#ifdef USE_GYRO_SPI_ICM20689 #ifdef USE_GYRO_SPI_ICM20689
if (icm20689SpiDetect()) { gyro->bus.spi.csnPin = gyro->bus.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(ICM20689_CS_PIN)) : gyro->bus.spi.csnPin;
if (icm20689SpiDetect(&gyro->bus)) {
gyro->mpuDetectionResult.sensor = ICM_20689_SPI; gyro->mpuDetectionResult.sensor = ICM_20689_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.readFn = icm20689SpiReadRegister; gyro->mpuConfiguration.readFn = icm20689SpiReadRegister;
@ -287,20 +299,18 @@ void mpuDetect(gyroDev_t *gyro)
// MPU datasheet specifies 30ms. // MPU datasheet specifies 30ms.
delay(35); delay(35);
#ifndef USE_I2C
uint8_t sig = 0; uint8_t sig = 0;
bool ack = false; #ifdef USE_I2C
bool ack = mpuReadRegisterI2C(&gyro->bus, MPU_RA_WHO_AM_I, 1, &sig);
#else #else
uint8_t sig; bool ack = false;
bool ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I, 1, &sig);
#endif #endif
if (ack) { if (ack) {
gyro->mpuConfiguration.readFn = mpuReadRegisterI2C; gyro->mpuConfiguration.readFn = mpuReadRegisterI2C;
gyro->mpuConfiguration.writeFn = mpuWriteRegisterI2C; gyro->mpuConfiguration.writeFn = mpuWriteRegisterI2C;
} else { } else {
#ifdef USE_SPI #ifdef USE_SPI
bool detectedSpiSensor = detectSPISensorsAndUpdateDetectionResult(gyro); detectSPISensorsAndUpdateDetectionResult(gyro);
UNUSED(detectedSpiSensor);
#endif #endif
return; return;
} }
@ -309,7 +319,7 @@ void mpuDetect(gyroDev_t *gyro)
// If an MPU3050 is connected sig will contain 0. // If an MPU3050 is connected sig will contain 0.
uint8_t inquiryResult; uint8_t inquiryResult;
ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I_LEGACY, 1, &inquiryResult); ack = mpuReadRegisterI2C(&gyro->bus, MPU_RA_WHO_AM_I_LEGACY, 1, &inquiryResult);
inquiryResult &= MPU_INQUIRY_MASK; inquiryResult &= MPU_INQUIRY_MASK;
if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) { if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) {
gyro->mpuDetectionResult.sensor = MPU_3050; gyro->mpuDetectionResult.sensor = MPU_3050;
@ -318,14 +328,12 @@ void mpuDetect(gyroDev_t *gyro)
} }
sig &= MPU_INQUIRY_MASK; sig &= MPU_INQUIRY_MASK;
if (sig == MPUx0x0_WHO_AM_I_CONST) { if (sig == MPUx0x0_WHO_AM_I_CONST) {
gyro->mpuDetectionResult.sensor = MPU_60x0; gyro->mpuDetectionResult.sensor = MPU_60x0;
mpu6050FindRevision(gyro); mpu6050FindRevision(gyro);
} else if (sig == MPU6500_WHO_AM_I_CONST) { } else if (sig == MPU6500_WHO_AM_I_CONST) {
gyro->mpuDetectionResult.sensor = MPU_65xx_I2C; gyro->mpuDetectionResult.sensor = MPU_65xx_I2C;
} }
return;
} }
void mpuGyroInit(gyroDev_t *gyro) void mpuGyroInit(gyroDev_t *gyro)

14
src/main/drivers/accgyro_mpu.h
View file

@ -20,6 +20,12 @@
#include "exti.h" #include "exti.h"
#include "sensor.h" #include "sensor.h"
//#define DEBUG_MPU_DATA_READY_INTERRUPT
#if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689)
#define GYRO_USES_SPI
#endif
// MPU6050 // MPU6050
#define MPU_RA_WHO_AM_I 0x75 #define MPU_RA_WHO_AM_I 0x75
#define MPU_RA_WHO_AM_I_LEGACY 0x00 #define MPU_RA_WHO_AM_I_LEGACY 0x00
@ -118,8 +124,8 @@
// RF = Register Flag // RF = Register Flag
#define MPU_RF_DATA_RDY_EN (1 << 0) #define MPU_RF_DATA_RDY_EN (1 << 0)
typedef bool (*mpuReadRegisterFnPtr)(uint8_t reg, uint8_t length, uint8_t* data); typedef bool (*mpuReadRegisterFnPtr)(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t* data);
typedef bool (*mpuWriteRegisterFnPtr)(uint8_t reg, uint8_t data); typedef bool (*mpuWriteRegisterFnPtr)(const busDevice_t *bus, uint8_t reg, uint8_t data);
typedef void(*mpuResetFnPtr)(void); typedef void(*mpuResetFnPtr)(void);
extern mpuResetFnPtr mpuResetFn; extern mpuResetFnPtr mpuResetFn;
@ -187,8 +193,8 @@ typedef struct mpuDetectionResult_s {
mpu6050Resolution_e resolution; mpu6050Resolution_e resolution;
} mpuDetectionResult_t; } mpuDetectionResult_t;
bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data); bool mpuReadRegisterI2C(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t* data);
bool mpuWriteRegisterI2C(uint8_t reg, uint8_t data); bool mpuWriteRegisterI2C(const busDevice_t *bus, uint8_t reg, uint8_t data);
struct gyroDev_s; struct gyroDev_s;
void mpuGyroInit(struct gyroDev_s *gyro); void mpuGyroInit(struct gyroDev_s *gyro);

12
src/main/drivers/accgyro_mpu3050.c
View file

@ -53,20 +53,20 @@ static void mpu3050Init(gyroDev_t *gyro)
delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe
ack = gyro->mpuConfiguration.writeFn(MPU3050_SMPLRT_DIV, 0); ack = gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_SMPLRT_DIV, 0);
if (!ack) if (!ack)
failureMode(FAILURE_ACC_INIT); failureMode(FAILURE_ACC_INIT);
gyro->mpuConfiguration.writeFn(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf); gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf);
gyro->mpuConfiguration.writeFn(MPU3050_INT_CFG, 0); gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_INT_CFG, 0);
gyro->mpuConfiguration.writeFn(MPU3050_USER_CTRL, MPU3050_USER_RESET); gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_USER_CTRL, MPU3050_USER_RESET);
gyro->mpuConfiguration.writeFn(MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX); gyro->mpuConfiguration.writeFn(&gyro->bus, MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
} }
static bool mpu3050ReadTemperature(gyroDev_t *gyro, int16_t *tempData) static bool mpu3050ReadTemperature(gyroDev_t *gyro, int16_t *tempData)
{ {
uint8_t buf[2]; uint8_t buf[2];
if (!gyro->mpuConfiguration.readFn(MPU3050_TEMP_OUT, 2, buf)) { if (!gyro->mpuConfiguration.readFn(&gyro->bus, MPU3050_TEMP_OUT, 2, buf)) {
return false; return false;
} }

16
src/main/drivers/accgyro_mpu6050.c
View file

@ -79,23 +79,23 @@ static void mpu6050GyroInit(gyroDev_t *gyro)
{ {
mpuGyroInit(gyro); mpuGyroInit(gyro);
bool ack = gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1 bool ack = gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
delay(100); delay(100);
ack = gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference) ack = gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
ack = gyro->mpuConfiguration.writeFn(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) ack = gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure
ack = gyro->mpuConfiguration.writeFn(MPU_RA_CONFIG, gyro->lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz) ack = gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_CONFIG, gyro->lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
ack = gyro->mpuConfiguration.writeFn(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec ack = gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
// ACC Init stuff. // ACC Init stuff.
// Accel scale 8g (4096 LSB/g) // Accel scale 8g (4096 LSB/g)
ack = gyro->mpuConfiguration.writeFn(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3); ack = gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
ack = gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG, ack = gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_PIN_CFG,
0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS 0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS
#ifdef USE_MPU_DATA_READY_SIGNAL #ifdef USE_MPU_DATA_READY_SIGNAL
ack = gyro->mpuConfiguration.writeFn(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); ack = gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
#endif #endif
UNUSED(ack); UNUSED(ack);
} }

22
src/main/drivers/accgyro_mpu6500.c
View file

@ -69,34 +69,34 @@ void mpu6500GyroInit(gyroDev_t *gyro)
} }
#endif #endif
gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET); gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
delay(100); delay(100);
gyro->mpuConfiguration.writeFn(MPU_RA_SIGNAL_PATH_RESET, 0x07); gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, 0x07);
delay(100); delay(100);
gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0); gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, 0);
delay(100); delay(100);
gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, INV_CLK_PLL); gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
delay(15); delay(15);
const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED); const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED);
gyro->mpuConfiguration.writeFn(MPU_RA_GYRO_CONFIG, raGyroConfigData); gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData);
delay(15); delay(15);
gyro->mpuConfiguration.writeFn(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3); gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
delay(15); delay(15);
gyro->mpuConfiguration.writeFn(MPU_RA_CONFIG, gyro->lpf); gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_CONFIG, gyro->lpf);
delay(15); delay(15);
gyro->mpuConfiguration.writeFn(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider
delay(100); delay(100);
// Data ready interrupt configuration // Data ready interrupt configuration
#ifdef USE_MPU9250_MAG #ifdef USE_MPU9250_MAG
gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN
#else #else
gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN
#endif #endif
delay(15); delay(15);
#ifdef USE_MPU_DATA_READY_SIGNAL #ifdef USE_MPU_DATA_READY_SIGNAL
gyro->mpuConfiguration.writeFn(MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable
#endif #endif
delay(15); delay(15);
} }

55
src/main/drivers/accgyro_spi_mpu6000.c
View file

@ -100,29 +100,27 @@ static bool mpuSpi6000InitDone = false;
#define MPU6000_REV_D9 0x59 #define MPU6000_REV_D9 0x59
#define MPU6000_REV_D10 0x5A #define MPU6000_REV_D10 0x5A
#define DISABLE_MPU6000 IOHi(mpuSpi6000CsPin) #define DISABLE_MPU6000(spiCsnPin) IOHi(spiCsnPin)
#define ENABLE_MPU6000 IOLo(mpuSpi6000CsPin) #define ENABLE_MPU6000(spiCsnPin) IOLo(spiCsnPin)
static IO_t mpuSpi6000CsPin = IO_NONE; bool mpu6000SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
bool mpu6000SpiWriteRegister(uint8_t reg, uint8_t data)
{ {
ENABLE_MPU6000; ENABLE_MPU6000(bus->spi.csnPin);
delayMicroseconds(1); delayMicroseconds(1);
spiTransferByte(MPU6000_SPI_INSTANCE, reg); spiTransferByte(MPU6000_SPI_INSTANCE, reg);
spiTransferByte(MPU6000_SPI_INSTANCE, data); spiTransferByte(MPU6000_SPI_INSTANCE, data);
DISABLE_MPU6000; DISABLE_MPU6000(bus->spi.csnPin);
delayMicroseconds(1); delayMicroseconds(1);
return true; return true;
} }
bool mpu6000SpiReadRegister(uint8_t reg, uint8_t length, uint8_t *data) bool mpu6000SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
{ {
ENABLE_MPU6000; ENABLE_MPU6000(bus->spi.csnPin);
spiTransferByte(MPU6000_SPI_INSTANCE, reg | 0x80); // read transaction spiTransferByte(MPU6000_SPI_INSTANCE, reg | 0x80); // read transaction
spiTransfer(MPU6000_SPI_INSTANCE, data, NULL, length); spiTransfer(MPU6000_SPI_INSTANCE, data, NULL, length);
DISABLE_MPU6000; DISABLE_MPU6000(bus->spi.csnPin);
return true; return true;
} }
@ -136,7 +134,7 @@ void mpu6000SpiGyroInit(gyroDev_t *gyro)
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
// Accel and Gyro DLPF Setting // Accel and Gyro DLPF Setting
mpu6000SpiWriteRegister(MPU6000_CONFIG, gyro->lpf); mpu6000SpiWriteRegister(&gyro->bus, MPU6000_CONFIG, gyro->lpf);
delayMicroseconds(1); delayMicroseconds(1);
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_FAST); // 18 MHz SPI clock spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_FAST); // 18 MHz SPI clock
@ -153,25 +151,22 @@ void mpu6000SpiAccInit(accDev_t *acc)
acc->acc_1G = 512 * 8; acc->acc_1G = 512 * 8;
} }
bool mpu6000SpiDetect(void) bool mpu6000SpiDetect(const busDevice_t *bus)
{ {
uint8_t in; uint8_t in;
uint8_t attemptsRemaining = 5; uint8_t attemptsRemaining = 5;
#ifdef MPU6000_CS_PIN IOInit(bus->spi.csnPin, OWNER_MPU, RESOURCE_SPI_CS, 0);
mpuSpi6000CsPin = IOGetByTag(IO_TAG(MPU6000_CS_PIN)); IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
#endif
IOInit(mpuSpi6000CsPin, OWNER_MPU, RESOURCE_SPI_CS, 0);
IOConfigGPIO(mpuSpi6000CsPin, SPI_IO_CS_CFG);
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
mpu6000SpiWriteRegister(MPU_RA_PWR_MGMT_1, BIT_H_RESET); mpu6000SpiWriteRegister(bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
do { do {
delay(150); delay(150);
mpu6000SpiReadRegister(MPU_RA_WHO_AM_I, 1, &in); mpu6000SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &in);
if (in == MPU6000_WHO_AM_I_CONST) { if (in == MPU6000_WHO_AM_I_CONST) {
break; break;
} }
@ -180,7 +175,7 @@ bool mpu6000SpiDetect(void)
} }
} while (attemptsRemaining--); } while (attemptsRemaining--);
mpu6000SpiReadRegister(MPU_RA_PRODUCT_ID, 1, &in); mpu6000SpiReadRegister(bus, MPU_RA_PRODUCT_ID, 1, &in);
/* look for a product ID we recognise */ /* look for a product ID we recognise */
@ -213,41 +208,41 @@ static void mpu6000AccAndGyroInit(gyroDev_t *gyro)
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
// Device Reset // Device Reset
mpu6000SpiWriteRegister(MPU_RA_PWR_MGMT_1, BIT_H_RESET); mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
delay(150); delay(150);
mpu6000SpiWriteRegister(MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP); mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP);
delay(150); delay(150);
// Clock Source PPL with Z axis gyro reference // Clock Source PPL with Z axis gyro reference
mpu6000SpiWriteRegister(MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
delayMicroseconds(15); delayMicroseconds(15);
// Disable Primary I2C Interface // Disable Primary I2C Interface
mpu6000SpiWriteRegister(MPU_RA_USER_CTRL, BIT_I2C_IF_DIS); mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_USER_CTRL, BIT_I2C_IF_DIS);
delayMicroseconds(15); delayMicroseconds(15);
mpu6000SpiWriteRegister(MPU_RA_PWR_MGMT_2, 0x00); mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_2, 0x00);
delayMicroseconds(15); delayMicroseconds(15);
// Accel Sample Rate 1kHz // Accel Sample Rate 1kHz
// Gyroscope Output Rate = 1kHz when the DLPF is enabled // Gyroscope Output Rate = 1kHz when the DLPF is enabled
mpu6000SpiWriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro));
delayMicroseconds(15); delayMicroseconds(15);
// Gyro +/- 1000 DPS Full Scale // Gyro +/- 1000 DPS Full Scale
mpu6000SpiWriteRegister(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
delayMicroseconds(15); delayMicroseconds(15);
// Accel +/- 8 G Full Scale // Accel +/- 8 G Full Scale
mpu6000SpiWriteRegister(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3); mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
delayMicroseconds(15); delayMicroseconds(15);
mpu6000SpiWriteRegister(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR
delayMicroseconds(15); delayMicroseconds(15);
#ifdef USE_MPU_DATA_READY_SIGNAL #ifdef USE_MPU_DATA_READY_SIGNAL
mpu6000SpiWriteRegister(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); mpu6000SpiWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
delayMicroseconds(15); delayMicroseconds(15);
#endif #endif

8
src/main/drivers/accgyro_spi_mpu6000.h
View file

@ -1,6 +1,8 @@
#pragma once #pragma once
#include "sensor.h"
#define MPU6000_CONFIG 0x1A #define MPU6000_CONFIG 0x1A
#define BITS_DLPF_CFG_256HZ 0x00 #define BITS_DLPF_CFG_256HZ 0x00
@ -15,10 +17,10 @@
// RF = Register Flag // RF = Register Flag
#define MPU_RF_DATA_RDY_EN (1 << 0) #define MPU_RF_DATA_RDY_EN (1 << 0)
bool mpu6000SpiDetect(void); bool mpu6000SpiDetect(const busDevice_t *bus);
bool mpu6000SpiAccDetect(accDev_t *acc); bool mpu6000SpiAccDetect(accDev_t *acc);
bool mpu6000SpiGyroDetect(gyroDev_t *gyro); bool mpu6000SpiGyroDetect(gyroDev_t *gyro);
bool mpu6000SpiWriteRegister(uint8_t reg, uint8_t data); bool mpu6000SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
bool mpu6000SpiReadRegister(uint8_t reg, uint8_t length, uint8_t *data); bool mpu6000SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);

39
src/main/drivers/accgyro_spi_mpu6500.c
View file

@ -20,6 +20,8 @@
#include "platform.h" #include "platform.h"
#if defined(USE_ACC_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6500)
#include "common/axis.h" #include "common/axis.h"
#include "common/maths.h" #include "common/maths.h"
@ -35,35 +37,34 @@
#include "accgyro_mpu6500.h" #include "accgyro_mpu6500.h"
#include "accgyro_spi_mpu6500.h" #include "accgyro_spi_mpu6500.h"
#if defined(USE_ACC_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6500) #define DISABLE_MPU6500(spiCsnPin) IOHi(spiCsnPin)
#define DISABLE_MPU6500 IOHi(mpuSpi6500CsPin) #define ENABLE_MPU6500(spiCsnPin) IOLo(spiCsnPin)
#define ENABLE_MPU6500 IOLo(mpuSpi6500CsPin)
static IO_t mpuSpi6500CsPin = IO_NONE; #define BIT_SLEEP 0x40
bool mpu6500SpiWriteRegister(uint8_t reg, uint8_t data) bool mpu6500SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
{ {
ENABLE_MPU6500; ENABLE_MPU6500(bus->spi.csnPin);
delayMicroseconds(1); delayMicroseconds(1);
spiTransferByte(MPU6500_SPI_INSTANCE, reg); spiTransferByte(MPU6500_SPI_INSTANCE, reg);
spiTransferByte(MPU6500_SPI_INSTANCE, data); spiTransferByte(MPU6500_SPI_INSTANCE, data);
DISABLE_MPU6500; DISABLE_MPU6500(bus->spi.csnPin);
delayMicroseconds(1); delayMicroseconds(1);
return true; return true;
} }
bool mpu6500SpiReadRegister(uint8_t reg, uint8_t length, uint8_t *data) bool mpu6500SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
{ {
ENABLE_MPU6500; ENABLE_MPU6500(bus->spi.csnPin);
spiTransferByte(MPU6500_SPI_INSTANCE, reg | 0x80); // read transaction spiTransferByte(MPU6500_SPI_INSTANCE, reg | 0x80); // read transaction
spiTransfer(MPU6500_SPI_INSTANCE, data, NULL, length); spiTransfer(MPU6500_SPI_INSTANCE, data, NULL, length);
DISABLE_MPU6500; DISABLE_MPU6500(bus->spi.csnPin);
return true; return true;
} }
static void mpu6500SpiInit(void) static void mpu6500SpiInit(const busDevice_t *bus)
{ {
static bool hardwareInitialised = false; static bool hardwareInitialised = false;
@ -71,22 +72,24 @@ static void mpu6500SpiInit(void)
return; return;
} }
mpuSpi6500CsPin = IOGetByTag(IO_TAG(MPU6500_CS_PIN)); IOInit(bus->spi.csnPin, OWNER_MPU, RESOURCE_SPI_CS, 0);
IOInit(mpuSpi6500CsPin, OWNER_MPU, RESOURCE_SPI_CS, 0); IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
IOConfigGPIO(mpuSpi6500CsPin, SPI_IO_CS_CFG);
spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST); spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST);
hardwareInitialised = true; hardwareInitialised = true;
} }
bool mpu6500SpiDetect(void) uint8_t mpu6500SpiDetect(const busDevice_t *bus)
{ {
mpu6500SpiInit(bus);
uint8_t tmp; uint8_t tmp;
mpu6500SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &tmp);
mpu6500SpiInit(); mpu6500SpiInit(bus);
mpu6500SpiReadRegister(MPU_RA_WHO_AM_I, 1, &tmp); mpu6500SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &tmp);
if (tmp == MPU6500_WHO_AM_I_CONST || if (tmp == MPU6500_WHO_AM_I_CONST ||
tmp == MPU9250_WHO_AM_I_CONST || tmp == MPU9250_WHO_AM_I_CONST ||
@ -123,7 +126,7 @@ void mpu6500SpiGyroInit(gyroDev_t *gyro)
mpu6500SpiGyroInit(gyro); mpu6500SpiGyroInit(gyro);
// Disable Primary I2C Interface // Disable Primary I2C Interface
mpu6500SpiWriteRegister(MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS); mpu6500SpiWriteRegister(&gyro->bus, MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS);
delay(100); delay(100);
spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST); spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST);

8
src/main/drivers/accgyro_spi_mpu6500.h
View file

@ -17,13 +17,15 @@
#pragma once #pragma once
bool mpu6500SpiDetect(void); #include "sensor.h"
uint8_t mpu6500SpiDetect(const busDevice_t *bus);
bool mpu6500SpiAccDetect(accDev_t *acc); bool mpu6500SpiAccDetect(accDev_t *acc);
bool mpu6500SpiGyroDetect(gyroDev_t *gyro); bool mpu6500SpiGyroDetect(gyroDev_t *gyro);
bool mpu6500SpiWriteRegister(uint8_t reg, uint8_t data); bool mpu6500SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
bool mpu6500SpiReadRegister(uint8_t reg, uint8_t length, uint8_t *data); bool mpu6500SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);
void mpu6500SpiGyroInit(gyroDev_t *gyro); void mpu6500SpiGyroInit(gyroDev_t *gyro);
void mpu6500SpiAccInit(accDev_t *acc); void mpu6500SpiAccInit(accDev_t *acc);

70
src/main/drivers/accgyro_spi_mpu9250.c
View file

@ -49,59 +49,57 @@
static bool mpuSpi9250InitDone = false; static bool mpuSpi9250InitDone = false;
static IO_t mpuSpi9250CsPin = IO_NONE; #define DISABLE_MPU9250(spiCsnPin) IOHi(spiCsnPin)
#define ENABLE_MPU9250(spiCsnPin) IOLo(spiCsnPin)
#define DISABLE_MPU9250 IOHi(mpuSpi9250CsPin) bool mpu9250SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
#define ENABLE_MPU9250 IOLo(mpuSpi9250CsPin)
bool mpu9250SpiReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
{ {
ENABLE_MPU9250; ENABLE_MPU9250(bus->spi.csnPin);
spiTransferByte(MPU9250_SPI_INSTANCE, reg | 0x80); // read transaction spiTransferByte(MPU9250_SPI_INSTANCE, reg | 0x80); // read transaction
spiTransfer(MPU9250_SPI_INSTANCE, data, NULL, length); spiTransfer(MPU9250_SPI_INSTANCE, data, NULL, length);
DISABLE_MPU9250; DISABLE_MPU9250(bus->spi.csnPin);
return true; return true;
} }
bool mpu9250SpiSlowReadRegister(uint8_t reg, uint8_t length, uint8_t *data) bool mpu9250SpiSlowReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data)
{ {
ENABLE_MPU9250; ENABLE_MPU9250(bus->spi.csnPin);
delayMicroseconds(1); delayMicroseconds(1);
spiTransferByte(MPU9250_SPI_INSTANCE, reg | 0x80); // read transaction spiTransferByte(MPU9250_SPI_INSTANCE, reg | 0x80); // read transaction
spiTransfer(MPU9250_SPI_INSTANCE, data, NULL, length); spiTransfer(MPU9250_SPI_INSTANCE, data, NULL, length);
DISABLE_MPU9250; DISABLE_MPU9250(bus->spi.csnPin);
delayMicroseconds(1); delayMicroseconds(1);
return true; return true;
} }
bool mpu9250SpiWriteRegister(uint8_t reg, uint8_t data) bool mpu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
{ {
ENABLE_MPU9250; ENABLE_MPU9250(bus->spi.csnPin);
delayMicroseconds(1); delayMicroseconds(1);
spiTransferByte(MPU9250_SPI_INSTANCE, reg); spiTransferByte(MPU9250_SPI_INSTANCE, reg);
spiTransferByte(MPU9250_SPI_INSTANCE, data); spiTransferByte(MPU9250_SPI_INSTANCE, data);
DISABLE_MPU9250; DISABLE_MPU9250(bus->spi.csnPin);
delayMicroseconds(1); delayMicroseconds(1);
return true; return true;
} }
bool verifympu9250SpiWriteRegister(uint8_t reg, uint8_t data) bool verifympu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
{ {
mpu9250SpiWriteRegister(reg, data); mpu9250SpiWriteRegister(bus, reg, data);
delayMicroseconds(100); delayMicroseconds(100);
uint8_t attemptsRemaining = 20; uint8_t attemptsRemaining = 20;
do { do {
uint8_t in; uint8_t in;
mpu9250SpiSlowReadRegister(reg, 1, &in); mpu9250SpiSlowReadRegister(bus, reg, 1, &in);
if (in == data) { if (in == data) {
return true; return true;
} else { } else {
debug[3]++; debug[3]++;
mpu9250SpiWriteRegister(reg, data); mpu9250SpiWriteRegister(bus, reg, data);
delayMicroseconds(100); delayMicroseconds(100);
} }
} while (attemptsRemaining--); } while (attemptsRemaining--);
@ -111,8 +109,11 @@ bool verifympu9250SpiWriteRegister(uint8_t reg, uint8_t data)
void mpu9250SpiResetGyro(void) void mpu9250SpiResetGyro(void)
{ {
// Device Reset // Device Reset
mpu9250SpiWriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); #ifdef MPU9250_CS_PIN
busDevice_t bus = { .spi = { .csnPin = IOGetByTag(IO_TAG(MPU9250_CS_PIN)) } };
mpu9250SpiWriteRegister(&bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
delay(150); delay(150);
#endif
} }
static void mpu9250AccAndGyroInit(gyroDev_t *gyro) static void mpu9250AccAndGyroInit(gyroDev_t *gyro)
@ -123,30 +124,30 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro)
spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed for writing to slow registers spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed for writing to slow registers
mpu9250SpiWriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); mpu9250SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
delay(50); delay(50);
verifympu9250SpiWriteRegister(MPU_RA_PWR_MGMT_1, INV_CLK_PLL); verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
//Fchoice_b defaults to 00 which makes fchoice 11 //Fchoice_b defaults to 00 which makes fchoice 11
const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED); const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED);
verifympu9250SpiWriteRegister(MPU_RA_GYRO_CONFIG, raGyroConfigData); verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData);
if (gyro->lpf == 4) { if (gyro->lpf == 4) {
verifympu9250SpiWriteRegister(MPU_RA_CONFIG, 1); //1KHz, 184DLPF verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_CONFIG, 1); //1KHz, 184DLPF
} else if (gyro->lpf < 4) { } else if (gyro->lpf < 4) {
verifympu9250SpiWriteRegister(MPU_RA_CONFIG, 7); //8KHz, 3600DLPF verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_CONFIG, 7); //8KHz, 3600DLPF
} else if (gyro->lpf > 4) { } else if (gyro->lpf > 4) {
verifympu9250SpiWriteRegister(MPU_RA_CONFIG, 0); //8KHz, 250DLPF verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_CONFIG, 0); //8KHz, 250DLPF
} }
verifympu9250SpiWriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro));
verifympu9250SpiWriteRegister(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3); verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
verifympu9250SpiWriteRegister(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN
#if defined(USE_MPU_DATA_READY_SIGNAL) #if defined(USE_MPU_DATA_READY_SIGNAL)
verifympu9250SpiWriteRegister(MPU_RA_INT_ENABLE, 0x01); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly. verifympu9250SpiWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly.
#endif #endif
spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST); spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST);
@ -154,23 +155,20 @@ static void mpu9250AccAndGyroInit(gyroDev_t *gyro)
mpuSpi9250InitDone = true; //init done mpuSpi9250InitDone = true; //init done
} }
bool mpu9250SpiDetect(void) bool mpu9250SpiDetect(const busDevice_t *bus)
{ {
/* not the best place for this - should really have an init method */ /* not the best place for this - should really have an init method */
#ifdef MPU9250_CS_PIN IOInit(bus->spi.csnPin, OWNER_MPU, RESOURCE_SPI_CS, 0);
mpuSpi9250CsPin = IOGetByTag(IO_TAG(MPU9250_CS_PIN)); IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
#endif
IOInit(mpuSpi9250CsPin, OWNER_MPU, RESOURCE_SPI_CS, 0);
IOConfigGPIO(mpuSpi9250CsPin, SPI_IO_CS_CFG);
spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed
mpu9250SpiWriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET); mpu9250SpiWriteRegister(bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
uint8_t attemptsRemaining = 20; uint8_t attemptsRemaining = 20;
do { do {
delay(150); delay(150);
uint8_t in; uint8_t in;
mpu9250SpiReadRegister(MPU_RA_WHO_AM_I, 1, &in); mpu9250SpiReadRegister(bus, MPU_RA_WHO_AM_I, 1, &in);
if (in == MPU9250_WHO_AM_I_CONST) { if (in == MPU9250_WHO_AM_I_CONST) {
break; break;
} }

13
src/main/drivers/accgyro_spi_mpu9250.h
View file

@ -1,6 +1,8 @@
#pragma once #pragma once
#include "sensor.h"
#define mpu9250_CONFIG 0x1A #define mpu9250_CONFIG 0x1A
/* We should probably use these. :) /* We should probably use these. :)
@ -17,6 +19,7 @@
#define GYRO_SCALE_FACTOR 0.00053292f // (4/131) * pi/180 (32.75 LSB = 1 DPS) #define GYRO_SCALE_FACTOR 0.00053292f // (4/131) * pi/180 (32.75 LSB = 1 DPS)
#define MPU9250_WHO_AM_I_CONST (0x71) #define MPU9250_WHO_AM_I_CONST (0x71)
#define MPU9255_WHO_AM_I_CONST (0x73)
#define MPU9250_BIT_RESET (0x80) #define MPU9250_BIT_RESET (0x80)
@ -25,12 +28,12 @@
void mpu9250SpiResetGyro(void); void mpu9250SpiResetGyro(void);
bool mpu9250SpiDetect(void); bool mpu9250SpiDetect(const busDevice_t *bus);
bool mpu9250SpiAccDetect(accDev_t *acc); bool mpu9250SpiAccDetect(accDev_t *acc);
bool mpu9250SpiGyroDetect(gyroDev_t *gyro); bool mpu9250SpiGyroDetect(gyroDev_t *gyro);
bool mpu9250SpiWriteRegister(uint8_t reg, uint8_t data); bool mpu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
bool verifympu9250SpiWriteRegister(uint8_t reg, uint8_t data); bool verifympu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
bool mpu9250SpiReadRegister(uint8_t reg, uint8_t length, uint8_t *data); bool mpu9250SpiReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);
bool mpu9250SpiSlowReadRegister(uint8_t reg, uint8_t length, uint8_t *data); bool mpu9250SpiSlowReadRegister(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);

1
src/main/drivers/compass.h
View file

@ -22,6 +22,7 @@
typedef struct magDev_s { typedef struct magDev_s {
sensorInitFuncPtr init; // initialize function sensorInitFuncPtr init; // initialize function
sensorReadFuncPtr read; // read 3 axis data function sensorReadFuncPtr read; // read 3 axis data function
busDevice_t bus;
sensor_align_e magAlign; sensor_align_e magAlign;
} magDev_t; } magDev_t;

30
src/main/drivers/compass_ak8963.c
View file

@ -101,22 +101,22 @@ static queuedReadState_t queuedRead = { false, 0, 0};
static bool ak8963SensorRead(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *buf) static bool ak8963SensorRead(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *buf)
{ {
mpuWriteRegisterI2C(MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read
mpuWriteRegisterI2C(MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
mpuWriteRegisterI2C(MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes
delay(10); delay(10);
__disable_irq(); __disable_irq();
mpuReadRegisterI2C(MPU_RA_EXT_SENS_DATA_00, len_, buf); // read I2C mpuReadRegisterI2C(NULL, MPU_RA_EXT_SENS_DATA_00, len_, buf); // read I2C
__enable_irq(); __enable_irq();
return true; return true;
} }
static bool ak8963SensorWrite(uint8_t addr_, uint8_t reg_, uint8_t data) static bool ak8963SensorWrite(uint8_t addr_, uint8_t reg_, uint8_t data)
{ {
mpuWriteRegisterI2C(MPU_RA_I2C_SLV0_ADDR, addr_); // set I2C slave address for write mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_ADDR, addr_); // set I2C slave address for write
mpuWriteRegisterI2C(MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
mpuWriteRegisterI2C(MPU_RA_I2C_SLV0_DO, data); // set I2C salve value mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_DO, data); // set I2C salve value
mpuWriteRegisterI2C(MPU_RA_I2C_SLV0_CTRL, 0x81); // write 1 byte mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_CTRL, 0x81); // write 1 byte
return true; return true;
} }
@ -128,9 +128,9 @@ static bool ak8963SensorStartRead(uint8_t addr_, uint8_t reg_, uint8_t len_)
queuedRead.len = len_; queuedRead.len = len_;
mpuWriteRegisterI2C(MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read
mpuWriteRegisterI2C(MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
mpuWriteRegisterI2C(MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes mpuWriteRegisterI2C(NULL, MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes
queuedRead.readStartedAt = micros(); queuedRead.readStartedAt = micros();
queuedRead.waiting = true; queuedRead.waiting = true;
@ -165,7 +165,7 @@ static bool ak8963SensorCompleteRead(uint8_t *buf)
queuedRead.waiting = false; queuedRead.waiting = false;
mpuReadRegisterI2C(MPU_RA_EXT_SENS_DATA_00, queuedRead.len, buf); // read I2C buffer mpuReadRegisterI2C(NULL, MPU_RA_EXT_SENS_DATA_00, queuedRead.len, buf); // read I2C buffer
return true; return true;
} }
#else #else
@ -327,13 +327,13 @@ bool ak8963Detect(magDev_t *mag)
#if defined(USE_SPI) && defined(MPU6500_SPI_INSTANCE) #if defined(USE_SPI) && defined(MPU6500_SPI_INSTANCE)
// initialze I2C master via SPI bus (MPU9250) // initialze I2C master via SPI bus (MPU9250)
ack = mpuWriteRegisterI2C(MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR ack = mpuWriteRegisterI2C(&mag->bus, MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR
delay(10); delay(10);
ack = mpuWriteRegisterI2C(MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz ack = mpuWriteRegisterI2C(&mag->bus, MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz
delay(10); delay(10);
ack = mpuWriteRegisterI2C(MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only ack = mpuWriteRegisterI2C(&mag->bus, MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only
delay(10); delay(10);
#endif #endif

11
src/main/drivers/sensor.h
View file

@ -17,6 +17,11 @@
#pragma once #pragma once
#include <stdbool.h>
#include <stdint.h>
#include "io_types.h"
typedef enum { typedef enum {
ALIGN_DEFAULT = 0, // driver-provided alignment ALIGN_DEFAULT = 0, // driver-provided alignment
CW0_DEG = 1, CW0_DEG = 1,
@ -29,6 +34,12 @@ typedef enum {
CW270_DEG_FLIP = 8 CW270_DEG_FLIP = 8
} sensor_align_e; } sensor_align_e;
typedef union busDevice_t {
struct deviceSpi_s {
IO_t csnPin;
} spi;
} busDevice_t;
typedef bool (*sensorInitFuncPtr)(void); // sensor init prototype typedef bool (*sensorInitFuncPtr)(void); // sensor init prototype
typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
typedef bool (*sensorInterruptFuncPtr)(void); typedef bool (*sensorInterruptFuncPtr)(void);

1
src/main/sensors/acceleration.c
View file

@ -274,6 +274,7 @@ bool accInit(uint32_t targetLooptime)
{ {
memset(&acc, 0, sizeof(acc)); memset(&acc, 0, sizeof(acc));
// copy over the common gyro mpu settings // copy over the common gyro mpu settings
acc.dev.bus = *gyroSensorBus();
acc.dev.mpuConfiguration = gyro.dev.mpuConfiguration; acc.dev.mpuConfiguration = gyro.dev.mpuConfiguration;
acc.dev.mpuDetectionResult = gyro.dev.mpuDetectionResult; acc.dev.mpuDetectionResult = gyro.dev.mpuDetectionResult;
if (!accDetect(&acc.dev, accelerometerConfig()->acc_hardware)) { if (!accDetect(&acc.dev, accelerometerConfig()->acc_hardware)) {

7
src/main/sensors/compass.c
View file

@ -46,8 +46,9 @@
#include "io/gps.h" #include "io/gps.h"
#include "sensors/boardalignment.h" #include "sensors/boardalignment.h"
#include "sensors/sensors.h"
#include "sensors/compass.h" #include "sensors/compass.h"
#include "sensors/gyro.h"
#include "sensors/sensors.h"
#ifdef NAZE #ifdef NAZE
#include "hardware_revision.h" #include "hardware_revision.h"
@ -233,7 +234,9 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)
bool compassInit(void) bool compassInit(void)
{ {
if (!compassDetect(&mag.dev, compassConfig()->mag_hardware)) { // copy over SPI bus settings for AK8963 compass
mag.dev.bus = *gyroSensorBus();
if (!compassDetect(&mag.dev, compassConfig()->mag_hardware)) {
return false; return false;
} }
// initialize and calibration. turn on led during mag calibration (calibration routine blinks it) // initialize and calibration. turn on led during mag calibration (calibration routine blinks it)

5
src/main/sensors/gyro.c
View file

@ -128,6 +128,11 @@ static const extiConfig_t *selectMPUIntExtiConfig(void)
#endif #endif
} }
const busDevice_t *gyroSensorBus(void)
{
return &gyro.dev.bus;
}
STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware) STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware)
{ {
dev->gyroAlign = ALIGN_DEFAULT; dev->gyroAlign = ALIGN_DEFAULT;

2
src/main/sensors/gyro.h
View file

@ -20,6 +20,7 @@
#include "drivers/accgyro.h" #include "drivers/accgyro.h"
#include "common/axis.h" #include "common/axis.h"
#include "config/parameter_group.h" #include "config/parameter_group.h"
#include "drivers/sensor.h"
typedef enum { typedef enum {
GYRO_NONE = 0, GYRO_NONE = 0,
@ -61,5 +62,6 @@ PG_DECLARE(gyroConfig_t, gyroConfig);
bool gyroInit(void); bool gyroInit(void);
void gyroInitFilters(void); void gyroInitFilters(void);
void gyroUpdate(void); void gyroUpdate(void);
const busDevice_t *gyroSensorBus(void);
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired); void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
bool gyroIsCalibrationComplete(void); bool gyroIsCalibrationComplete(void);

1
src/main/target/PIXRACER/target.h
View file

@ -46,6 +46,7 @@
#ifdef PIXRACER_ICM20608 #ifdef PIXRACER_ICM20608
// Variant that uses ICM20608 gyro/acc // Variant that uses ICM20608 gyro/acc
#define ICM20608_CS_PIN PC15
#define MPU6500_CS_PIN PC15 #define MPU6500_CS_PIN PC15
#define MPU6500_SPI_INSTANCE SPI1 #define MPU6500_SPI_INSTANCE SPI1