AlienFlight fixes
AK8963 driver updates Increase resolution for brushed motors from 250 to 750 steps (@32Khz pwm rate)
This commit is contained in:
parent
00f179397e
commit
070ea81816
|
@ -650,12 +650,14 @@ static void resetConf(void)
|
|||
|
||||
#if defined(ALIENFLIGHT)
|
||||
featureClear(FEATURE_ONESHOT125);
|
||||
#ifdef ALIENFLIGHTF3
|
||||
#ifdef ALIENFLIGHTF1
|
||||
masterConfig.serialConfig.portConfigs[1].functionMask = FUNCTION_RX_SERIAL;
|
||||
#else
|
||||
masterConfig.serialConfig.portConfigs[2].functionMask = FUNCTION_RX_SERIAL;
|
||||
#endif
|
||||
#ifdef ALIENFLIGHTF3
|
||||
masterConfig.batteryConfig.vbatscale = 20;
|
||||
masterConfig.mag_hardware = MAG_NONE; // disabled by default
|
||||
#else
|
||||
masterConfig.serialConfig.portConfigs[1].functionMask = FUNCTION_RX_SERIAL;
|
||||
#endif
|
||||
masterConfig.rxConfig.serialrx_provider = 1;
|
||||
masterConfig.rxConfig.spektrum_sat_bind = 5;
|
||||
|
|
|
@ -33,6 +33,7 @@
|
|||
#include "gpio.h"
|
||||
#include "exti.h"
|
||||
#include "bus_i2c.h"
|
||||
#include "bus_spi.h"
|
||||
|
||||
#include "sensors/boardalignment.h"
|
||||
#include "sensors/sensors.h"
|
||||
|
@ -42,7 +43,9 @@
|
|||
|
||||
#include "accgyro.h"
|
||||
#include "accgyro_mpu.h"
|
||||
#include "accgyro_mpu6500.h"
|
||||
#include "accgyro_spi_mpu6500.h"
|
||||
#include "accgyro_spi_mpu9250.h"
|
||||
#include "compass_ak8963.h"
|
||||
|
||||
// This sensor is available in MPU-9250.
|
||||
|
@ -83,18 +86,10 @@
|
|||
#define CNTL_MODE_SELF_TEST 0x08
|
||||
#define CNTL_MODE_FUSE_ROM 0x0F
|
||||
|
||||
typedef bool (*ak8963ReadRegisterFunc)(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *buf);
|
||||
typedef bool (*ak8963WriteRegisterFunc)(uint8_t addr_, uint8_t reg_, uint8_t data);
|
||||
|
||||
typedef struct ak8963Configuration_s {
|
||||
ak8963ReadRegisterFunc read;
|
||||
ak8963WriteRegisterFunc write;
|
||||
} ak8963Configuration_t;
|
||||
|
||||
ak8963Configuration_t ak8963config;
|
||||
static float magGain[3] = { 1.0f, 1.0f, 1.0f };
|
||||
|
||||
// FIXME pretend we have real MPU9250 support
|
||||
// Is an separate MPU9250 driver really needed? The GYRO/ACC part between MPU6500 and MPU9250 is exactly the same.
|
||||
#if defined(MPU6500_SPI_INSTANCE) && !defined(MPU9250_SPI_INSTANCE)
|
||||
#define MPU9250_SPI_INSTANCE
|
||||
#define verifympu9250WriteRegister mpu6500WriteRegister
|
||||
|
@ -102,30 +97,7 @@ static float magGain[3] = { 1.0f, 1.0f, 1.0f };
|
|||
#define mpu9250ReadRegister mpu6500ReadRegister
|
||||
#endif
|
||||
|
||||
#ifdef USE_SPI
|
||||
bool ak8963SPIRead(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *buf)
|
||||
{
|
||||
mpu6500WriteRegister(MPU_RA_I2C_SLV0_ADDR, addr_ | READ_FLAG); // set I2C slave address for read
|
||||
mpu6500WriteRegister(MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
|
||||
mpu6500WriteRegister(MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes
|
||||
delay(8);
|
||||
__disable_irq();
|
||||
mpu6500ReadRegister(MPU_RA_EXT_SENS_DATA_00, len_, buf); // read I2C
|
||||
__enable_irq();
|
||||
return true;
|
||||
}
|
||||
|
||||
bool ak8963SPIWrite(uint8_t addr_, uint8_t reg_, uint8_t data)
|
||||
{
|
||||
mpu6500WriteRegister(MPU_RA_I2C_SLV0_ADDR, addr_); // set I2C slave address for write
|
||||
mpu6500WriteRegister(MPU_RA_I2C_SLV0_REG, reg_); // set I2C slave register
|
||||
mpu6500WriteRegister(MPU_RA_I2C_SLV0_DO, data); // set I2C salve value
|
||||
mpu6500WriteRegister(MPU_RA_I2C_SLV0_CTRL, 0x81); // write 1 byte
|
||||
return true;
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef SPRACINGF3EVO
|
||||
#if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE)
|
||||
|
||||
typedef struct queuedReadState_s {
|
||||
bool waiting;
|
||||
|
@ -133,9 +105,36 @@ typedef struct queuedReadState_s {
|
|||
uint32_t readStartedAt; // time read was queued in micros.
|
||||
} queuedReadState_t;
|
||||
|
||||
typedef enum {
|
||||
CHECK_STATUS = 0,
|
||||
WAITING_FOR_STATUS,
|
||||
WAITING_FOR_DATA
|
||||
} ak8963ReadState_e;
|
||||
|
||||
static queuedReadState_t queuedRead = { false, 0, 0};
|
||||
|
||||
bool ak8963SPIStartRead(uint8_t addr_, uint8_t reg_, uint8_t len_)
|
||||
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
|
||||
verifympu9250WriteRegister(MPU_RA_I2C_SLV0_CTRL, len_ | 0x80); // read number of bytes
|
||||
delay(10);
|
||||
__disable_irq();
|
||||
mpu9250ReadRegister(MPU_RA_EXT_SENS_DATA_00, len_, buf); // read I2C
|
||||
__enable_irq();
|
||||
return true;
|
||||
}
|
||||
|
||||
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
|
||||
verifympu9250WriteRegister(MPU_RA_I2C_SLV0_DO, data); // set I2C salve value
|
||||
verifympu9250WriteRegister(MPU_RA_I2C_SLV0_CTRL, 0x81); // write 1 byte
|
||||
return true;
|
||||
}
|
||||
|
||||
bool ak8963SensorStartRead(uint8_t addr_, uint8_t reg_, uint8_t len_)
|
||||
{
|
||||
if (queuedRead.waiting) {
|
||||
return false;
|
||||
|
@ -153,7 +152,7 @@ bool ak8963SPIStartRead(uint8_t addr_, uint8_t reg_, uint8_t len_)
|
|||
return true;
|
||||
}
|
||||
|
||||
static uint32_t ak8963SPIQueuedReadTimeRemaining(void)
|
||||
static uint32_t ak8963SensorQueuedReadTimeRemaining(void)
|
||||
{
|
||||
if (!queuedRead.waiting) {
|
||||
return 0;
|
||||
|
@ -170,9 +169,9 @@ static uint32_t ak8963SPIQueuedReadTimeRemaining(void)
|
|||
return timeRemaining;
|
||||
}
|
||||
|
||||
bool ak8963SPICompleteRead(uint8_t *buf)
|
||||
bool ak8963SensorCompleteRead(uint8_t *buf)
|
||||
{
|
||||
uint32_t timeRemaining = ak8963SPIQueuedReadTimeRemaining();
|
||||
uint32_t timeRemaining = ak8963SensorQueuedReadTimeRemaining();
|
||||
|
||||
if (timeRemaining > 0) {
|
||||
delayMicroseconds(timeRemaining);
|
||||
|
@ -183,18 +182,15 @@ bool ak8963SPICompleteRead(uint8_t *buf)
|
|||
mpu9250ReadRegister(MPU_RA_EXT_SENS_DATA_00, queuedRead.len, buf); // read I2C buffer
|
||||
return true;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
#ifdef USE_I2C
|
||||
bool c_i2cWrite(uint8_t addr_, uint8_t reg_, uint8_t data)
|
||||
#else
|
||||
bool ak8963SensorRead(uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf)
|
||||
{
|
||||
return i2cWrite(MAG_I2C_INSTANCE, addr_, reg_, data);
|
||||
return i2cRead(MAG_I2C_INSTANCE, addr_, reg_, len, buf);
|
||||
}
|
||||
|
||||
bool c_i2cRead(uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf)
|
||||
bool ak8963SensorWrite(uint8_t addr_, uint8_t reg_, uint8_t data)
|
||||
{
|
||||
return i2cRead(MAG_I2C_INSTANCE, addr_, reg_, len, buf);
|
||||
return i2cWrite(MAG_I2C_INSTANCE, addr_, reg_, data);
|
||||
}
|
||||
#endif
|
||||
|
||||
|
@ -203,43 +199,28 @@ bool ak8963Detect(mag_t *mag)
|
|||
bool ack = false;
|
||||
uint8_t sig = 0;
|
||||
|
||||
#ifdef USE_I2C
|
||||
// check for AK8963 on I2C bus
|
||||
ack = i2cRead(MAG_I2C_INSTANCE, AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_WHO_AM_I, 1, &sig);
|
||||
#if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE)
|
||||
// initialze I2C master via SPI bus (MPU9250)
|
||||
|
||||
ack = verifympu9250WriteRegister(MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR
|
||||
delay(10);
|
||||
|
||||
ack = verifympu9250WriteRegister(MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz
|
||||
delay(10);
|
||||
|
||||
ack = verifympu9250WriteRegister(MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only
|
||||
delay(10);
|
||||
#endif
|
||||
|
||||
// check for AK8963
|
||||
ack = ak8963SensorRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_WHO_AM_I, 1, &sig);
|
||||
if (ack && sig == AK8963_Device_ID) // 0x48 / 01001000 / 'H'
|
||||
{
|
||||
ak8963config.read = c_i2cRead;
|
||||
ak8963config.write = c_i2cWrite;
|
||||
mag->init = ak8963Init;
|
||||
mag->read = ak8963Read;
|
||||
|
||||
return true;
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_SPI
|
||||
// check for AK8963 on I2C master via SPI bus (as part of MPU9250)
|
||||
|
||||
ack = mpu6500WriteRegister(MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR
|
||||
delay(10);
|
||||
|
||||
ack = mpu6500WriteRegister(MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz
|
||||
delay(10);
|
||||
|
||||
ack = mpu6500WriteRegister(MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only
|
||||
delay(10);
|
||||
|
||||
ack = ak8963SPIRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_WHO_AM_I, 1, &sig);
|
||||
if (ack && sig == AK8963_Device_ID) // 0x48 / 01001000 / 'H'
|
||||
{
|
||||
ak8963config.read = ak8963SPIRead;
|
||||
ak8963config.write = ak8963SPIWrite;
|
||||
mag->init = ak8963Init;
|
||||
mag->read = ak8963Read;
|
||||
|
||||
return true;
|
||||
}
|
||||
#endif
|
||||
return false;
|
||||
}
|
||||
|
||||
|
@ -250,49 +231,43 @@ void ak8963Init()
|
|||
uint8_t calibration[3];
|
||||
uint8_t status;
|
||||
|
||||
ack = ak8963config.write(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down before entering fuse mode
|
||||
ack = ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down before entering fuse mode
|
||||
delay(20);
|
||||
|
||||
ack = ak8963config.write(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_FUSE_ROM); // Enter Fuse ROM access mode
|
||||
ack = ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_FUSE_ROM); // Enter Fuse ROM access mode
|
||||
delay(10);
|
||||
|
||||
ack = ak8963config.read(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_ASAX, sizeof(calibration), calibration); // Read the x-, y-, and z-axis calibration values
|
||||
ack = ak8963SensorRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_ASAX, sizeof(calibration), calibration); // Read the x-, y-, and z-axis calibration values
|
||||
delay(10);
|
||||
|
||||
magGain[X] = (((((float)(int8_t)calibration[X] - 128) / 256) + 1) * 30);
|
||||
magGain[Y] = (((((float)(int8_t)calibration[Y] - 128) / 256) + 1) * 30);
|
||||
magGain[Z] = (((((float)(int8_t)calibration[Z] - 128) / 256) + 1) * 30);
|
||||
|
||||
ack = ak8963config.write(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down after reading.
|
||||
ack = ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down after reading.
|
||||
delay(10);
|
||||
|
||||
// Clear status registers
|
||||
ack = ak8963config.read(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS1, 1, &status);
|
||||
ack = ak8963config.read(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS2, 1, &status);
|
||||
ack = ak8963SensorRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS1, 1, &status);
|
||||
ack = ak8963SensorRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS2, 1, &status);
|
||||
|
||||
// Trigger first measurement
|
||||
#ifdef SPRACINGF3EVO
|
||||
ack = ak8963config.write(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_CONT1);
|
||||
#if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE)
|
||||
ack = ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_CONT1);
|
||||
#else
|
||||
ack = ak8963config.write(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_ONCE);
|
||||
ack = ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_ONCE);
|
||||
#endif
|
||||
}
|
||||
|
||||
typedef enum {
|
||||
CHECK_STATUS = 0,
|
||||
WAITING_FOR_STATUS,
|
||||
WAITING_FOR_DATA
|
||||
} ak8963ReadState_e;
|
||||
|
||||
bool ak8963Read(int16_t *magData)
|
||||
{
|
||||
bool ack;
|
||||
bool ack = false;
|
||||
uint8_t buf[7];
|
||||
|
||||
#ifdef SPRACINGF3EVO
|
||||
#if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE)
|
||||
|
||||
// we currently need a different approach on the SPRacingF3EVO which has an MPU9250 connected via SPI.
|
||||
// we cannot use the ak8963SPIRead() method, it is to slow and blocks for far too long.
|
||||
// we currently need a different approach for the MPU9250 connected via SPI.
|
||||
// we cannot use the ak8963SensorRead() method for SPI, it is to slow and blocks for far too long.
|
||||
|
||||
static ak8963ReadState_e state = CHECK_STATUS;
|
||||
|
||||
|
@ -301,17 +276,17 @@ bool ak8963Read(int16_t *magData)
|
|||
restart:
|
||||
switch (state) {
|
||||
case CHECK_STATUS:
|
||||
ak8963SPIStartRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS1, 1);
|
||||
ak8963SensorStartRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS1, 1);
|
||||
state++;
|
||||
return false;
|
||||
|
||||
case WAITING_FOR_STATUS: {
|
||||
uint32_t timeRemaining = ak8963SPIQueuedReadTimeRemaining();
|
||||
uint32_t timeRemaining = ak8963SensorQueuedReadTimeRemaining();
|
||||
if (timeRemaining) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ack = ak8963SPICompleteRead(&buf[0]);
|
||||
ack = ak8963SensorCompleteRead(&buf[0]);
|
||||
|
||||
uint8_t status = buf[0];
|
||||
|
||||
|
@ -327,7 +302,7 @@ restart:
|
|||
|
||||
|
||||
// read the 6 bytes of data and the status2 register
|
||||
ak8963SPIStartRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_HXL, 7);
|
||||
ak8963SensorStartRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_HXL, 7);
|
||||
|
||||
state++;
|
||||
|
||||
|
@ -335,31 +310,16 @@ restart:
|
|||
}
|
||||
|
||||
case WAITING_FOR_DATA: {
|
||||
uint32_t timeRemaining = ak8963SPIQueuedReadTimeRemaining();
|
||||
uint32_t timeRemaining = ak8963SensorQueuedReadTimeRemaining();
|
||||
if (timeRemaining) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ack = ak8963SPICompleteRead(&buf[0]);
|
||||
|
||||
uint8_t status2 = buf[6];
|
||||
if (!ack || (status2 & STATUS2_DATA_ERROR) || (status2 & STATUS2_MAG_SENSOR_OVERFLOW)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
magData[X] = -(int16_t)(buf[1] << 8 | buf[0]) * magGain[X];
|
||||
magData[Y] = -(int16_t)(buf[3] << 8 | buf[2]) * magGain[Y];
|
||||
magData[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * magGain[Z];
|
||||
|
||||
state = CHECK_STATUS;
|
||||
|
||||
return true;
|
||||
ack = ak8963SensorCompleteRead(&buf[0]);
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
#else
|
||||
ack = ak8963config.read(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS1, 1, &buf[0]);
|
||||
ack = ak8963SensorRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_STATUS1, 1, &buf[0]);
|
||||
|
||||
uint8_t status = buf[0];
|
||||
|
||||
|
@ -367,8 +327,8 @@ restart:
|
|||
return false;
|
||||
}
|
||||
|
||||
ack = ak8963config.read(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_HXL, 7, &buf[0]);
|
||||
|
||||
ack = ak8963SensorRead(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_HXL, 7, &buf[0]);
|
||||
#endif
|
||||
uint8_t status2 = buf[6];
|
||||
if (!ack || (status2 & STATUS2_DATA_ERROR) || (status2 & STATUS2_MAG_SENSOR_OVERFLOW)) {
|
||||
return false;
|
||||
|
@ -378,6 +338,10 @@ restart:
|
|||
magData[Y] = -(int16_t)(buf[3] << 8 | buf[2]) * magGain[Y];
|
||||
magData[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * magGain[Z];
|
||||
|
||||
return ak8963config.write(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_ONCE); // start reading again
|
||||
#if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE)
|
||||
state = CHECK_STATUS;
|
||||
return true;
|
||||
#else
|
||||
return ak8963SensorWrite(AK8963_MAG_I2C_ADDRESS, AK8963_MAG_REG_CNTL, CNTL_MODE_ONCE); // start reading again
|
||||
#endif
|
||||
}
|
||||
|
|
|
@ -37,7 +37,7 @@
|
|||
|
||||
#define PWM_TIMER_MHZ 1
|
||||
|
||||
#define PWM_BRUSHED_TIMER_MHZ 8
|
||||
#define PWM_BRUSHED_TIMER_MHZ 24
|
||||
#define MULTISHOT_TIMER_MHZ 72
|
||||
#define ONESHOT42_TIMER_MHZ 24
|
||||
#define ONESHOT125_TIMER_MHZ 8
|
||||
|
|
|
@ -730,7 +730,7 @@ void main_init(void)
|
|||
#endif
|
||||
#ifdef MAG
|
||||
setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG));
|
||||
#ifdef SPRACINGF3EVO
|
||||
#if defined(USE_SPI) && defined(USE_MAG_AK8963)
|
||||
// fixme temporary solution for AK6983 via slave I2C on MPU9250
|
||||
rescheduleTask(TASK_COMPASS, 1000000 / 40);
|
||||
#endif
|
||||
|
|
|
@ -43,6 +43,7 @@ mag_t mag; // mag access functions
|
|||
|
||||
extern uint32_t currentTime; // FIXME dependency on global variable, pass it in instead.
|
||||
|
||||
int16_t magADCRaw[XYZ_AXIS_COUNT];
|
||||
int32_t magADC[XYZ_AXIS_COUNT];
|
||||
sensor_align_e magAlign = 0;
|
||||
#ifdef MAG
|
||||
|
@ -57,27 +58,19 @@ void compassInit(void)
|
|||
magInit = 1;
|
||||
}
|
||||
|
||||
#define COMPASS_UPDATE_FREQUENCY_10HZ (1000 * 100)
|
||||
|
||||
void updateCompass(flightDynamicsTrims_t *magZero)
|
||||
{
|
||||
static uint32_t nextUpdateAt, tCal = 0;
|
||||
static uint32_t tCal = 0;
|
||||
static flightDynamicsTrims_t magZeroTempMin;
|
||||
static flightDynamicsTrims_t magZeroTempMax;
|
||||
int16_t magADCRaw[XYZ_AXIS_COUNT];
|
||||
uint32_t axis;
|
||||
|
||||
if ((int32_t)(currentTime - nextUpdateAt) < 0)
|
||||
return;
|
||||
|
||||
nextUpdateAt = currentTime + COMPASS_UPDATE_FREQUENCY_10HZ;
|
||||
|
||||
mag.read(magADCRaw);
|
||||
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) magADC[axis] = magADCRaw[axis];
|
||||
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) magADC[axis] = magADCRaw[axis]; // int32_t copy to work with
|
||||
alignSensors(magADC, magADC, magAlign);
|
||||
|
||||
if (STATE(CALIBRATE_MAG)) {
|
||||
tCal = nextUpdateAt;
|
||||
tCal = currentTime;
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
magZero->raw[axis] = 0;
|
||||
magZeroTempMin.raw[axis] = magADC[axis];
|
||||
|
@ -93,7 +86,7 @@ void updateCompass(flightDynamicsTrims_t *magZero)
|
|||
}
|
||||
|
||||
if (tCal != 0) {
|
||||
if ((nextUpdateAt - tCal) < 30000000) { // 30s: you have 30s to turn the multi in all directions
|
||||
if ((currentTime - tCal) < 30000000) { // 30s: you have 30s to turn the multi in all directions
|
||||
LED0_TOGGLE;
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
if (magADC[axis] < magZeroTempMin.raw[axis])
|
||||
|
|
|
@ -216,7 +216,7 @@ bool detectGyro(void)
|
|||
; // fallthrough
|
||||
|
||||
case GYRO_MPU6500:
|
||||
#ifdef USE_GYRO_MPU6500
|
||||
#if defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500)
|
||||
#ifdef USE_GYRO_SPI_MPU6500
|
||||
if (mpu6500GyroDetect(&gyro) || mpu6500SpiGyroDetect(&gyro))
|
||||
#else
|
||||
|
@ -361,7 +361,7 @@ retry:
|
|||
#endif
|
||||
; // fallthrough
|
||||
case ACC_MPU6500:
|
||||
#ifdef USE_ACC_MPU6500
|
||||
#if defined(USE_ACC_MPU6500) || defined(USE_ACC_SPI_MPU6500)
|
||||
#ifdef USE_ACC_SPI_MPU6500
|
||||
if (mpu6500AccDetect(&acc) || mpu6500SpiAccDetect(&acc))
|
||||
#else
|
||||
|
|
|
@ -45,7 +45,6 @@
|
|||
// Using MPU6050 for the moment.
|
||||
#define GYRO
|
||||
#define USE_GYRO_MPU6050
|
||||
#define USE_GYRO_MPU6500
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
|
||||
#define GYRO_MPU6050_ALIGN CW270_DEG
|
||||
|
@ -53,7 +52,6 @@
|
|||
|
||||
#define ACC
|
||||
#define USE_ACC_MPU6050
|
||||
#define USE_ACC_MPU6500
|
||||
#define USE_ACC_SPI_MPU6500
|
||||
|
||||
#define ACC_MPU6050_ALIGN CW270_DEG
|
||||
|
@ -100,8 +98,8 @@
|
|||
#define USE_I2C
|
||||
#define I2C_DEVICE (I2CDEV_2) // SDA (PA10/AF4), SCL (PA9/AF4)
|
||||
|
||||
#define I2C2_SCL_PIN PA9
|
||||
#define I2C2_SDA_PIN PA10
|
||||
#define I2C2_SCL PA9
|
||||
#define I2C2_SDA PA10
|
||||
|
||||
// SPI3
|
||||
// PA15 38 SPI3_NSS
|
||||
|
|
|
@ -76,8 +76,8 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
|
|||
{ TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1, 0}, // PWM1 - PA8 RC1
|
||||
{ TIM1, IO_TAG(PB0), TIM_Channel_2, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1, 0}, // PWM2 - PB0 RC2
|
||||
{ TIM1, IO_TAG(PB1), TIM_Channel_3, TIM1_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM1, 0}, // PWM3 - PB1 RC3
|
||||
{ TIM8, IO_TAG(PB14),TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, 0}, // PWM4 - PA14 RC4
|
||||
{ TIM8, IO_TAG(PB15),TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, 0}, // PWM5 - PA15 RC5
|
||||
{ TIM8, IO_TAG(PB14),TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, 0}, // PWM4 - PA14 RC4
|
||||
{ TIM8, IO_TAG(PB15),TIM_Channel_3, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8, 0}, // PWM5 - PA15 RC5
|
||||
|
||||
{ TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM4, 0}, // PWM6 - PB8 OUT1
|
||||
{ TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM4, 0}, // PWM7 - PB9 OUT2
|
||||
|
|
|
@ -45,22 +45,16 @@
|
|||
|
||||
#define MPU6500_CS_PIN PA4
|
||||
#define MPU6500_SPI_INSTANCE SPI1
|
||||
#define MPU9250_CS_PIN PA4
|
||||
#define MPU9250_SPI_INSTANCE SPI1
|
||||
|
||||
#define ACC
|
||||
#define USE_ACC_SPI_MPU6500
|
||||
#define USE_ACC_SPI_MPU9250
|
||||
|
||||
#define ACC_MPU6500_ALIGN CW270_DEG
|
||||
#define ACC_MPU9250_ALIGN CW270_DEG
|
||||
|
||||
#define GYRO
|
||||
#define USE_GYRO_SPI_MPU6500
|
||||
#define USE_GYRO_SPI_MPU9250
|
||||
|
||||
#define GYRO_MPU6500_ALIGN CW270_DEG
|
||||
#define GYRO_MPU9250_ALIGN CW270_DEG
|
||||
|
||||
#define MAG
|
||||
#define USE_MAG_HMC5883
|
||||
|
|
|
@ -4,7 +4,6 @@ FEATURES += SDCARD VCP
|
|||
TARGET_SRC = \
|
||||
drivers/accgyro_mpu6500.c \
|
||||
drivers/accgyro_spi_mpu6500.c \
|
||||
drivers/accgyro_spi_mpu9250.c \
|
||||
drivers/barometer_bmp280.c \
|
||||
drivers/barometer_ms5611.c \
|
||||
drivers/compass_ak8963.c \
|
||||
|
|
Loading…
Reference in New Issue