Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-21 15:25:36 +03:00
Code Issues Wiki Activity

Reodered compass driver functions to avoid forward declarations

Browse source
This commit is contained in:
Martin Budden 2016-12-14 17:29:00 +00:00
parent d93224be06
commit 18c04bf609
3 changed files with 74 additions and 83 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -42,9 +42,6 @@
#include "accgyro_spi_mpu9250.h"
#include "compass_ak8963.h"
void ak8963Init(void);
bool ak8963Read(int16_t *magData);
// This sensor is available in MPU-9250.
// AK8963, mag sensor address
@ -110,7 +107,7 @@ typedef enum {
static queuedReadState_t queuedRead = { false, 0, 0};
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)
{
verifympu9250WriteRegister(MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read
verifympu9250WriteRegister(MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
@ -122,7 +119,7 @@ bool ak8963SensorRead(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *buf)
return true;
}
bool ak8963SensorWrite(uint8_t addr_, uint8_t reg_, uint8_t data)
static bool ak8963SensorWrite(uint8_t addr_, uint8_t reg_, uint8_t data)
{
verifympu9250WriteRegister(MPU_RA_I2C_SLV0_ADDR, addr_); // set I2C slave address for write
verifympu9250WriteRegister(MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
@ -131,7 +128,7 @@ bool ak8963SensorWrite(uint8_t addr_, uint8_t reg_, uint8_t data)
return true;
}
bool ak8963SensorStartRead(uint8_t addr_, uint8_t reg_, uint8_t len_)
static bool ak8963SensorStartRead(uint8_t addr_, uint8_t reg_, uint8_t len_)
{
if (queuedRead.waiting) {
return false;
@ -166,7 +163,7 @@ static uint32_t ak8963SensorQueuedReadTimeRemaining(void)
return timeRemaining;
}
bool ak8963SensorCompleteRead(uint8_t *buf)
static bool ak8963SensorCompleteRead(uint8_t *buf)
{
uint32_t timeRemaining = ak8963SensorQueuedReadTimeRemaining();
@ -180,47 +177,18 @@ bool ak8963SensorCompleteRead(uint8_t *buf)
return true;
}
#else
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)
{
return i2cRead(MAG_I2C_INSTANCE, addr_, reg_, len, buf);
}
bool ak8963SensorWrite(uint8_t addr_, uint8_t reg_, uint8_t data)
static bool ak8963SensorWrite(uint8_t addr_, uint8_t reg_, uint8_t data)
{
return i2cWrite(MAG_I2C_INSTANCE, addr_, reg_, data);
}
#endif
bool ak8963Detect(magDev_t *mag)
{
uint8_t sig = 0;
#if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE)
// initialze I2C master via SPI bus (MPU9250)
verifympu9250WriteRegister(MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR
delay(10);
verifympu9250WriteRegister(MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz
delay(10);
verifympu9250WriteRegister(MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only
delay(10);
#endif
// check for AK8963
bool ack = ak8963SensorRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_WHO_AM_I, 1, &sig);
if (ack && sig == AK8963_Device_ID) // 0x48 / 01001000 / 'H'
{
mag->init = ak8963Init;
mag->read = ak8963Read;
return true;
}
return false;
}
void ak8963Init()
static void ak8963Init()
{
uint8_t calibration[3];
uint8_t status;
@ -253,7 +221,7 @@ void ak8963Init()
#endif
}
bool ak8963Read(int16_t *magData)
static bool ak8963Read(int16_t *magData)
{
bool ack = false;
uint8_t buf[7];
@ -339,3 +307,32 @@ restart:
return ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_ONCE); // start reading again
#endif
}
bool ak8963Detect(magDev_t *mag)
{
uint8_t sig = 0;
#if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE)
// initialze I2C master via SPI bus (MPU9250)
verifympu9250WriteRegister(MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR
delay(10);
verifympu9250WriteRegister(MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz
delay(10);
verifympu9250WriteRegister(MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only
delay(10);
#endif
// check for AK8963
bool ack = ak8963SensorRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_WHO_AM_I, 1, &sig);
if (ack && sig == AK8963_Device_ID) // 0x48 / 01001000 / 'H'
{
mag->init = ak8963Init;
mag->read = ak8963Read;
return true;
}
return false;
}

35
src/main/drivers/compass_ak8975.c
View file

@ -37,9 +37,6 @@
#include "compass_ak8975.h"
void ak8975Init(void);
bool ak8975Read(int16_t *magData);
// This sensor is available in MPU-9150.
// AK8975, mag sensor address
@ -60,25 +57,11 @@ bool ak8975Read(int16_t *magData);
#define AK8975_MAG_REG_CNTL 0x0a
#define AK8975_MAG_REG_ASCT 0x0c // self test
bool ak8975Detect(magDev_t *mag)
{
uint8_t sig = 0;
bool ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_WHO_AM_I, 1, &sig);
if (!ack || sig != 'H') // 0x48 / 01001000 / 'H'
return false;
mag->init = ak8975Init;
mag->read = ak8975Read;
return true;
}
#define AK8975A_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value
#define AK8975A_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value
#define AK8975A_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value
void ak8975Init()
static void ak8975Init()
{
uint8_t buffer[3];
uint8_t status;
@ -108,7 +91,7 @@ void ak8975Init()
#define BIT_STATUS2_REG_DATA_ERROR (1 << 2)
#define BIT_STATUS2_REG_MAG_SENSOR_OVERFLOW (1 << 3)
bool ak8975Read(int16_t *magData)
static bool ak8975Read(int16_t *magData)
{
bool ack;
uint8_t status;
@ -142,4 +125,18 @@ bool ak8975Read(int16_t *magData)
i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x01); // start reading again
return true;
}
bool ak8975Detect(magDev_t *mag)
{
uint8_t sig = 0;
bool ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_WHO_AM_I, 1, &sig);
if (!ack || sig != 'H') // 0x48 / 01001000 / 'H'
return false;
mag->init = ak8975Init;
mag->read = ak8975Read;
return true;
}
#endif

45
src/main/drivers/compass_hmc5883l.c
View file

@ -41,9 +41,6 @@
#include "compass_hmc5883l.h"
void hmc5883lInit(void);
bool hmc5883lRead(int16_t *magData);
//#define DEBUG_MAG_DATA_READY_INTERRUPT
// HMC5883L, default address 0x1E
@ -129,7 +126,7 @@ static const hmc5883Config_t *hmc5883Config = NULL;
static IO_t intIO;
static extiCallbackRec_t hmc5883_extiCallbackRec;
void hmc5883_extiHandler(extiCallbackRec_t* cb)
static void hmc5883_extiHandler(extiCallbackRec_t* cb)
{
UNUSED(cb);
#ifdef DEBUG_MAG_DATA_READY_INTERRUPT
@ -170,23 +167,24 @@ static void hmc5883lConfigureDataReadyInterruptHandling(void)
#endif
}
bool hmc5883lDetect(magDev_t* mag, const hmc5883Config_t *hmc5883ConfigToUse)
static bool hmc5883lRead(int16_t *magData)
{
hmc5883Config = hmc5883ConfigToUse;
uint8_t buf[6];
uint8_t sig = 0;
bool ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, 0x0A, 1, &sig);
if (!ack || sig != 'H')
bool ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf);
if (!ack) {
return false;
mag->init = hmc5883lInit;
mag->read = hmc5883lRead;
}
// During calibration, magGain is 1.0, so the read returns normal non-calibrated values.
// After calibration is done, magGain is set to calculated gain values.
magData[X] = (int16_t)(buf[0] << 8 | buf[1]) * magGain[X];
magData[Z] = (int16_t)(buf[2] << 8 | buf[3]) * magGain[Z];
magData[Y] = (int16_t)(buf[4] << 8 | buf[5]) * magGain[Y];
return true;
}
void hmc5883lInit(void)
static void hmc5883lInit(void)
{
int16_t magADC[3];
int i;
@ -258,19 +256,18 @@ void hmc5883lInit(void)
hmc5883lConfigureDataReadyInterruptHandling();
}
bool hmc5883lRead(int16_t *magData)
bool hmc5883lDetect(magDev_t* mag, const hmc5883Config_t *hmc5883ConfigToUse)
{
uint8_t buf[6];
hmc5883Config = hmc5883ConfigToUse;
bool ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf);
if (!ack) {
uint8_t sig = 0;
bool ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, 0x0A, 1, &sig);
if (!ack || sig != 'H')
return false;
}
// During calibration, magGain is 1.0, so the read returns normal non-calibrated values.
// After calibration is done, magGain is set to calculated gain values.
magData[X] = (int16_t)(buf[0] << 8 | buf[1]) * magGain[X];
magData[Z] = (int16_t)(buf[2] << 8 | buf[3]) * magGain[Z];
magData[Y] = (int16_t)(buf[4] << 8 | buf[5]) * magGain[Y];
mag->init = hmc5883lInit;
mag->read = hmc5883lRead;
return true;
}