mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-23 16:25:31 +03:00
Merge remote-tracking branch 'origin/development' into osd-improvement
This commit is contained in:
commit
d6ce148ae6
83 changed files with 681 additions and 711 deletions
|
@ -325,9 +325,6 @@ extern uint32_t currentTime;
|
||||||
//From rx.c:
|
//From rx.c:
|
||||||
extern uint16_t rssi;
|
extern uint16_t rssi;
|
||||||
|
|
||||||
//From gyro.c
|
|
||||||
extern uint32_t targetLooptime;
|
|
||||||
|
|
||||||
//From rc_controls.c
|
//From rc_controls.c
|
||||||
extern uint32_t rcModeActivationMask;
|
extern uint32_t rcModeActivationMask;
|
||||||
|
|
||||||
|
@ -1169,7 +1166,7 @@ static bool blackboxWriteSysinfo()
|
||||||
}
|
}
|
||||||
);
|
);
|
||||||
|
|
||||||
BLACKBOX_PRINT_HEADER_LINE("looptime:%d", targetLooptime);
|
BLACKBOX_PRINT_HEADER_LINE("looptime:%d", gyro.targetLooptime);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("rcRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcRate8);
|
BLACKBOX_PRINT_HEADER_LINE("rcRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcRate8);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("rcExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcExpo8);
|
BLACKBOX_PRINT_HEADER_LINE("rcExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcExpo8);
|
||||||
BLACKBOX_PRINT_HEADER_LINE("rcYawRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcYawRate8);
|
BLACKBOX_PRINT_HEADER_LINE("rcYawRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcYawRate8);
|
||||||
|
|
|
@ -22,7 +22,6 @@
|
||||||
#include "drivers/accgyro.h"
|
#include "drivers/accgyro.h"
|
||||||
#include "drivers/light_led.h"
|
#include "drivers/light_led.h"
|
||||||
#include "drivers/sound_beeper.h"
|
#include "drivers/sound_beeper.h"
|
||||||
#include "drivers/gyro_sync.h"
|
|
||||||
|
|
||||||
#include "sensors/sensors.h"
|
#include "sensors/sensors.h"
|
||||||
#include "sensors/boardalignment.h"
|
#include "sensors/boardalignment.h"
|
||||||
|
|
|
@ -38,7 +38,6 @@
|
||||||
#include "drivers/timer.h"
|
#include "drivers/timer.h"
|
||||||
#include "drivers/pwm_rx.h"
|
#include "drivers/pwm_rx.h"
|
||||||
#include "drivers/serial.h"
|
#include "drivers/serial.h"
|
||||||
#include "drivers/gyro_sync.h"
|
|
||||||
#include "drivers/pwm_output.h"
|
#include "drivers/pwm_output.h"
|
||||||
#include "drivers/max7456.h"
|
#include "drivers/max7456.h"
|
||||||
|
|
||||||
|
@ -751,7 +750,7 @@ void activateConfig(void)
|
||||||
¤tProfile->pidProfile
|
¤tProfile->pidProfile
|
||||||
);
|
);
|
||||||
|
|
||||||
useGyroConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz);
|
gyroUseConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz);
|
||||||
|
|
||||||
#ifdef TELEMETRY
|
#ifdef TELEMETRY
|
||||||
telemetryUseConfig(&masterConfig.telemetryConfig);
|
telemetryUseConfig(&masterConfig.telemetryConfig);
|
||||||
|
|
|
@ -27,6 +27,7 @@ typedef struct gyro_s {
|
||||||
sensorReadFuncPtr temperature; // read temperature if available
|
sensorReadFuncPtr temperature; // read temperature if available
|
||||||
sensorInterruptFuncPtr intStatus;
|
sensorInterruptFuncPtr intStatus;
|
||||||
float scale; // scalefactor
|
float scale; // scalefactor
|
||||||
|
uint32_t targetLooptime;
|
||||||
} gyro_t;
|
} gyro_t;
|
||||||
|
|
||||||
typedef struct acc_s {
|
typedef struct acc_s {
|
||||||
|
|
|
@ -40,7 +40,7 @@ bool bma280Detect(acc_t *acc)
|
||||||
bool ack = false;
|
bool ack = false;
|
||||||
uint8_t sig = 0;
|
uint8_t sig = 0;
|
||||||
|
|
||||||
ack = i2cRead(MPU_I2C_INSTANCE, BMA280_ADDRESS, 0x00, 1, &sig);
|
ack = i2cRead(MPU_I2C_INSTANCE, BMA280_ADDRESS, 0x00, 1, &sig);
|
||||||
if (!ack || sig != 0xFB)
|
if (!ack || sig != 0xFB)
|
||||||
return false;
|
return false;
|
||||||
|
|
||||||
|
@ -51,8 +51,8 @@ bool bma280Detect(acc_t *acc)
|
||||||
|
|
||||||
static void bma280Init(acc_t *acc)
|
static void bma280Init(acc_t *acc)
|
||||||
{
|
{
|
||||||
i2cWrite(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_PMU_RANGE, 0x08); // +-8g range
|
i2cWrite(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_PMU_RANGE, 0x08); // +-8g range
|
||||||
i2cWrite(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_PMU_BW, 0x0E); // 500Hz BW
|
i2cWrite(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_PMU_BW, 0x0E); // 500Hz BW
|
||||||
|
|
||||||
acc->acc_1G = 512 * 8;
|
acc->acc_1G = 512 * 8;
|
||||||
}
|
}
|
||||||
|
@ -61,7 +61,7 @@ static bool bma280Read(int16_t *accelData)
|
||||||
{
|
{
|
||||||
uint8_t buf[6];
|
uint8_t buf[6];
|
||||||
|
|
||||||
if (!i2cRead(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_ACC_X_LSB, 6, buf)) {
|
if (!i2cRead(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_ACC_X_LSB, 6, buf)) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -63,7 +63,7 @@ bool l3g4200dDetect(gyro_t *gyro)
|
||||||
|
|
||||||
delay(25);
|
delay(25);
|
||||||
|
|
||||||
i2cRead(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_WHO_AM_I, 1, &deviceid);
|
i2cRead(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_WHO_AM_I, 1, &deviceid);
|
||||||
if (deviceid != L3G4200D_ID)
|
if (deviceid != L3G4200D_ID)
|
||||||
return false;
|
return false;
|
||||||
|
|
||||||
|
@ -100,12 +100,12 @@ static void l3g4200dInit(uint8_t lpf)
|
||||||
|
|
||||||
delay(100);
|
delay(100);
|
||||||
|
|
||||||
ack = i2cWrite(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_CTRL_REG4, L3G4200D_FS_SEL_2000DPS);
|
ack = i2cWrite(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_CTRL_REG4, L3G4200D_FS_SEL_2000DPS);
|
||||||
if (!ack)
|
if (!ack)
|
||||||
failureMode(FAILURE_ACC_INIT);
|
failureMode(FAILURE_ACC_INIT);
|
||||||
|
|
||||||
delay(5);
|
delay(5);
|
||||||
i2cWrite(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_CTRL_REG1, L3G4200D_POWER_ON | mpuLowPassFilter);
|
i2cWrite(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_CTRL_REG1, L3G4200D_POWER_ON | mpuLowPassFilter);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Read 3 gyro values into user-provided buffer. No overrun checking is done.
|
// Read 3 gyro values into user-provided buffer. No overrun checking is done.
|
||||||
|
@ -113,7 +113,7 @@ static bool l3g4200dRead(int16_t *gyroADC)
|
||||||
{
|
{
|
||||||
uint8_t buf[6];
|
uint8_t buf[6];
|
||||||
|
|
||||||
if (!i2cRead(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_AUTOINCR | L3G4200D_GYRO_OUT, 6, buf)) {
|
if (!i2cRead(MPU_I2C_INSTANCE, L3G4200D_ADDRESS, L3G4200D_AUTOINCR | L3G4200D_GYRO_OUT, 6, buf)) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -62,7 +62,7 @@
|
||||||
|
|
||||||
#define BLOCK_DATA_UPDATE_CONTINUOUS ((uint8_t)0x00)
|
#define BLOCK_DATA_UPDATE_CONTINUOUS ((uint8_t)0x00)
|
||||||
|
|
||||||
#define BLE_MSB ((uint8_t)0x40)
|
#define BLE_MSB ((uint8_t)0x40)
|
||||||
|
|
||||||
#define BOOT ((uint8_t)0x80)
|
#define BOOT ((uint8_t)0x80)
|
||||||
|
|
||||||
|
|
|
@ -115,15 +115,15 @@ int32_t accelSummedSamples500Hz[3];
|
||||||
|
|
||||||
void lsm303dlhcAccInit(acc_t *acc)
|
void lsm303dlhcAccInit(acc_t *acc)
|
||||||
{
|
{
|
||||||
i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG5_A, BOOT);
|
i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG5_A, BOOT);
|
||||||
|
|
||||||
delay(100);
|
delay(100);
|
||||||
|
|
||||||
i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG1_A, ODR_1344_HZ | AXES_ENABLE);
|
i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG1_A, ODR_1344_HZ | AXES_ENABLE);
|
||||||
|
|
||||||
delay(10);
|
delay(10);
|
||||||
|
|
||||||
i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG4_A, FULLSCALE_4G);
|
i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG4_A, FULLSCALE_4G);
|
||||||
|
|
||||||
delay(100);
|
delay(100);
|
||||||
|
|
||||||
|
|
|
@ -89,7 +89,7 @@ bool mma8452Detect(acc_t *acc)
|
||||||
bool ack = false;
|
bool ack = false;
|
||||||
uint8_t sig = 0;
|
uint8_t sig = 0;
|
||||||
|
|
||||||
ack = i2cRead(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_WHO_AM_I, 1, &sig);
|
ack = i2cRead(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_WHO_AM_I, 1, &sig);
|
||||||
if (!ack || (sig != MMA8452_DEVICE_SIGNATURE && sig != MMA8451_DEVICE_SIGNATURE))
|
if (!ack || (sig != MMA8452_DEVICE_SIGNATURE && sig != MMA8451_DEVICE_SIGNATURE))
|
||||||
return false;
|
return false;
|
||||||
|
|
||||||
|
@ -109,9 +109,9 @@ static inline void mma8451ConfigureInterrupt(void)
|
||||||
IOConfigGPIO(IOGetByTag(IO_TAG(PA5)), IOCFG_IN_FLOATING); // TODO - maybe pullup / pulldown ?
|
IOConfigGPIO(IOGetByTag(IO_TAG(PA5)), IOCFG_IN_FLOATING); // TODO - maybe pullup / pulldown ?
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG3, MMA8452_CTRL_REG3_IPOL); // Interrupt polarity (active HIGH)
|
i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG3, MMA8452_CTRL_REG3_IPOL); // Interrupt polarity (active HIGH)
|
||||||
i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG4, MMA8452_CTRL_REG4_INT_EN_DRDY); // Enable DRDY interrupt (unused by this driver)
|
i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG4, MMA8452_CTRL_REG4_INT_EN_DRDY); // Enable DRDY interrupt (unused by this driver)
|
||||||
i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG5, 0); // DRDY routed to INT2
|
i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG5, 0); // DRDY routed to INT2
|
||||||
}
|
}
|
||||||
|
|
||||||
static void mma8452Init(acc_t *acc)
|
static void mma8452Init(acc_t *acc)
|
||||||
|
|
|
@ -32,7 +32,6 @@
|
||||||
#include "gpio.h"
|
#include "gpio.h"
|
||||||
#include "exti.h"
|
#include "exti.h"
|
||||||
#include "bus_i2c.h"
|
#include "bus_i2c.h"
|
||||||
#include "gyro_sync.h"
|
|
||||||
|
|
||||||
#include "sensor.h"
|
#include "sensor.h"
|
||||||
#include "accgyro.h"
|
#include "accgyro.h"
|
||||||
|
@ -228,43 +227,43 @@ void mpuIntExtiHandler(extiCallbackRec_t *cb)
|
||||||
|
|
||||||
void mpuIntExtiInit(void)
|
void mpuIntExtiInit(void)
|
||||||
{
|
{
|
||||||
static bool mpuExtiInitDone = false;
|
static bool mpuExtiInitDone = false;
|
||||||
|
|
||||||
if (mpuExtiInitDone || !mpuIntExtiConfig) {
|
if (mpuExtiInitDone || !mpuIntExtiConfig) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
#if defined(USE_MPU_DATA_READY_SIGNAL) && defined(USE_EXTI)
|
#if defined(USE_MPU_DATA_READY_SIGNAL) && defined(USE_EXTI)
|
||||||
|
|
||||||
IO_t mpuIntIO = IOGetByTag(mpuIntExtiConfig->tag);
|
IO_t mpuIntIO = IOGetByTag(mpuIntExtiConfig->tag);
|
||||||
|
|
||||||
#ifdef ENSURE_MPU_DATA_READY_IS_LOW
|
#ifdef ENSURE_MPU_DATA_READY_IS_LOW
|
||||||
uint8_t status = IORead(mpuIntIO);
|
uint8_t status = IORead(mpuIntIO);
|
||||||
if (status) {
|
if (status) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
IOInit(mpuIntIO, OWNER_SYSTEM, RESOURCE_INPUT | RESOURCE_EXTI);
|
IOInit(mpuIntIO, OWNER_SYSTEM, RESOURCE_INPUT | RESOURCE_EXTI);
|
||||||
IOConfigGPIO(mpuIntIO, IOCFG_IN_FLOATING); // TODO - maybe pullup / pulldown ?
|
IOConfigGPIO(mpuIntIO, IOCFG_IN_FLOATING); // TODO - maybe pullup / pulldown ?
|
||||||
|
|
||||||
EXTIHandlerInit(&mpuIntCallbackRec, mpuIntExtiHandler);
|
EXTIHandlerInit(&mpuIntCallbackRec, mpuIntExtiHandler);
|
||||||
EXTIConfig(mpuIntIO, &mpuIntCallbackRec, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising);
|
EXTIConfig(mpuIntIO, &mpuIntCallbackRec, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising);
|
||||||
EXTIEnable(mpuIntIO, true);
|
EXTIEnable(mpuIntIO, true);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
mpuExtiInitDone = true;
|
mpuExtiInitDone = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data)
|
static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data)
|
||||||
{
|
{
|
||||||
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;
|
||||||
}
|
}
|
||||||
|
|
||||||
static bool mpuWriteRegisterI2C(uint8_t reg, uint8_t data)
|
static bool mpuWriteRegisterI2C(uint8_t reg, uint8_t data)
|
||||||
{
|
{
|
||||||
bool ack = i2cWrite(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, data);
|
bool ack = i2cWrite(MPU_I2C_INSTANCE, MPU_ADDRESS, reg, data);
|
||||||
return ack;
|
return ack;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -300,11 +299,14 @@ bool mpuGyroRead(int16_t *gyroADC)
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
void checkMPUDataReady(bool *mpuDataReadyPtr) {
|
bool checkMPUDataReady(void)
|
||||||
|
{
|
||||||
|
bool ret;
|
||||||
if (mpuDataReady) {
|
if (mpuDataReady) {
|
||||||
*mpuDataReadyPtr = true;
|
ret = true;
|
||||||
mpuDataReady= false;
|
mpuDataReady= false;
|
||||||
} else {
|
} else {
|
||||||
*mpuDataReadyPtr = false;
|
ret = false;
|
||||||
}
|
}
|
||||||
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
|
@ -120,12 +120,12 @@ typedef bool (*mpuWriteRegisterFunc)(uint8_t reg, uint8_t data);
|
||||||
typedef void(*mpuResetFuncPtr)(void);
|
typedef void(*mpuResetFuncPtr)(void);
|
||||||
|
|
||||||
typedef struct mpuConfiguration_s {
|
typedef struct mpuConfiguration_s {
|
||||||
uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each
|
uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each
|
||||||
mpuReadRegisterFunc read;
|
mpuReadRegisterFunc read;
|
||||||
mpuWriteRegisterFunc write;
|
mpuWriteRegisterFunc write;
|
||||||
mpuReadRegisterFunc slowread;
|
mpuReadRegisterFunc slowread;
|
||||||
mpuWriteRegisterFunc verifywrite;
|
mpuWriteRegisterFunc verifywrite;
|
||||||
mpuResetFuncPtr reset;
|
mpuResetFuncPtr reset;
|
||||||
} mpuConfiguration_t;
|
} mpuConfiguration_t;
|
||||||
|
|
||||||
extern mpuConfiguration_t mpuConfiguration;
|
extern mpuConfiguration_t mpuConfiguration;
|
||||||
|
@ -185,4 +185,4 @@ void mpuIntExtiInit(void);
|
||||||
bool mpuAccRead(int16_t *accData);
|
bool mpuAccRead(int16_t *accData);
|
||||||
bool mpuGyroRead(int16_t *gyroADC);
|
bool mpuGyroRead(int16_t *gyroADC);
|
||||||
mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse);
|
mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse);
|
||||||
void checkMPUDataReady(bool *mpuDataReadyPtr);
|
bool checkMPUDataReady(void);
|
||||||
|
|
|
@ -30,7 +30,6 @@
|
||||||
#include "accgyro.h"
|
#include "accgyro.h"
|
||||||
#include "accgyro_mpu.h"
|
#include "accgyro_mpu.h"
|
||||||
#include "accgyro_mpu3050.h"
|
#include "accgyro_mpu3050.h"
|
||||||
#include "gyro_sync.h"
|
|
||||||
|
|
||||||
// MPU3050, Standard address 0x68
|
// MPU3050, Standard address 0x68
|
||||||
#define MPU3050_ADDRESS 0x68
|
#define MPU3050_ADDRESS 0x68
|
||||||
|
|
|
@ -1,4 +1,4 @@
|
||||||
/*
|
/*
|
||||||
* This file is part of Cleanflight.
|
* This file is part of Cleanflight.
|
||||||
*
|
*
|
||||||
* Cleanflight is free software: you can redistribute it and/or modify
|
* Cleanflight is free software: you can redistribute it and/or modify
|
||||||
|
|
|
@ -49,12 +49,12 @@ static bool mpuSpi6000InitDone = false;
|
||||||
|
|
||||||
|
|
||||||
// Bits
|
// Bits
|
||||||
#define BIT_SLEEP 0x40
|
#define BIT_SLEEP 0x40
|
||||||
#define BIT_H_RESET 0x80
|
#define BIT_H_RESET 0x80
|
||||||
#define BITS_CLKSEL 0x07
|
#define BITS_CLKSEL 0x07
|
||||||
#define MPU_CLK_SEL_PLLGYROX 0x01
|
#define MPU_CLK_SEL_PLLGYROX 0x01
|
||||||
#define MPU_CLK_SEL_PLLGYROZ 0x03
|
#define MPU_CLK_SEL_PLLGYROZ 0x03
|
||||||
#define MPU_EXT_SYNC_GYROX 0x02
|
#define MPU_EXT_SYNC_GYROX 0x02
|
||||||
#define BITS_FS_250DPS 0x00
|
#define BITS_FS_250DPS 0x00
|
||||||
#define BITS_FS_500DPS 0x08
|
#define BITS_FS_500DPS 0x08
|
||||||
#define BITS_FS_1000DPS 0x10
|
#define BITS_FS_1000DPS 0x10
|
||||||
|
@ -74,9 +74,9 @@ static bool mpuSpi6000InitDone = false;
|
||||||
#define BITS_DLPF_CFG_2100HZ_NOLPF 0x07
|
#define BITS_DLPF_CFG_2100HZ_NOLPF 0x07
|
||||||
#define BITS_DLPF_CFG_MASK 0x07
|
#define BITS_DLPF_CFG_MASK 0x07
|
||||||
#define BIT_INT_ANYRD_2CLEAR 0x10
|
#define BIT_INT_ANYRD_2CLEAR 0x10
|
||||||
#define BIT_RAW_RDY_EN 0x01
|
#define BIT_RAW_RDY_EN 0x01
|
||||||
#define BIT_I2C_IF_DIS 0x10
|
#define BIT_I2C_IF_DIS 0x10
|
||||||
#define BIT_INT_STATUS_DATA 0x01
|
#define BIT_INT_STATUS_DATA 0x01
|
||||||
#define BIT_GYRO 3
|
#define BIT_GYRO 3
|
||||||
#define BIT_ACC 2
|
#define BIT_ACC 2
|
||||||
#define BIT_TEMP 1
|
#define BIT_TEMP 1
|
||||||
|
|
|
@ -1,7 +1,7 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#define MPU6000_CONFIG 0x1A
|
#define MPU6000_CONFIG 0x1A
|
||||||
|
|
||||||
#define BITS_DLPF_CFG_256HZ 0x00
|
#define BITS_DLPF_CFG_256HZ 0x00
|
||||||
#define BITS_DLPF_CFG_188HZ 0x01
|
#define BITS_DLPF_CFG_188HZ 0x01
|
||||||
|
|
|
@ -72,7 +72,7 @@ static void mpu6500SpiInit(void)
|
||||||
IOInit(mpuSpi6500CsPin, OWNER_SYSTEM, RESOURCE_SPI);
|
IOInit(mpuSpi6500CsPin, OWNER_SYSTEM, RESOURCE_SPI);
|
||||||
IOConfigGPIO(mpuSpi6500CsPin, 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;
|
||||||
}
|
}
|
||||||
|
|
|
@ -64,7 +64,7 @@ void mpu9250ResetGyro(void)
|
||||||
|
|
||||||
bool mpu9250WriteRegister(uint8_t reg, uint8_t data)
|
bool mpu9250WriteRegister(uint8_t reg, uint8_t data)
|
||||||
{
|
{
|
||||||
ENABLE_MPU9250;
|
ENABLE_MPU9250;
|
||||||
delayMicroseconds(1);
|
delayMicroseconds(1);
|
||||||
spiTransferByte(MPU9250_SPI_INSTANCE, reg);
|
spiTransferByte(MPU9250_SPI_INSTANCE, reg);
|
||||||
spiTransferByte(MPU9250_SPI_INSTANCE, data);
|
spiTransferByte(MPU9250_SPI_INSTANCE, data);
|
||||||
|
@ -76,7 +76,7 @@ bool mpu9250WriteRegister(uint8_t reg, uint8_t data)
|
||||||
|
|
||||||
bool mpu9250ReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
|
bool mpu9250ReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
|
||||||
{
|
{
|
||||||
ENABLE_MPU9250;
|
ENABLE_MPU9250;
|
||||||
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;
|
||||||
|
@ -86,7 +86,7 @@ bool mpu9250ReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
|
||||||
|
|
||||||
bool mpu9250SlowReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
|
bool mpu9250SlowReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
|
||||||
{
|
{
|
||||||
ENABLE_MPU9250;
|
ENABLE_MPU9250;
|
||||||
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);
|
||||||
|
@ -98,7 +98,7 @@ bool mpu9250SlowReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
|
||||||
|
|
||||||
void mpu9250SpiGyroInit(uint8_t lpf)
|
void mpu9250SpiGyroInit(uint8_t lpf)
|
||||||
{
|
{
|
||||||
(void)(lpf);
|
(void)(lpf);
|
||||||
|
|
||||||
mpuIntExtiInit();
|
mpuIntExtiInit();
|
||||||
|
|
||||||
|
@ -126,55 +126,55 @@ void mpu9250SpiAccInit(acc_t *acc)
|
||||||
|
|
||||||
bool verifympu9250WriteRegister(uint8_t reg, uint8_t data)
|
bool verifympu9250WriteRegister(uint8_t reg, uint8_t data)
|
||||||
{
|
{
|
||||||
uint8_t in;
|
uint8_t in;
|
||||||
uint8_t attemptsRemaining = 20;
|
uint8_t attemptsRemaining = 20;
|
||||||
|
|
||||||
mpu9250WriteRegister(reg, data);
|
mpu9250WriteRegister(reg, data);
|
||||||
delayMicroseconds(100);
|
delayMicroseconds(100);
|
||||||
|
|
||||||
do {
|
do {
|
||||||
mpu9250SlowReadRegister(reg, 1, &in);
|
mpu9250SlowReadRegister(reg, 1, &in);
|
||||||
if (in == data) {
|
if (in == data) {
|
||||||
return true;
|
return true;
|
||||||
} else {
|
} else {
|
||||||
debug[3]++;
|
debug[3]++;
|
||||||
mpu9250WriteRegister(reg, data);
|
mpu9250WriteRegister(reg, data);
|
||||||
delayMicroseconds(100);
|
delayMicroseconds(100);
|
||||||
}
|
}
|
||||||
} while (attemptsRemaining--);
|
} while (attemptsRemaining--);
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
static void mpu9250AccAndGyroInit(uint8_t lpf) {
|
static void mpu9250AccAndGyroInit(uint8_t lpf) {
|
||||||
|
|
||||||
if (mpuSpi9250InitDone) {
|
if (mpuSpi9250InitDone) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
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
|
||||||
|
|
||||||
mpu9250WriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
|
mpu9250WriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
|
||||||
delay(50);
|
delay(50);
|
||||||
|
|
||||||
verifympu9250WriteRegister(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
|
verifympu9250WriteRegister(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
|
||||||
|
|
||||||
verifympu9250WriteRegister(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3 | FCB_DISABLED); //Fchoice_b defaults to 00 which makes fchoice 11
|
verifympu9250WriteRegister(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3 | FCB_DISABLED); //Fchoice_b defaults to 00 which makes fchoice 11
|
||||||
|
|
||||||
if (lpf == 4) {
|
if (lpf == 4) {
|
||||||
verifympu9250WriteRegister(MPU_RA_CONFIG, 1); //1KHz, 184DLPF
|
verifympu9250WriteRegister(MPU_RA_CONFIG, 1); //1KHz, 184DLPF
|
||||||
} else if (lpf < 4) {
|
} else if (lpf < 4) {
|
||||||
verifympu9250WriteRegister(MPU_RA_CONFIG, 7); //8KHz, 3600DLPF
|
verifympu9250WriteRegister(MPU_RA_CONFIG, 7); //8KHz, 3600DLPF
|
||||||
} else if (lpf > 4) {
|
} else if (lpf > 4) {
|
||||||
verifympu9250WriteRegister(MPU_RA_CONFIG, 0); //8KHz, 250DLPF
|
verifympu9250WriteRegister(MPU_RA_CONFIG, 0); //8KHz, 250DLPF
|
||||||
}
|
}
|
||||||
|
|
||||||
verifympu9250WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops
|
verifympu9250WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops
|
||||||
|
|
||||||
verifympu9250WriteRegister(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
|
verifympu9250WriteRegister(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
|
||||||
verifympu9250WriteRegister(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
|
verifympu9250WriteRegister(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)
|
||||||
verifympu9250WriteRegister(MPU_RA_INT_ENABLE, 0x01); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly.
|
verifympu9250WriteRegister(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);
|
||||||
|
@ -191,10 +191,10 @@ bool mpu9250SpiDetect(void)
|
||||||
#ifdef MPU9250_CS_PIN
|
#ifdef MPU9250_CS_PIN
|
||||||
mpuSpi9250CsPin = IOGetByTag(IO_TAG(MPU9250_CS_PIN));
|
mpuSpi9250CsPin = IOGetByTag(IO_TAG(MPU9250_CS_PIN));
|
||||||
#endif
|
#endif
|
||||||
IOInit(mpuSpi9250CsPin, OWNER_SYSTEM, RESOURCE_SPI);
|
IOInit(mpuSpi9250CsPin, OWNER_SYSTEM, RESOURCE_SPI);
|
||||||
IOConfigGPIO(mpuSpi9250CsPin, SPI_IO_CS_CFG);
|
IOConfigGPIO(mpuSpi9250CsPin, SPI_IO_CS_CFG);
|
||||||
|
|
||||||
spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed
|
spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed
|
||||||
mpu9250WriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
|
mpu9250WriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
|
||||||
|
|
||||||
do {
|
do {
|
||||||
|
|
|
@ -1,7 +1,7 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#define mpu9250_CONFIG 0x1A
|
#define mpu9250_CONFIG 0x1A
|
||||||
|
|
||||||
/* We should probably use these. :)
|
/* We should probably use these. :)
|
||||||
#define BITS_DLPF_CFG_256HZ 0x00
|
#define BITS_DLPF_CFG_256HZ 0x00
|
||||||
|
|
|
@ -35,11 +35,11 @@ volatile uint16_t adcValues[ADC_CHANNEL_COUNT];
|
||||||
|
|
||||||
uint8_t adcChannelByTag(ioTag_t ioTag)
|
uint8_t adcChannelByTag(ioTag_t ioTag)
|
||||||
{
|
{
|
||||||
for (uint8_t i = 0; i < ARRAYLEN(adcTagMap); i++) {
|
for (uint8_t i = 0; i < ARRAYLEN(adcTagMap); i++) {
|
||||||
if (ioTag == adcTagMap[i].tag)
|
if (ioTag == adcTagMap[i].tag)
|
||||||
return adcTagMap[i].channel;
|
return adcTagMap[i].channel;
|
||||||
}
|
}
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint16_t adcGetChannel(uint8_t channel)
|
uint16_t adcGetChannel(uint8_t channel)
|
||||||
|
|
|
@ -81,7 +81,7 @@ void adcInit(drv_adc_config_t *init)
|
||||||
{
|
{
|
||||||
|
|
||||||
#if !defined(VBAT_ADC_PIN) && !defined(EXTERNAL1_ADC_PIN) && !defined(RSSI_ADC_PIN) && !defined(CURRENT_METER_ADC_PIN)
|
#if !defined(VBAT_ADC_PIN) && !defined(EXTERNAL1_ADC_PIN) && !defined(RSSI_ADC_PIN) && !defined(CURRENT_METER_ADC_PIN)
|
||||||
UNUSED(init);
|
UNUSED(init);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
uint8_t i;
|
uint8_t i;
|
||||||
|
@ -114,7 +114,7 @@ void adcInit(drv_adc_config_t *init)
|
||||||
#ifdef EXTERNAL1_ADC_PIN
|
#ifdef EXTERNAL1_ADC_PIN
|
||||||
if (init->enableExternal1) {
|
if (init->enableExternal1) {
|
||||||
IOInit(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC);
|
IOInit(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC);
|
||||||
IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0));
|
IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0));
|
||||||
adcConfig[ADC_EXTERNAL1].adcChannel = adcChannelByTag(IO_TAG(EXTERNAL1_ADC_PIN));
|
adcConfig[ADC_EXTERNAL1].adcChannel = adcChannelByTag(IO_TAG(EXTERNAL1_ADC_PIN));
|
||||||
adcConfig[ADC_EXTERNAL1].dmaIndex = configuredAdcChannels++;
|
adcConfig[ADC_EXTERNAL1].dmaIndex = configuredAdcChannels++;
|
||||||
adcConfig[ADC_EXTERNAL1].enabled = true;
|
adcConfig[ADC_EXTERNAL1].enabled = true;
|
||||||
|
@ -125,7 +125,7 @@ void adcInit(drv_adc_config_t *init)
|
||||||
#ifdef CURRENT_METER_ADC_PIN
|
#ifdef CURRENT_METER_ADC_PIN
|
||||||
if (init->enableCurrentMeter) {
|
if (init->enableCurrentMeter) {
|
||||||
IOInit(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC);
|
IOInit(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC);
|
||||||
IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0));
|
IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AIN, 0));
|
||||||
adcConfig[ADC_CURRENT].adcChannel = adcChannelByTag(IO_TAG(CURRENT_METER_ADC_PIN));
|
adcConfig[ADC_CURRENT].adcChannel = adcChannelByTag(IO_TAG(CURRENT_METER_ADC_PIN));
|
||||||
adcConfig[ADC_CURRENT].dmaIndex = configuredAdcChannels++;
|
adcConfig[ADC_CURRENT].dmaIndex = configuredAdcChannels++;
|
||||||
adcConfig[ADC_CURRENT].enabled = true;
|
adcConfig[ADC_CURRENT].enabled = true;
|
||||||
|
|
|
@ -107,8 +107,8 @@ void adcInit(drv_adc_config_t *init)
|
||||||
|
|
||||||
#ifdef VBAT_ADC_PIN
|
#ifdef VBAT_ADC_PIN
|
||||||
if (init->enableVBat) {
|
if (init->enableVBat) {
|
||||||
IOInit(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC);
|
IOInit(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC);
|
||||||
IOConfigGPIO(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL));
|
IOConfigGPIO(IOGetByTag(IO_TAG(VBAT_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL));
|
||||||
|
|
||||||
adcConfig[ADC_BATTERY].adcChannel = adcChannelByTag(IO_TAG(VBAT_ADC_PIN));
|
adcConfig[ADC_BATTERY].adcChannel = adcChannelByTag(IO_TAG(VBAT_ADC_PIN));
|
||||||
adcConfig[ADC_BATTERY].dmaIndex = adcChannelCount;
|
adcConfig[ADC_BATTERY].dmaIndex = adcChannelCount;
|
||||||
|
@ -120,8 +120,8 @@ void adcInit(drv_adc_config_t *init)
|
||||||
|
|
||||||
#ifdef RSSI_ADC_PIN
|
#ifdef RSSI_ADC_PIN
|
||||||
if (init->enableRSSI) {
|
if (init->enableRSSI) {
|
||||||
IOInit(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC);
|
IOInit(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC);
|
||||||
IOConfigGPIO(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL));
|
IOConfigGPIO(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL));
|
||||||
|
|
||||||
adcConfig[ADC_RSSI].adcChannel = adcChannelByTag(IO_TAG(RSSI_ADC_PIN));
|
adcConfig[ADC_RSSI].adcChannel = adcChannelByTag(IO_TAG(RSSI_ADC_PIN));
|
||||||
adcConfig[ADC_RSSI].dmaIndex = adcChannelCount;
|
adcConfig[ADC_RSSI].dmaIndex = adcChannelCount;
|
||||||
|
@ -133,8 +133,8 @@ void adcInit(drv_adc_config_t *init)
|
||||||
|
|
||||||
#ifdef CURRENT_METER_ADC_GPIO
|
#ifdef CURRENT_METER_ADC_GPIO
|
||||||
if (init->enableCurrentMeter) {
|
if (init->enableCurrentMeter) {
|
||||||
IOInit(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC);
|
IOInit(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC);
|
||||||
IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL));
|
IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL));
|
||||||
|
|
||||||
adcConfig[ADC_CURRENT].adcChannel = adcChannelByTag(IO_TAG(CURRENT_METER_ADC_PIN));
|
adcConfig[ADC_CURRENT].adcChannel = adcChannelByTag(IO_TAG(CURRENT_METER_ADC_PIN));
|
||||||
adcConfig[ADC_CURRENT].dmaIndex = adcChannelCount;
|
adcConfig[ADC_CURRENT].dmaIndex = adcChannelCount;
|
||||||
|
@ -146,8 +146,8 @@ void adcInit(drv_adc_config_t *init)
|
||||||
|
|
||||||
#ifdef EXTERNAL1_ADC_GPIO
|
#ifdef EXTERNAL1_ADC_GPIO
|
||||||
if (init->enableExternal1) {
|
if (init->enableExternal1) {
|
||||||
IOInit(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC);
|
IOInit(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC);
|
||||||
IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL));
|
IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL));
|
||||||
|
|
||||||
adcConfig[ADC_EXTERNAL1].adcChannel = adcChannelByTag(IO_TAG(EXTERNAL1_ADC_PIN));
|
adcConfig[ADC_EXTERNAL1].adcChannel = adcChannelByTag(IO_TAG(EXTERNAL1_ADC_PIN));
|
||||||
adcConfig[ADC_EXTERNAL1].dmaIndex = adcChannelCount;
|
adcConfig[ADC_EXTERNAL1].dmaIndex = adcChannelCount;
|
||||||
|
|
|
@ -39,8 +39,8 @@
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
const adcDevice_t adcHardware[] = {
|
const adcDevice_t adcHardware[] = {
|
||||||
{ .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream4, .channel = DMA_Channel_0 },
|
{ .ADCx = ADC1, .rccADC = RCC_APB2(ADC1), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream4, .channel = DMA_Channel_0 },
|
||||||
//{ .ADCx = ADC2, .rccADC = RCC_APB2(ADC2), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream1, .channel = DMA_Channel_0 }
|
//{ .ADCx = ADC2, .rccADC = RCC_APB2(ADC2), .rccDMA = RCC_AHB1(DMA2), .DMAy_Streamx = DMA2_Stream1, .channel = DMA_Channel_0 }
|
||||||
};
|
};
|
||||||
|
|
||||||
/* note these could be packed up for saving space */
|
/* note these could be packed up for saving space */
|
||||||
|
@ -75,13 +75,13 @@ const adcTagMap_t adcTagMap[] = {
|
||||||
|
|
||||||
ADCDevice adcDeviceByInstance(ADC_TypeDef *instance)
|
ADCDevice adcDeviceByInstance(ADC_TypeDef *instance)
|
||||||
{
|
{
|
||||||
if (instance == ADC1)
|
if (instance == ADC1)
|
||||||
return ADCDEV_1;
|
return ADCDEV_1;
|
||||||
/*
|
/*
|
||||||
if (instance == ADC2) // TODO add ADC2 and 3
|
if (instance == ADC2) // TODO add ADC2 and 3
|
||||||
return ADCDEV_2;
|
return ADCDEV_2;
|
||||||
*/
|
*/
|
||||||
return ADCINVALID;
|
return ADCINVALID;
|
||||||
}
|
}
|
||||||
|
|
||||||
void adcInit(drv_adc_config_t *init)
|
void adcInit(drv_adc_config_t *init)
|
||||||
|
@ -112,7 +112,7 @@ void adcInit(drv_adc_config_t *init)
|
||||||
#ifdef EXTERNAL1_ADC_PIN
|
#ifdef EXTERNAL1_ADC_PIN
|
||||||
if (init->enableExternal1) {
|
if (init->enableExternal1) {
|
||||||
IOInit(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC);
|
IOInit(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC);
|
||||||
IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL));
|
IOConfigGPIO(IOGetByTag(IO_TAG(EXTERNAL1_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL));
|
||||||
adcConfig[ADC_EXTERNAL1].adcChannel = adcChannelByTag(IO_TAG(EXTERNAL1_ADC_PIN)); //EXTERNAL1_ADC_CHANNEL;
|
adcConfig[ADC_EXTERNAL1].adcChannel = adcChannelByTag(IO_TAG(EXTERNAL1_ADC_PIN)); //EXTERNAL1_ADC_CHANNEL;
|
||||||
adcConfig[ADC_EXTERNAL1].dmaIndex = configuredAdcChannels++;
|
adcConfig[ADC_EXTERNAL1].dmaIndex = configuredAdcChannels++;
|
||||||
adcConfig[ADC_EXTERNAL1].enabled = true;
|
adcConfig[ADC_EXTERNAL1].enabled = true;
|
||||||
|
@ -123,7 +123,7 @@ void adcInit(drv_adc_config_t *init)
|
||||||
#ifdef RSSI_ADC_PIN
|
#ifdef RSSI_ADC_PIN
|
||||||
if (init->enableRSSI) {
|
if (init->enableRSSI) {
|
||||||
IOInit(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC);
|
IOInit(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC);
|
||||||
IOConfigGPIO(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL));
|
IOConfigGPIO(IOGetByTag(IO_TAG(RSSI_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL));
|
||||||
adcConfig[ADC_RSSI].adcChannel = adcChannelByTag(IO_TAG(RSSI_ADC_PIN)); //RSSI_ADC_CHANNEL;
|
adcConfig[ADC_RSSI].adcChannel = adcChannelByTag(IO_TAG(RSSI_ADC_PIN)); //RSSI_ADC_CHANNEL;
|
||||||
adcConfig[ADC_RSSI].dmaIndex = configuredAdcChannels++;
|
adcConfig[ADC_RSSI].dmaIndex = configuredAdcChannels++;
|
||||||
adcConfig[ADC_RSSI].enabled = true;
|
adcConfig[ADC_RSSI].enabled = true;
|
||||||
|
@ -134,7 +134,7 @@ void adcInit(drv_adc_config_t *init)
|
||||||
#ifdef CURRENT_METER_ADC_PIN
|
#ifdef CURRENT_METER_ADC_PIN
|
||||||
if (init->enableCurrentMeter) {
|
if (init->enableCurrentMeter) {
|
||||||
IOInit(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC);
|
IOInit(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), OWNER_SYSTEM, RESOURCE_ADC);
|
||||||
IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL));
|
IOConfigGPIO(IOGetByTag(IO_TAG(CURRENT_METER_ADC_PIN)), IO_CONFIG(GPIO_Mode_AN, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL));
|
||||||
adcConfig[ADC_CURRENT].adcChannel = adcChannelByTag(IO_TAG(CURRENT_METER_ADC_PIN)); //CURRENT_METER_ADC_CHANNEL;
|
adcConfig[ADC_CURRENT].adcChannel = adcChannelByTag(IO_TAG(CURRENT_METER_ADC_PIN)); //CURRENT_METER_ADC_CHANNEL;
|
||||||
adcConfig[ADC_CURRENT].dmaIndex = configuredAdcChannels++;
|
adcConfig[ADC_CURRENT].dmaIndex = configuredAdcChannels++;
|
||||||
adcConfig[ADC_CURRENT].enabled = true;
|
adcConfig[ADC_CURRENT].enabled = true;
|
||||||
|
@ -184,11 +184,11 @@ void adcInit(drv_adc_config_t *init)
|
||||||
|
|
||||||
ADC_InitStructure.ADC_ContinuousConvMode = ENABLE;
|
ADC_InitStructure.ADC_ContinuousConvMode = ENABLE;
|
||||||
ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b;
|
ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b;
|
||||||
ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_T1_CC1;
|
ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_T1_CC1;
|
||||||
ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None;
|
ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None;
|
||||||
ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
|
ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
|
||||||
ADC_InitStructure.ADC_NbrOfConversion = configuredAdcChannels;
|
ADC_InitStructure.ADC_NbrOfConversion = configuredAdcChannels;
|
||||||
ADC_InitStructure.ADC_ScanConvMode = configuredAdcChannels > 1 ? ENABLE : DISABLE; // 1=scan more that one channel in group
|
ADC_InitStructure.ADC_ScanConvMode = configuredAdcChannels > 1 ? ENABLE : DISABLE; // 1=scan more that one channel in group
|
||||||
|
|
||||||
ADC_Init(adc.ADCx, &ADC_InitStructure);
|
ADC_Init(adc.ADCx, &ADC_InitStructure);
|
||||||
|
|
||||||
|
|
|
@ -45,8 +45,8 @@ static bool isEOCConnected = true;
|
||||||
// EXTI14 for BMP085 End of Conversion Interrupt
|
// EXTI14 for BMP085 End of Conversion Interrupt
|
||||||
void bmp085_extiHandler(extiCallbackRec_t* cb)
|
void bmp085_extiHandler(extiCallbackRec_t* cb)
|
||||||
{
|
{
|
||||||
UNUSED(cb);
|
UNUSED(cb);
|
||||||
isConversionComplete = true;
|
isConversionComplete = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool bmp085TestEOCConnected(const bmp085Config_t *config);
|
bool bmp085TestEOCConnected(const bmp085Config_t *config);
|
||||||
|
@ -184,13 +184,13 @@ bool bmp085Detect(const bmp085Config_t *config, baro_t *baro)
|
||||||
|
|
||||||
delay(20); // datasheet says 10ms, we'll be careful and do 20.
|
delay(20); // datasheet says 10ms, we'll be careful and do 20.
|
||||||
|
|
||||||
ack = i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_CHIP_ID__REG, 1, &data); /* read Chip Id */
|
ack = i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_CHIP_ID__REG, 1, &data); /* read Chip Id */
|
||||||
if (ack) {
|
if (ack) {
|
||||||
bmp085.chip_id = BMP085_GET_BITSLICE(data, BMP085_CHIP_ID);
|
bmp085.chip_id = BMP085_GET_BITSLICE(data, BMP085_CHIP_ID);
|
||||||
bmp085.oversampling_setting = 3;
|
bmp085.oversampling_setting = 3;
|
||||||
|
|
||||||
if (bmp085.chip_id == BMP085_CHIP_ID) { /* get bitslice */
|
if (bmp085.chip_id == BMP085_CHIP_ID) { /* get bitslice */
|
||||||
i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_VERSION_REG, 1, &data); /* read Version reg */
|
i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_VERSION_REG, 1, &data); /* read Version reg */
|
||||||
bmp085.ml_version = BMP085_GET_BITSLICE(data, BMP085_ML_VERSION); /* get ML Version */
|
bmp085.ml_version = BMP085_GET_BITSLICE(data, BMP085_ML_VERSION); /* get ML Version */
|
||||||
bmp085.al_version = BMP085_GET_BITSLICE(data, BMP085_AL_VERSION); /* get AL Version */
|
bmp085.al_version = BMP085_GET_BITSLICE(data, BMP085_AL_VERSION); /* get AL Version */
|
||||||
bmp085_get_cal_param(); /* readout bmp085 calibparam structure */
|
bmp085_get_cal_param(); /* readout bmp085 calibparam structure */
|
||||||
|
@ -277,7 +277,7 @@ static void bmp085_start_ut(void)
|
||||||
#if defined(BARO_EOC_GPIO)
|
#if defined(BARO_EOC_GPIO)
|
||||||
isConversionComplete = false;
|
isConversionComplete = false;
|
||||||
#endif
|
#endif
|
||||||
i2cWrite(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_CTRL_MEAS_REG, BMP085_T_MEASURE);
|
i2cWrite(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_CTRL_MEAS_REG, BMP085_T_MEASURE);
|
||||||
}
|
}
|
||||||
|
|
||||||
static void bmp085_get_ut(void)
|
static void bmp085_get_ut(void)
|
||||||
|
@ -291,7 +291,7 @@ static void bmp085_get_ut(void)
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_ADC_OUT_MSB_REG, 2, data);
|
i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_ADC_OUT_MSB_REG, 2, data);
|
||||||
bmp085_ut = (data[0] << 8) | data[1];
|
bmp085_ut = (data[0] << 8) | data[1];
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -305,7 +305,7 @@ static void bmp085_start_up(void)
|
||||||
isConversionComplete = false;
|
isConversionComplete = false;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
i2cWrite(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_CTRL_MEAS_REG, ctrl_reg_data);
|
i2cWrite(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_CTRL_MEAS_REG, ctrl_reg_data);
|
||||||
}
|
}
|
||||||
|
|
||||||
/** read out up for pressure conversion
|
/** read out up for pressure conversion
|
||||||
|
@ -323,7 +323,7 @@ static void bmp085_get_up(void)
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_ADC_OUT_MSB_REG, 3, data);
|
i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_ADC_OUT_MSB_REG, 3, data);
|
||||||
bmp085_up = (((uint32_t) data[0] << 16) | ((uint32_t) data[1] << 8) | (uint32_t) data[2])
|
bmp085_up = (((uint32_t) data[0] << 16) | ((uint32_t) data[1] << 8) | (uint32_t) data[2])
|
||||||
>> (8 - bmp085.oversampling_setting);
|
>> (8 - bmp085.oversampling_setting);
|
||||||
}
|
}
|
||||||
|
@ -343,7 +343,7 @@ STATIC_UNIT_TESTED void bmp085_calculate(int32_t *pressure, int32_t *temperature
|
||||||
static void bmp085_get_cal_param(void)
|
static void bmp085_get_cal_param(void)
|
||||||
{
|
{
|
||||||
uint8_t data[22];
|
uint8_t data[22];
|
||||||
i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_PROM_START__ADDR, BMP085_PROM_DATA__LEN, data);
|
i2cRead(BARO_I2C_INSTANCE, BMP085_I2C_ADDR, BMP085_PROM_START__ADDR, BMP085_PROM_DATA__LEN, data);
|
||||||
|
|
||||||
/*parameters AC1-AC6*/
|
/*parameters AC1-AC6*/
|
||||||
bmp085.cal_param.ac1 = (data[0] << 8) | data[1];
|
bmp085.cal_param.ac1 = (data[0] << 8) | data[1];
|
||||||
|
|
|
@ -18,8 +18,8 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
typedef struct bmp085Config_s {
|
typedef struct bmp085Config_s {
|
||||||
ioTag_t xclrIO;
|
ioTag_t xclrIO;
|
||||||
ioTag_t eocIO;
|
ioTag_t eocIO;
|
||||||
} bmp085Config_t;
|
} bmp085Config_t;
|
||||||
|
|
||||||
bool bmp085Detect(const bmp085Config_t *config, baro_t *baro);
|
bool bmp085Detect(const bmp085Config_t *config, baro_t *baro);
|
||||||
|
|
|
@ -83,14 +83,14 @@ bool bmp280Detect(baro_t *baro)
|
||||||
// set oversampling + power mode (forced), and start sampling
|
// set oversampling + power mode (forced), and start sampling
|
||||||
bmp280WriteRegister(BMP280_CTRL_MEAS_REG, BMP280_MODE);
|
bmp280WriteRegister(BMP280_CTRL_MEAS_REG, BMP280_MODE);
|
||||||
#else
|
#else
|
||||||
i2cRead(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_CHIP_ID_REG, 1, &bmp280_chip_id); /* read Chip Id */
|
i2cRead(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_CHIP_ID_REG, 1, &bmp280_chip_id); /* read Chip Id */
|
||||||
if (bmp280_chip_id != BMP280_DEFAULT_CHIP_ID)
|
if (bmp280_chip_id != BMP280_DEFAULT_CHIP_ID)
|
||||||
return false;
|
return false;
|
||||||
|
|
||||||
// read calibration
|
// read calibration
|
||||||
i2cRead(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_TEMPERATURE_CALIB_DIG_T1_LSB_REG, 24, (uint8_t *)&bmp280_cal);
|
i2cRead(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_TEMPERATURE_CALIB_DIG_T1_LSB_REG, 24, (uint8_t *)&bmp280_cal);
|
||||||
// set oversampling + power mode (forced), and start sampling
|
// set oversampling + power mode (forced), and start sampling
|
||||||
i2cWrite(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_CTRL_MEAS_REG, BMP280_MODE);
|
i2cWrite(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_CTRL_MEAS_REG, BMP280_MODE);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
bmp280InitDone = true;
|
bmp280InitDone = true;
|
||||||
|
@ -129,7 +129,7 @@ static void bmp280_start_up(void)
|
||||||
{
|
{
|
||||||
// start measurement
|
// start measurement
|
||||||
// set oversampling + power mode (forced), and start sampling
|
// set oversampling + power mode (forced), and start sampling
|
||||||
i2cWrite(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_CTRL_MEAS_REG, BMP280_MODE);
|
i2cWrite(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_CTRL_MEAS_REG, BMP280_MODE);
|
||||||
}
|
}
|
||||||
|
|
||||||
static void bmp280_get_up(void)
|
static void bmp280_get_up(void)
|
||||||
|
@ -137,7 +137,7 @@ static void bmp280_get_up(void)
|
||||||
uint8_t data[BMP280_DATA_FRAME_SIZE];
|
uint8_t data[BMP280_DATA_FRAME_SIZE];
|
||||||
|
|
||||||
// read data from sensor
|
// read data from sensor
|
||||||
i2cRead(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_PRESSURE_MSB_REG, BMP280_DATA_FRAME_SIZE, data);
|
i2cRead(BARO_I2C_INSTANCE, BMP280_I2C_ADDR, BMP280_PRESSURE_MSB_REG, BMP280_DATA_FRAME_SIZE, data);
|
||||||
bmp280_up = (int32_t)((((uint32_t)(data[0])) << 12) | (((uint32_t)(data[1])) << 4) | ((uint32_t)data[2] >> 4));
|
bmp280_up = (int32_t)((((uint32_t)(data[0])) << 12) | (((uint32_t)(data[1])) << 4) | ((uint32_t)data[2] >> 4));
|
||||||
bmp280_ut = (int32_t)((((uint32_t)(data[3])) << 12) | (((uint32_t)(data[4])) << 4) | ((uint32_t)data[5] >> 4));
|
bmp280_ut = (int32_t)((((uint32_t)(data[3])) << 12) | (((uint32_t)(data[4])) << 4) | ((uint32_t)data[5] >> 4));
|
||||||
}
|
}
|
||||||
|
|
|
@ -67,7 +67,7 @@ bool ms5611Detect(baro_t *baro)
|
||||||
|
|
||||||
delay(10); // No idea how long the chip takes to power-up, but let's make it 10ms
|
delay(10); // No idea how long the chip takes to power-up, but let's make it 10ms
|
||||||
|
|
||||||
ack = i2cRead(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_PROM_RD, 1, &sig);
|
ack = i2cRead(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_PROM_RD, 1, &sig);
|
||||||
if (!ack)
|
if (!ack)
|
||||||
return false;
|
return false;
|
||||||
|
|
||||||
|
@ -93,14 +93,14 @@ bool ms5611Detect(baro_t *baro)
|
||||||
|
|
||||||
static void ms5611_reset(void)
|
static void ms5611_reset(void)
|
||||||
{
|
{
|
||||||
i2cWrite(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_RESET, 1);
|
i2cWrite(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_RESET, 1);
|
||||||
delayMicroseconds(2800);
|
delayMicroseconds(2800);
|
||||||
}
|
}
|
||||||
|
|
||||||
static uint16_t ms5611_prom(int8_t coef_num)
|
static uint16_t ms5611_prom(int8_t coef_num)
|
||||||
{
|
{
|
||||||
uint8_t rxbuf[2] = { 0, 0 };
|
uint8_t rxbuf[2] = { 0, 0 };
|
||||||
i2cRead(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_PROM_RD + coef_num * 2, 2, rxbuf); // send PROM READ command
|
i2cRead(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_PROM_RD + coef_num * 2, 2, rxbuf); // send PROM READ command
|
||||||
return rxbuf[0] << 8 | rxbuf[1];
|
return rxbuf[0] << 8 | rxbuf[1];
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -137,13 +137,13 @@ STATIC_UNIT_TESTED int8_t ms5611_crc(uint16_t *prom)
|
||||||
static uint32_t ms5611_read_adc(void)
|
static uint32_t ms5611_read_adc(void)
|
||||||
{
|
{
|
||||||
uint8_t rxbuf[3];
|
uint8_t rxbuf[3];
|
||||||
i2cRead(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_ADC_READ, 3, rxbuf); // read ADC
|
i2cRead(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_ADC_READ, 3, rxbuf); // read ADC
|
||||||
return (rxbuf[0] << 16) | (rxbuf[1] << 8) | rxbuf[2];
|
return (rxbuf[0] << 16) | (rxbuf[1] << 8) | rxbuf[2];
|
||||||
}
|
}
|
||||||
|
|
||||||
static void ms5611_start_ut(void)
|
static void ms5611_start_ut(void)
|
||||||
{
|
{
|
||||||
i2cWrite(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_ADC_CONV + CMD_ADC_D2 + ms5611_osr, 1); // D2 (temperature) conversion start!
|
i2cWrite(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_ADC_CONV + CMD_ADC_D2 + ms5611_osr, 1); // D2 (temperature) conversion start!
|
||||||
}
|
}
|
||||||
|
|
||||||
static void ms5611_get_ut(void)
|
static void ms5611_get_ut(void)
|
||||||
|
@ -153,7 +153,7 @@ static void ms5611_get_ut(void)
|
||||||
|
|
||||||
static void ms5611_start_up(void)
|
static void ms5611_start_up(void)
|
||||||
{
|
{
|
||||||
i2cWrite(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_ADC_CONV + CMD_ADC_D1 + ms5611_osr, 1); // D1 (pressure) conversion start!
|
i2cWrite(BARO_I2C_INSTANCE, MS5611_ADDR, CMD_ADC_CONV + CMD_ADC_D1 + ms5611_osr, 1); // D1 (pressure) conversion start!
|
||||||
}
|
}
|
||||||
|
|
||||||
static void ms5611_get_up(void)
|
static void ms5611_get_up(void)
|
||||||
|
|
|
@ -48,218 +48,218 @@ static volatile uint16_t i2cErrorCount = 0;
|
||||||
|
|
||||||
static void I2C_delay(void)
|
static void I2C_delay(void)
|
||||||
{
|
{
|
||||||
volatile int i = 7;
|
volatile int i = 7;
|
||||||
while (i) {
|
while (i) {
|
||||||
i--;
|
i--;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
static bool I2C_Start(void)
|
static bool I2C_Start(void)
|
||||||
{
|
{
|
||||||
SDA_H;
|
SDA_H;
|
||||||
SCL_H;
|
SCL_H;
|
||||||
I2C_delay();
|
I2C_delay();
|
||||||
if (!SDA_read) {
|
if (!SDA_read) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
SDA_L;
|
SDA_L;
|
||||||
I2C_delay();
|
I2C_delay();
|
||||||
if (SDA_read) {
|
if (SDA_read) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
SDA_L;
|
SDA_L;
|
||||||
I2C_delay();
|
I2C_delay();
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
static void I2C_Stop(void)
|
static void I2C_Stop(void)
|
||||||
{
|
{
|
||||||
SCL_L;
|
SCL_L;
|
||||||
I2C_delay();
|
I2C_delay();
|
||||||
SDA_L;
|
SDA_L;
|
||||||
I2C_delay();
|
I2C_delay();
|
||||||
SCL_H;
|
SCL_H;
|
||||||
I2C_delay();
|
I2C_delay();
|
||||||
SDA_H;
|
SDA_H;
|
||||||
I2C_delay();
|
I2C_delay();
|
||||||
}
|
}
|
||||||
|
|
||||||
static void I2C_Ack(void)
|
static void I2C_Ack(void)
|
||||||
{
|
{
|
||||||
SCL_L;
|
SCL_L;
|
||||||
I2C_delay();
|
I2C_delay();
|
||||||
SDA_L;
|
SDA_L;
|
||||||
I2C_delay();
|
I2C_delay();
|
||||||
SCL_H;
|
SCL_H;
|
||||||
I2C_delay();
|
I2C_delay();
|
||||||
SCL_L;
|
SCL_L;
|
||||||
I2C_delay();
|
I2C_delay();
|
||||||
}
|
}
|
||||||
|
|
||||||
static void I2C_NoAck(void)
|
static void I2C_NoAck(void)
|
||||||
{
|
{
|
||||||
SCL_L;
|
SCL_L;
|
||||||
I2C_delay();
|
I2C_delay();
|
||||||
SDA_H;
|
SDA_H;
|
||||||
I2C_delay();
|
I2C_delay();
|
||||||
SCL_H;
|
SCL_H;
|
||||||
I2C_delay();
|
I2C_delay();
|
||||||
SCL_L;
|
SCL_L;
|
||||||
I2C_delay();
|
I2C_delay();
|
||||||
}
|
}
|
||||||
|
|
||||||
static bool I2C_WaitAck(void)
|
static bool I2C_WaitAck(void)
|
||||||
{
|
{
|
||||||
SCL_L;
|
SCL_L;
|
||||||
I2C_delay();
|
I2C_delay();
|
||||||
SDA_H;
|
SDA_H;
|
||||||
I2C_delay();
|
I2C_delay();
|
||||||
SCL_H;
|
SCL_H;
|
||||||
I2C_delay();
|
I2C_delay();
|
||||||
if (SDA_read) {
|
if (SDA_read) {
|
||||||
SCL_L;
|
SCL_L;
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
SCL_L;
|
SCL_L;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
static void I2C_SendByte(uint8_t byte)
|
static void I2C_SendByte(uint8_t byte)
|
||||||
{
|
{
|
||||||
uint8_t i = 8;
|
uint8_t i = 8;
|
||||||
while (i--) {
|
while (i--) {
|
||||||
SCL_L;
|
SCL_L;
|
||||||
I2C_delay();
|
I2C_delay();
|
||||||
if (byte & 0x80) {
|
if (byte & 0x80) {
|
||||||
SDA_H;
|
SDA_H;
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
SDA_L;
|
SDA_L;
|
||||||
}
|
}
|
||||||
byte <<= 1;
|
byte <<= 1;
|
||||||
I2C_delay();
|
I2C_delay();
|
||||||
SCL_H;
|
SCL_H;
|
||||||
I2C_delay();
|
I2C_delay();
|
||||||
}
|
}
|
||||||
SCL_L;
|
SCL_L;
|
||||||
}
|
}
|
||||||
|
|
||||||
static uint8_t I2C_ReceiveByte(void)
|
static uint8_t I2C_ReceiveByte(void)
|
||||||
{
|
{
|
||||||
uint8_t i = 8;
|
uint8_t i = 8;
|
||||||
uint8_t byte = 0;
|
uint8_t byte = 0;
|
||||||
|
|
||||||
SDA_H;
|
SDA_H;
|
||||||
while (i--) {
|
while (i--) {
|
||||||
byte <<= 1;
|
byte <<= 1;
|
||||||
SCL_L;
|
SCL_L;
|
||||||
I2C_delay();
|
I2C_delay();
|
||||||
SCL_H;
|
SCL_H;
|
||||||
I2C_delay();
|
I2C_delay();
|
||||||
if (SDA_read) {
|
if (SDA_read) {
|
||||||
byte |= 0x01;
|
byte |= 0x01;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
SCL_L;
|
SCL_L;
|
||||||
return byte;
|
return byte;
|
||||||
}
|
}
|
||||||
|
|
||||||
void i2cInit(I2CDevice device)
|
void i2cInit(I2CDevice device)
|
||||||
{
|
{
|
||||||
UNUSED(device);
|
UNUSED(device);
|
||||||
|
|
||||||
scl = IOGetByTag(IO_TAG(SOFT_I2C_SCL));
|
scl = IOGetByTag(IO_TAG(SOFT_I2C_SCL));
|
||||||
sda = IOGetByTag(IO_TAG(SOFT_I2C_SDA));
|
sda = IOGetByTag(IO_TAG(SOFT_I2C_SDA));
|
||||||
|
|
||||||
IOConfigGPIO(scl, IOCFG_OUT_OD);
|
IOConfigGPIO(scl, IOCFG_OUT_OD);
|
||||||
IOConfigGPIO(sda, IOCFG_OUT_OD);
|
IOConfigGPIO(sda, IOCFG_OUT_OD);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool i2cWriteBuffer(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t len, uint8_t * data)
|
bool i2cWriteBuffer(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t len, uint8_t * data)
|
||||||
{
|
{
|
||||||
UNUSED(device);
|
UNUSED(device);
|
||||||
|
|
||||||
int i;
|
int i;
|
||||||
if (!I2C_Start()) {
|
if (!I2C_Start()) {
|
||||||
i2cErrorCount++;
|
i2cErrorCount++;
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
I2C_SendByte(addr << 1 | I2C_Direction_Transmitter);
|
I2C_SendByte(addr << 1 | I2C_Direction_Transmitter);
|
||||||
if (!I2C_WaitAck()) {
|
if (!I2C_WaitAck()) {
|
||||||
I2C_Stop();
|
I2C_Stop();
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
I2C_SendByte(reg);
|
I2C_SendByte(reg);
|
||||||
I2C_WaitAck();
|
I2C_WaitAck();
|
||||||
for (i = 0; i < len; i++) {
|
for (i = 0; i < len; i++) {
|
||||||
I2C_SendByte(data[i]);
|
I2C_SendByte(data[i]);
|
||||||
if (!I2C_WaitAck()) {
|
if (!I2C_WaitAck()) {
|
||||||
I2C_Stop();
|
I2C_Stop();
|
||||||
i2cErrorCount++;
|
i2cErrorCount++;
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
I2C_Stop();
|
I2C_Stop();
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool i2cWrite(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t data)
|
bool i2cWrite(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t data)
|
||||||
{
|
{
|
||||||
UNUSED(device);
|
UNUSED(device);
|
||||||
|
|
||||||
if (!I2C_Start()) {
|
if (!I2C_Start()) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
I2C_SendByte(addr << 1 | I2C_Direction_Transmitter);
|
I2C_SendByte(addr << 1 | I2C_Direction_Transmitter);
|
||||||
if (!I2C_WaitAck()) {
|
if (!I2C_WaitAck()) {
|
||||||
I2C_Stop();
|
I2C_Stop();
|
||||||
i2cErrorCount++;
|
i2cErrorCount++;
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
I2C_SendByte(reg);
|
I2C_SendByte(reg);
|
||||||
I2C_WaitAck();
|
I2C_WaitAck();
|
||||||
I2C_SendByte(data);
|
I2C_SendByte(data);
|
||||||
I2C_WaitAck();
|
I2C_WaitAck();
|
||||||
I2C_Stop();
|
I2C_Stop();
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool i2cRead(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf)
|
bool i2cRead(I2CDevice device, uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf)
|
||||||
{
|
{
|
||||||
UNUSED(device);
|
UNUSED(device);
|
||||||
|
|
||||||
if (!I2C_Start()) {
|
if (!I2C_Start()) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
I2C_SendByte(addr << 1 | I2C_Direction_Transmitter);
|
I2C_SendByte(addr << 1 | I2C_Direction_Transmitter);
|
||||||
if (!I2C_WaitAck()) {
|
if (!I2C_WaitAck()) {
|
||||||
I2C_Stop();
|
I2C_Stop();
|
||||||
i2cErrorCount++;
|
i2cErrorCount++;
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
I2C_SendByte(reg);
|
I2C_SendByte(reg);
|
||||||
I2C_WaitAck();
|
I2C_WaitAck();
|
||||||
I2C_Start();
|
I2C_Start();
|
||||||
I2C_SendByte(addr << 1 | I2C_Direction_Receiver);
|
I2C_SendByte(addr << 1 | I2C_Direction_Receiver);
|
||||||
I2C_WaitAck();
|
I2C_WaitAck();
|
||||||
while (len) {
|
while (len) {
|
||||||
*buf = I2C_ReceiveByte();
|
*buf = I2C_ReceiveByte();
|
||||||
if (len == 1) {
|
if (len == 1) {
|
||||||
I2C_NoAck();
|
I2C_NoAck();
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
I2C_Ack();
|
I2C_Ack();
|
||||||
}
|
}
|
||||||
buf++;
|
buf++;
|
||||||
len--;
|
len--;
|
||||||
}
|
}
|
||||||
I2C_Stop();
|
I2C_Stop();
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint16_t i2cGetErrorCounter(void)
|
uint16_t i2cGetErrorCounter(void)
|
||||||
{
|
{
|
||||||
return i2cErrorCount;
|
return i2cErrorCount;
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -83,8 +83,8 @@ void i2cInit(I2CDevice device)
|
||||||
RCC_ClockCmd(i2c->rcc, ENABLE);
|
RCC_ClockCmd(i2c->rcc, ENABLE);
|
||||||
RCC_I2CCLKConfig(I2Cx == I2C2 ? RCC_I2C2CLK_SYSCLK : RCC_I2C1CLK_SYSCLK);
|
RCC_I2CCLKConfig(I2Cx == I2C2 ? RCC_I2C2CLK_SYSCLK : RCC_I2C1CLK_SYSCLK);
|
||||||
|
|
||||||
IOConfigGPIOAF(scl, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_NOPULL), GPIO_AF_4);
|
IOConfigGPIOAF(scl, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_UP), GPIO_AF_4);
|
||||||
IOConfigGPIOAF(sda, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_NOPULL), GPIO_AF_4);
|
IOConfigGPIOAF(sda, IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_UP), GPIO_AF_4);
|
||||||
|
|
||||||
I2C_InitTypeDef i2cInit = {
|
I2C_InitTypeDef i2cInit = {
|
||||||
.I2C_Mode = I2C_Mode_I2C,
|
.I2C_Mode = I2C_Mode_I2C,
|
||||||
|
|
|
@ -52,23 +52,23 @@ typedef enum {
|
||||||
} SPIClockDivider_e;
|
} SPIClockDivider_e;
|
||||||
|
|
||||||
typedef enum SPIDevice {
|
typedef enum SPIDevice {
|
||||||
SPIINVALID = -1,
|
SPIINVALID = -1,
|
||||||
SPIDEV_1 = 0,
|
SPIDEV_1 = 0,
|
||||||
SPIDEV_2,
|
SPIDEV_2,
|
||||||
SPIDEV_3,
|
SPIDEV_3,
|
||||||
SPIDEV_MAX = SPIDEV_3,
|
SPIDEV_MAX = SPIDEV_3,
|
||||||
} SPIDevice;
|
} SPIDevice;
|
||||||
|
|
||||||
typedef struct SPIDevice_s {
|
typedef struct SPIDevice_s {
|
||||||
SPI_TypeDef *dev;
|
SPI_TypeDef *dev;
|
||||||
ioTag_t nss;
|
ioTag_t nss;
|
||||||
ioTag_t sck;
|
ioTag_t sck;
|
||||||
ioTag_t mosi;
|
ioTag_t mosi;
|
||||||
ioTag_t miso;
|
ioTag_t miso;
|
||||||
rccPeriphTag_t rcc;
|
rccPeriphTag_t rcc;
|
||||||
uint8_t af;
|
uint8_t af;
|
||||||
volatile uint16_t errorCount;
|
volatile uint16_t errorCount;
|
||||||
bool sdcard;
|
bool sdcard;
|
||||||
} spiDevice_t;
|
} spiDevice_t;
|
||||||
|
|
||||||
bool spiInit(SPIDevice device);
|
bool spiInit(SPIDevice device);
|
||||||
|
|
|
@ -64,7 +64,7 @@ bool ak8975detect(mag_t *mag)
|
||||||
bool ack = false;
|
bool ack = false;
|
||||||
uint8_t sig = 0;
|
uint8_t sig = 0;
|
||||||
|
|
||||||
ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_WHO_AM_I, 1, &sig);
|
ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_WHO_AM_I, 1, &sig);
|
||||||
if (!ack || sig != 'H') // 0x48 / 01001000 / 'H'
|
if (!ack || sig != 'H') // 0x48 / 01001000 / 'H'
|
||||||
return false;
|
return false;
|
||||||
|
|
||||||
|
@ -86,24 +86,24 @@ void ak8975Init()
|
||||||
|
|
||||||
UNUSED(ack);
|
UNUSED(ack);
|
||||||
|
|
||||||
ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x00); // power down before entering fuse mode
|
ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x00); // power down before entering fuse mode
|
||||||
delay(20);
|
delay(20);
|
||||||
|
|
||||||
ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x0F); // Enter Fuse ROM access mode
|
ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x0F); // Enter Fuse ROM access mode
|
||||||
delay(10);
|
delay(10);
|
||||||
|
|
||||||
ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975A_ASAX, 3, &buffer[0]); // Read the x-, y-, and z-axis calibration values
|
ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975A_ASAX, 3, &buffer[0]); // Read the x-, y-, and z-axis calibration values
|
||||||
delay(10);
|
delay(10);
|
||||||
|
|
||||||
ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x00); // power down after reading.
|
ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x00); // power down after reading.
|
||||||
delay(10);
|
delay(10);
|
||||||
|
|
||||||
// Clear status registers
|
// Clear status registers
|
||||||
ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS1, 1, &status);
|
ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS1, 1, &status);
|
||||||
ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS2, 1, &status);
|
ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS2, 1, &status);
|
||||||
|
|
||||||
// Trigger first measurement
|
// Trigger first measurement
|
||||||
ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x01);
|
ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x01);
|
||||||
}
|
}
|
||||||
|
|
||||||
#define BIT_STATUS1_REG_DATA_READY (1 << 0)
|
#define BIT_STATUS1_REG_DATA_READY (1 << 0)
|
||||||
|
@ -118,13 +118,13 @@ bool ak8975Read(int16_t *magData)
|
||||||
uint8_t status;
|
uint8_t status;
|
||||||
uint8_t buf[6];
|
uint8_t buf[6];
|
||||||
|
|
||||||
ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS1, 1, &status);
|
ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS1, 1, &status);
|
||||||
if (!ack || (status & BIT_STATUS1_REG_DATA_READY) == 0) {
|
if (!ack || (status & BIT_STATUS1_REG_DATA_READY) == 0) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
#if 1 // USE_I2C_SINGLE_BYTE_READS
|
#if 1 // USE_I2C_SINGLE_BYTE_READS
|
||||||
ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_HXL, 6, buf); // read from AK8975_MAG_REG_HXL to AK8975_MAG_REG_HZH
|
ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_HXL, 6, buf); // read from AK8975_MAG_REG_HXL to AK8975_MAG_REG_HZH
|
||||||
#else
|
#else
|
||||||
for (uint8_t i = 0; i < 6; i++) {
|
for (uint8_t i = 0; i < 6; i++) {
|
||||||
ack = i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_HXL + i, 1, &buf[i]); // read from AK8975_MAG_REG_HXL to AK8975_MAG_REG_HZH
|
ack = i2cRead(AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_HXL + i, 1, &buf[i]); // read from AK8975_MAG_REG_HXL to AK8975_MAG_REG_HZH
|
||||||
|
@ -134,7 +134,7 @@ bool ak8975Read(int16_t *magData)
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS2, 1, &status);
|
ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS2, 1, &status);
|
||||||
if (!ack) {
|
if (!ack) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
@ -152,6 +152,6 @@ bool ak8975Read(int16_t *magData)
|
||||||
magData[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * 4;
|
magData[Z] = -(int16_t)(buf[5] << 8 | buf[4]) * 4;
|
||||||
|
|
||||||
|
|
||||||
ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x01); // start reading again
|
ack = i2cWrite(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_CNTL, 0x01); // start reading again
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
|
@ -204,7 +204,7 @@ bool hmc5883lDetect(mag_t* mag, const hmc5883Config_t *hmc5883ConfigToUse)
|
||||||
|
|
||||||
hmc5883Config = hmc5883ConfigToUse;
|
hmc5883Config = hmc5883ConfigToUse;
|
||||||
|
|
||||||
ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, 0x0A, 1, &sig);
|
ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, 0x0A, 1, &sig);
|
||||||
if (!ack || sig != 'H')
|
if (!ack || sig != 'H')
|
||||||
return false;
|
return false;
|
||||||
|
|
||||||
|
@ -241,15 +241,15 @@ void hmc5883lInit(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
delay(50);
|
delay(50);
|
||||||
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to pos bias
|
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to pos bias
|
||||||
// Note that the very first measurement after a gain change maintains the same gain as the previous setting.
|
// Note that the very first measurement after a gain change maintains the same gain as the previous setting.
|
||||||
// The new gain setting is effective from the second measurement and on.
|
// The new gain setting is effective from the second measurement and on.
|
||||||
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFB, 0x60); // Set the Gain to 2.5Ga (7:5->011)
|
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFB, 0x60); // Set the Gain to 2.5Ga (7:5->011)
|
||||||
delay(100);
|
delay(100);
|
||||||
hmc5883lRead(magADC);
|
hmc5883lRead(magADC);
|
||||||
|
|
||||||
for (i = 0; i < 10; i++) { // Collect 10 samples
|
for (i = 0; i < 10; i++) { // Collect 10 samples
|
||||||
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 1);
|
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 1);
|
||||||
delay(50);
|
delay(50);
|
||||||
hmc5883lRead(magADC); // Get the raw values in case the scales have already been changed.
|
hmc5883lRead(magADC); // Get the raw values in case the scales have already been changed.
|
||||||
|
|
||||||
|
@ -267,9 +267,9 @@ void hmc5883lInit(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
// Apply the negative bias. (Same gain)
|
// Apply the negative bias. (Same gain)
|
||||||
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_NEG_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to negative bias.
|
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_NEG_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to negative bias.
|
||||||
for (i = 0; i < 10; i++) {
|
for (i = 0; i < 10; i++) {
|
||||||
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 1);
|
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 1);
|
||||||
delay(50);
|
delay(50);
|
||||||
hmc5883lRead(magADC); // Get the raw values in case the scales have already been changed.
|
hmc5883lRead(magADC); // Get the raw values in case the scales have already been changed.
|
||||||
|
|
||||||
|
@ -291,9 +291,9 @@ void hmc5883lInit(void)
|
||||||
magGain[Z] = fabsf(660.0f * HMC58X3_Z_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Z]);
|
magGain[Z] = fabsf(660.0f * HMC58X3_Z_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Z]);
|
||||||
|
|
||||||
// leave test mode
|
// leave test mode
|
||||||
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x70); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode
|
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x70); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode
|
||||||
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFB, 0x20); // Configuration Register B -- 001 00000 configuration gain 1.3Ga
|
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFB, 0x20); // Configuration Register B -- 001 00000 configuration gain 1.3Ga
|
||||||
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 0x00); // Mode register -- 000000 00 continuous Conversion Mode
|
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 0x00); // Mode register -- 000000 00 continuous Conversion Mode
|
||||||
delay(100);
|
delay(100);
|
||||||
|
|
||||||
if (!bret) { // Something went wrong so get a best guess
|
if (!bret) { // Something went wrong so get a best guess
|
||||||
|
@ -309,7 +309,7 @@ bool hmc5883lRead(int16_t *magData)
|
||||||
{
|
{
|
||||||
uint8_t buf[6];
|
uint8_t buf[6];
|
||||||
|
|
||||||
bool ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf);
|
bool ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf);
|
||||||
if (!ack) {
|
if (!ack) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
|
@ -32,7 +32,7 @@ static dmaHandlers_t dmaHandlers;
|
||||||
|
|
||||||
void dmaNoOpHandler(DMA_Stream_TypeDef *stream)
|
void dmaNoOpHandler(DMA_Stream_TypeDef *stream)
|
||||||
{
|
{
|
||||||
UNUSED(stream);
|
UNUSED(stream);
|
||||||
}
|
}
|
||||||
|
|
||||||
void DMA1_Stream2_IRQHandler(void)
|
void DMA1_Stream2_IRQHandler(void)
|
||||||
|
|
|
@ -4,48 +4,40 @@
|
||||||
* Created on: 3 aug. 2015
|
* Created on: 3 aug. 2015
|
||||||
* Author: borisb
|
* Author: borisb
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include <stdbool.h>
|
#include <stdbool.h>
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
#include <stdlib.h>
|
|
||||||
|
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
#include "build_config.h"
|
#include "build_config.h"
|
||||||
|
|
||||||
#include "common/axis.h"
|
|
||||||
#include "common/maths.h"
|
|
||||||
|
|
||||||
#include "drivers/sensor.h"
|
#include "drivers/sensor.h"
|
||||||
#include "drivers/accgyro.h"
|
#include "drivers/accgyro.h"
|
||||||
#include "drivers/gyro_sync.h"
|
#include "drivers/gyro_sync.h"
|
||||||
|
|
||||||
#include "sensors/sensors.h"
|
|
||||||
#include "sensors/acceleration.h"
|
|
||||||
|
|
||||||
#include "config/runtime_config.h"
|
|
||||||
#include "config/config.h"
|
|
||||||
|
|
||||||
extern gyro_t gyro;
|
|
||||||
|
|
||||||
uint32_t targetLooptime;
|
|
||||||
static uint8_t mpuDividerDrops;
|
static uint8_t mpuDividerDrops;
|
||||||
|
|
||||||
bool getMpuDataStatus(gyro_t *gyro)
|
bool gyroSyncCheckUpdate(const gyro_t *gyro)
|
||||||
{
|
{
|
||||||
bool mpuDataStatus;
|
|
||||||
if (!gyro->intStatus)
|
if (!gyro->intStatus)
|
||||||
return false;
|
return false;
|
||||||
gyro->intStatus(&mpuDataStatus);
|
return gyro->intStatus();
|
||||||
return mpuDataStatus;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
bool gyroSyncCheckUpdate(void) {
|
#define GYRO_LPF_256HZ 0
|
||||||
return getMpuDataStatus(&gyro);
|
#define GYRO_LPF_188HZ 1
|
||||||
}
|
#define GYRO_LPF_98HZ 2
|
||||||
|
#define GYRO_LPF_42HZ 3
|
||||||
|
#define GYRO_LPF_20HZ 4
|
||||||
|
#define GYRO_LPF_10HZ 5
|
||||||
|
#define GYRO_LPF_5HZ 6
|
||||||
|
#define GYRO_LPF_NONE 7
|
||||||
|
|
||||||
void gyroUpdateSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator) {
|
uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator)
|
||||||
|
{
|
||||||
int gyroSamplePeriod;
|
int gyroSamplePeriod;
|
||||||
|
|
||||||
if (!lpf || lpf == 7) {
|
if (lpf == GYRO_LPF_256HZ || lpf == GYRO_LPF_NONE) {
|
||||||
gyroSamplePeriod = 125;
|
gyroSamplePeriod = 125;
|
||||||
} else {
|
} else {
|
||||||
gyroSamplePeriod = 1000;
|
gyroSamplePeriod = 1000;
|
||||||
|
@ -54,9 +46,11 @@ void gyroUpdateSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator) {
|
||||||
|
|
||||||
// calculate gyro divider and targetLooptime (expected cycleTime)
|
// calculate gyro divider and targetLooptime (expected cycleTime)
|
||||||
mpuDividerDrops = gyroSyncDenominator - 1;
|
mpuDividerDrops = gyroSyncDenominator - 1;
|
||||||
targetLooptime = (mpuDividerDrops + 1) * gyroSamplePeriod;
|
const uint32_t targetLooptime = gyroSyncDenominator * gyroSamplePeriod;
|
||||||
|
return targetLooptime;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint8_t gyroMPU6xxxGetDividerDrops(void) {
|
uint8_t gyroMPU6xxxGetDividerDrops(void)
|
||||||
|
{
|
||||||
return mpuDividerDrops;
|
return mpuDividerDrops;
|
||||||
}
|
}
|
||||||
|
|
|
@ -5,8 +5,7 @@
|
||||||
* Author: borisb
|
* Author: borisb
|
||||||
*/
|
*/
|
||||||
|
|
||||||
extern uint32_t targetLooptime;
|
struct gyro_s;
|
||||||
|
bool gyroSyncCheckUpdate(const struct gyro_s *gyro);
|
||||||
bool gyroSyncCheckUpdate(void);
|
|
||||||
uint8_t gyroMPU6xxxGetDividerDrops(void);
|
uint8_t gyroMPU6xxxGetDividerDrops(void);
|
||||||
void gyroUpdateSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator);
|
uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator);
|
||||||
|
|
|
@ -31,15 +31,15 @@ static const IO_t pin = DEFIO_IO(INVERTER);
|
||||||
|
|
||||||
void initInverter(void)
|
void initInverter(void)
|
||||||
{
|
{
|
||||||
IOInit(pin, OWNER_SYSTEM, RESOURCE_OUTPUT);
|
IOInit(pin, OWNER_SYSTEM, RESOURCE_OUTPUT);
|
||||||
IOConfigGPIO(pin, IOCFG_OUT_PP);
|
IOConfigGPIO(pin, IOCFG_OUT_PP);
|
||||||
|
|
||||||
inverterSet(false);
|
inverterSet(false);
|
||||||
}
|
}
|
||||||
|
|
||||||
void inverterSet(bool on)
|
void inverterSet(bool on)
|
||||||
{
|
{
|
||||||
IOWrite(pin, on);
|
IOWrite(pin, on);
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -306,7 +306,7 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
|
||||||
|
|
||||||
if (type == MAP_TO_PPM_INPUT) {
|
if (type == MAP_TO_PPM_INPUT) {
|
||||||
#if defined(SPARKY) || defined(ALIENFLIGHTF3)
|
#if defined(SPARKY) || defined(ALIENFLIGHTF3)
|
||||||
if (init->useFastPwm || init->pwmProtocolType == PWM_TYPE_BRUSHED) {
|
if (init->useFastPwm || init->pwmProtocolType == PWM_TYPE_BRUSHED) {
|
||||||
ppmAvoidPWMTimerClash(timerHardwarePtr, TIM2);
|
ppmAvoidPWMTimerClash(timerHardwarePtr, TIM2);
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -78,19 +78,19 @@ typedef struct drv_pwm_config_s {
|
||||||
} drv_pwm_config_t;
|
} drv_pwm_config_t;
|
||||||
|
|
||||||
enum {
|
enum {
|
||||||
MAP_TO_PPM_INPUT = 1,
|
MAP_TO_PPM_INPUT = 1,
|
||||||
MAP_TO_PWM_INPUT,
|
MAP_TO_PWM_INPUT,
|
||||||
MAP_TO_MOTOR_OUTPUT,
|
MAP_TO_MOTOR_OUTPUT,
|
||||||
MAP_TO_SERVO_OUTPUT,
|
MAP_TO_SERVO_OUTPUT,
|
||||||
};
|
};
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
PWM_PF_NONE = 0,
|
PWM_PF_NONE = 0,
|
||||||
PWM_PF_MOTOR = (1 << 0),
|
PWM_PF_MOTOR = (1 << 0),
|
||||||
PWM_PF_SERVO = (1 << 1),
|
PWM_PF_SERVO = (1 << 1),
|
||||||
PWM_PF_MOTOR_MODE_BRUSHED = (1 << 2),
|
PWM_PF_MOTOR_MODE_BRUSHED = (1 << 2),
|
||||||
PWM_PF_OUTPUT_PROTOCOL_PWM = (1 << 3),
|
PWM_PF_OUTPUT_PROTOCOL_PWM = (1 << 3),
|
||||||
PWM_PF_OUTPUT_PROTOCOL_ONESHOT = (1 << 4)
|
PWM_PF_OUTPUT_PROTOCOL_ONESHOT = (1 << 4)
|
||||||
} pwmPortFlags_e;
|
} pwmPortFlags_e;
|
||||||
|
|
||||||
enum {PWM_INVERTED = 1};
|
enum {PWM_INVERTED = 1};
|
||||||
|
|
|
@ -339,8 +339,8 @@ static void pwmEdgeCallback(timerCCHandlerRec_t *cbRec, captureCompare_t capture
|
||||||
|
|
||||||
static void pwmGPIOConfig(ioTag_t pin, ioConfig_t mode)
|
static void pwmGPIOConfig(ioTag_t pin, ioConfig_t mode)
|
||||||
{
|
{
|
||||||
IOInit(IOGetByTag(pin), OWNER_PWMINPUT, RESOURCE_INPUT);
|
IOInit(IOGetByTag(pin), OWNER_PWMINPUT, RESOURCE_INPUT);
|
||||||
IOConfigGPIO(IOGetByTag(pin), mode);
|
IOConfigGPIO(IOGetByTag(pin), mode);
|
||||||
}
|
}
|
||||||
|
|
||||||
void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
|
void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
|
||||||
|
|
|
@ -22,4 +22,4 @@ typedef void (*sensorInitFuncPtr)(void); // sensor init proto
|
||||||
typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
|
typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
|
||||||
typedef void (*sensorAccInitFuncPtr)(struct acc_s *acc); // sensor init prototype
|
typedef void (*sensorAccInitFuncPtr)(struct acc_s *acc); // sensor init prototype
|
||||||
typedef void (*sensorGyroInitFuncPtr)(uint8_t lpf); // gyro sensor init prototype
|
typedef void (*sensorGyroInitFuncPtr)(uint8_t lpf); // gyro sensor init prototype
|
||||||
typedef void (*sensorInterruptFuncPtr)(bool *data); // sensor Interrupt Data Ready
|
typedef bool (*sensorInterruptFuncPtr)(void); // sensor Interrupt Data Ready
|
||||||
|
|
|
@ -96,7 +96,7 @@ void setTxSignal(softSerial_t *softSerial, uint8_t state)
|
||||||
if (state) {
|
if (state) {
|
||||||
IOHi(softSerial->txIO);
|
IOHi(softSerial->txIO);
|
||||||
} else {
|
} else {
|
||||||
IOLo(softSerial->txIO);
|
IOLo(softSerial->txIO);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -40,13 +40,13 @@ typedef struct {
|
||||||
serialPort_t port;
|
serialPort_t port;
|
||||||
|
|
||||||
#ifdef STM32F4
|
#ifdef STM32F4
|
||||||
DMA_Stream_TypeDef *rxDMAStream;
|
DMA_Stream_TypeDef *rxDMAStream;
|
||||||
DMA_Stream_TypeDef *txDMAStream;
|
DMA_Stream_TypeDef *txDMAStream;
|
||||||
uint32_t rxDMAChannel;
|
uint32_t rxDMAChannel;
|
||||||
uint32_t txDMAChannel;
|
uint32_t txDMAChannel;
|
||||||
#else
|
#else
|
||||||
DMA_Channel_TypeDef *rxDMAChannel;
|
DMA_Channel_TypeDef *rxDMAChannel;
|
||||||
DMA_Channel_TypeDef *txDMAChannel;
|
DMA_Channel_TypeDef *txDMAChannel;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
uint32_t rxDMAIrq;
|
uint32_t rxDMAIrq;
|
||||||
|
|
|
@ -365,22 +365,22 @@ void DMA2_Stream7_IRQHandler(void)
|
||||||
uartPort_t *s = &(uartHardwareMap[UARTDEV_1]->port);
|
uartPort_t *s = &(uartHardwareMap[UARTDEV_1]->port);
|
||||||
if(DMA_GetITStatus(s->txDMAStream,DMA_IT_TCIF7))
|
if(DMA_GetITStatus(s->txDMAStream,DMA_IT_TCIF7))
|
||||||
{
|
{
|
||||||
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF7);
|
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF7);
|
||||||
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF7);
|
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF7);
|
||||||
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF7)==SET)
|
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF7)==SET)
|
||||||
{
|
{
|
||||||
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF7);
|
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF7);
|
||||||
}
|
}
|
||||||
handleUsartTxDma(s);
|
handleUsartTxDma(s);
|
||||||
|
}
|
||||||
|
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF7)==SET)
|
||||||
|
{
|
||||||
|
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF7);
|
||||||
|
}
|
||||||
|
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF7)==SET)
|
||||||
|
{
|
||||||
|
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF7);
|
||||||
}
|
}
|
||||||
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF7)==SET)
|
|
||||||
{
|
|
||||||
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF7);
|
|
||||||
}
|
|
||||||
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF7)==SET)
|
|
||||||
{
|
|
||||||
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF7);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// USART1 Rx/Tx IRQ Handler
|
// USART1 Rx/Tx IRQ Handler
|
||||||
|
@ -402,30 +402,30 @@ uartPort_t *serialUSART2(uint32_t baudRate, portMode_t mode, portOptions_t optio
|
||||||
void DMA1_Stream6_IRQHandler(void)
|
void DMA1_Stream6_IRQHandler(void)
|
||||||
{
|
{
|
||||||
uartPort_t *s = &(uartHardwareMap[UARTDEV_2]->port);
|
uartPort_t *s = &(uartHardwareMap[UARTDEV_2]->port);
|
||||||
if(DMA_GetITStatus(s->txDMAStream,DMA_IT_TCIF6))
|
if(DMA_GetITStatus(s->txDMAStream,DMA_IT_TCIF6))
|
||||||
{
|
{
|
||||||
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF6);
|
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF6);
|
||||||
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF6);
|
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF6);
|
||||||
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF6)==SET)
|
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF6)==SET)
|
||||||
{
|
{
|
||||||
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF6);
|
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF6);
|
||||||
}
|
}
|
||||||
handleUsartTxDma(s);
|
handleUsartTxDma(s);
|
||||||
}
|
}
|
||||||
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF6)==SET)
|
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF6)==SET)
|
||||||
{
|
{
|
||||||
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF6);
|
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF6);
|
||||||
}
|
}
|
||||||
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF6)==SET)
|
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF6)==SET)
|
||||||
{
|
{
|
||||||
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF6);
|
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF6);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void USART2_IRQHandler(void)
|
void USART2_IRQHandler(void)
|
||||||
{
|
{
|
||||||
uartPort_t *s = &(uartHardwareMap[UARTDEV_2]->port);
|
uartPort_t *s = &(uartHardwareMap[UARTDEV_2]->port);
|
||||||
usartIrqHandler(s);
|
usartIrqHandler(s);
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
@ -442,22 +442,22 @@ void DMA1_Stream3_IRQHandler(void)
|
||||||
uartPort_t *s = &(uartHardwareMap[UARTDEV_3]->port);
|
uartPort_t *s = &(uartHardwareMap[UARTDEV_3]->port);
|
||||||
if(DMA_GetITStatus(s->txDMAStream,DMA_IT_TCIF3))
|
if(DMA_GetITStatus(s->txDMAStream,DMA_IT_TCIF3))
|
||||||
{
|
{
|
||||||
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF3);
|
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF3);
|
||||||
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF3);
|
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF3);
|
||||||
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF3)==SET)
|
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF3)==SET)
|
||||||
{
|
{
|
||||||
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF3);
|
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF3);
|
||||||
}
|
}
|
||||||
handleUsartTxDma(s);
|
handleUsartTxDma(s);
|
||||||
|
}
|
||||||
|
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF3)==SET)
|
||||||
|
{
|
||||||
|
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF3);
|
||||||
|
}
|
||||||
|
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF3)==SET)
|
||||||
|
{
|
||||||
|
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF3);
|
||||||
}
|
}
|
||||||
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF3)==SET)
|
|
||||||
{
|
|
||||||
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF3);
|
|
||||||
}
|
|
||||||
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF3)==SET)
|
|
||||||
{
|
|
||||||
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF3);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void USART3_IRQHandler(void)
|
void USART3_IRQHandler(void)
|
||||||
|
@ -480,22 +480,22 @@ void DMA1_Stream4_IRQHandler(void)
|
||||||
uartPort_t *s = &(uartHardwareMap[UARTDEV_4]->port);
|
uartPort_t *s = &(uartHardwareMap[UARTDEV_4]->port);
|
||||||
if(DMA_GetITStatus(s->txDMAStream,DMA_IT_TCIF4))
|
if(DMA_GetITStatus(s->txDMAStream,DMA_IT_TCIF4))
|
||||||
{
|
{
|
||||||
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF4);
|
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF4);
|
||||||
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF4);
|
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF4);
|
||||||
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF4)==SET)
|
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF4)==SET)
|
||||||
{
|
{
|
||||||
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF4);
|
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF4);
|
||||||
}
|
}
|
||||||
handleUsartTxDma(s);
|
handleUsartTxDma(s);
|
||||||
|
}
|
||||||
|
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF4)==SET)
|
||||||
|
{
|
||||||
|
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF4);
|
||||||
|
}
|
||||||
|
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF4)==SET)
|
||||||
|
{
|
||||||
|
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF4);
|
||||||
}
|
}
|
||||||
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF4)==SET)
|
|
||||||
{
|
|
||||||
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF4);
|
|
||||||
}
|
|
||||||
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF4)==SET)
|
|
||||||
{
|
|
||||||
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF4);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void UART4_IRQHandler(void)
|
void UART4_IRQHandler(void)
|
||||||
|
@ -518,22 +518,22 @@ void DMA1_Stream7_IRQHandler(void)
|
||||||
uartPort_t *s = &(uartHardwareMap[UARTDEV_5]->port);
|
uartPort_t *s = &(uartHardwareMap[UARTDEV_5]->port);
|
||||||
if(DMA_GetITStatus(s->txDMAStream,DMA_IT_TCIF7))
|
if(DMA_GetITStatus(s->txDMAStream,DMA_IT_TCIF7))
|
||||||
{
|
{
|
||||||
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF7);
|
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF7);
|
||||||
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF7);
|
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF7);
|
||||||
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF7)==SET)
|
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF7)==SET)
|
||||||
{
|
{
|
||||||
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF7);
|
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF7);
|
||||||
}
|
}
|
||||||
handleUsartTxDma(s);
|
handleUsartTxDma(s);
|
||||||
|
}
|
||||||
|
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF7)==SET)
|
||||||
|
{
|
||||||
|
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF7);
|
||||||
|
}
|
||||||
|
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF7)==SET)
|
||||||
|
{
|
||||||
|
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF7);
|
||||||
}
|
}
|
||||||
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF7)==SET)
|
|
||||||
{
|
|
||||||
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF7);
|
|
||||||
}
|
|
||||||
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF7)==SET)
|
|
||||||
{
|
|
||||||
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF7);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void UART5_IRQHandler(void)
|
void UART5_IRQHandler(void)
|
||||||
|
@ -556,22 +556,22 @@ void DMA2_Stream6_IRQHandler(void)
|
||||||
uartPort_t *s = &(uartHardwareMap[UARTDEV_6]->port);
|
uartPort_t *s = &(uartHardwareMap[UARTDEV_6]->port);
|
||||||
if(DMA_GetITStatus(s->txDMAStream,DMA_IT_TCIF6))
|
if(DMA_GetITStatus(s->txDMAStream,DMA_IT_TCIF6))
|
||||||
{
|
{
|
||||||
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF6);
|
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TCIF6);
|
||||||
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF6);
|
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_HTIF6);
|
||||||
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF6)==SET)
|
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_FEIF6)==SET)
|
||||||
{
|
{
|
||||||
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF6);
|
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_FEIF6);
|
||||||
}
|
}
|
||||||
handleUsartTxDma(s);
|
handleUsartTxDma(s);
|
||||||
|
}
|
||||||
|
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF6)==SET)
|
||||||
|
{
|
||||||
|
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF6);
|
||||||
|
}
|
||||||
|
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF6)==SET)
|
||||||
|
{
|
||||||
|
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF6);
|
||||||
}
|
}
|
||||||
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_TEIF6)==SET)
|
|
||||||
{
|
|
||||||
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_TEIF6);
|
|
||||||
}
|
|
||||||
if(DMA_GetFlagStatus(s->txDMAStream,DMA_IT_DMEIF6)==SET)
|
|
||||||
{
|
|
||||||
DMA_ClearITPendingBit(s->txDMAStream,DMA_IT_DMEIF6);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void USART6_IRQHandler(void)
|
void USART6_IRQHandler(void)
|
||||||
|
|
|
@ -183,12 +183,12 @@ serialPort_t *usbVcpOpen(void)
|
||||||
#ifdef STM32F4
|
#ifdef STM32F4
|
||||||
IOInit(IOGetByTag(IO_TAG(PA11)), OWNER_USB, RESOURCE_IO);
|
IOInit(IOGetByTag(IO_TAG(PA11)), OWNER_USB, RESOURCE_IO);
|
||||||
IOInit(IOGetByTag(IO_TAG(PA12)), OWNER_USB, RESOURCE_IO);
|
IOInit(IOGetByTag(IO_TAG(PA12)), OWNER_USB, RESOURCE_IO);
|
||||||
USBD_Init(&USB_OTG_dev, USB_OTG_FS_CORE_ID, &USR_desc, &USBD_CDC_cb, &USR_cb);
|
USBD_Init(&USB_OTG_dev, USB_OTG_FS_CORE_ID, &USR_desc, &USBD_CDC_cb, &USR_cb);
|
||||||
#else
|
#else
|
||||||
Set_System();
|
Set_System();
|
||||||
Set_USBClock();
|
Set_USBClock();
|
||||||
USB_Interrupts_Config();
|
USB_Interrupts_Config();
|
||||||
USB_Init();
|
USB_Init();
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
s = &vcpPort;
|
s = &vcpPort;
|
||||||
|
|
|
@ -92,9 +92,9 @@ void hcsr04_init(sonarRange_t *sonarRange)
|
||||||
IOConfigGPIO(echoIO, IOCFG_IN_FLOATING);
|
IOConfigGPIO(echoIO, IOCFG_IN_FLOATING);
|
||||||
|
|
||||||
#ifdef USE_EXTI
|
#ifdef USE_EXTI
|
||||||
EXTIHandlerInit(&hcsr04_extiCallbackRec, hcsr04_extiHandler);
|
EXTIHandlerInit(&hcsr04_extiCallbackRec, hcsr04_extiHandler);
|
||||||
EXTIConfig(echoIO, &hcsr04_extiCallbackRec, NVIC_PRIO_SONAR_EXTI, EXTI_Trigger_Rising_Falling); // TODO - priority!
|
EXTIConfig(echoIO, &hcsr04_extiCallbackRec, NVIC_PRIO_SONAR_EXTI, EXTI_Trigger_Rising_Falling); // TODO - priority!
|
||||||
EXTIEnable(echoIO, true);
|
EXTIEnable(echoIO, true);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
lastMeasurementAt = millis() - 60; // force 1st measurement in hcsr04_get_distance()
|
lastMeasurementAt = millis() - 60; // force 1st measurement in hcsr04_get_distance()
|
||||||
|
|
|
@ -39,31 +39,31 @@ static bool beeperInverted = false;
|
||||||
void systemBeep(bool onoff)
|
void systemBeep(bool onoff)
|
||||||
{
|
{
|
||||||
#ifndef BEEPER
|
#ifndef BEEPER
|
||||||
UNUSED(onoff);
|
UNUSED(onoff);
|
||||||
#else
|
#else
|
||||||
IOWrite(beeperIO, beeperInverted ? onoff : !onoff);
|
IOWrite(beeperIO, beeperInverted ? onoff : !onoff);
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
void systemBeepToggle(void)
|
void systemBeepToggle(void)
|
||||||
{
|
{
|
||||||
#ifdef BEEPER
|
#ifdef BEEPER
|
||||||
IOToggle(beeperIO);
|
IOToggle(beeperIO);
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
void beeperInit(const beeperConfig_t *config)
|
void beeperInit(const beeperConfig_t *config)
|
||||||
{
|
{
|
||||||
#ifndef BEEPER
|
#ifndef BEEPER
|
||||||
UNUSED(config);
|
UNUSED(config);
|
||||||
#else
|
#else
|
||||||
beeperIO = IOGetByTag(config->ioTag);
|
beeperIO = IOGetByTag(config->ioTag);
|
||||||
beeperInverted = config->isInverted;
|
beeperInverted = config->isInverted;
|
||||||
|
|
||||||
if (beeperIO) {
|
if (beeperIO) {
|
||||||
IOInit(beeperIO, OWNER_BEEPER, RESOURCE_OUTPUT);
|
IOInit(beeperIO, OWNER_BEEPER, RESOURCE_OUTPUT);
|
||||||
IOConfigGPIO(beeperIO, config->isOD ? IOCFG_OUT_OD : IOCFG_OUT_PP);
|
IOConfigGPIO(beeperIO, config->isOD ? IOCFG_OUT_OD : IOCFG_OUT_PP);
|
||||||
}
|
}
|
||||||
systemBeep(false);
|
systemBeep(false);
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
|
@ -30,9 +30,9 @@
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
typedef struct beeperConfig_s {
|
typedef struct beeperConfig_s {
|
||||||
ioTag_t ioTag;
|
ioTag_t ioTag;
|
||||||
unsigned isInverted : 1;
|
unsigned isInverted : 1;
|
||||||
unsigned isOD : 1;
|
unsigned isOD : 1;
|
||||||
} beeperConfig_t;
|
} beeperConfig_t;
|
||||||
|
|
||||||
void systemBeep(bool on);
|
void systemBeep(bool on);
|
||||||
|
|
|
@ -25,13 +25,13 @@ uint32_t micros(void);
|
||||||
uint32_t millis(void);
|
uint32_t millis(void);
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
FAILURE_DEVELOPER = 0,
|
FAILURE_DEVELOPER = 0,
|
||||||
FAILURE_MISSING_ACC,
|
FAILURE_MISSING_ACC,
|
||||||
FAILURE_ACC_INIT,
|
FAILURE_ACC_INIT,
|
||||||
FAILURE_ACC_INCOMPATIBLE,
|
FAILURE_ACC_INCOMPATIBLE,
|
||||||
FAILURE_INVALID_EEPROM_CONTENTS,
|
FAILURE_INVALID_EEPROM_CONTENTS,
|
||||||
FAILURE_FLASH_WRITE_FAILED,
|
FAILURE_FLASH_WRITE_FAILED,
|
||||||
FAILURE_GYRO_INIT_FAILED
|
FAILURE_GYRO_INIT_FAILED
|
||||||
} failureMode_e;
|
} failureMode_e;
|
||||||
|
|
||||||
// failure
|
// failure
|
||||||
|
|
|
@ -45,8 +45,8 @@ void systemReset(void)
|
||||||
if (mpuConfiguration.reset)
|
if (mpuConfiguration.reset)
|
||||||
mpuConfiguration.reset();
|
mpuConfiguration.reset();
|
||||||
|
|
||||||
__disable_irq();
|
__disable_irq();
|
||||||
NVIC_SystemReset();
|
NVIC_SystemReset();
|
||||||
}
|
}
|
||||||
|
|
||||||
void systemResetToBootloader(void)
|
void systemResetToBootloader(void)
|
||||||
|
@ -54,10 +54,10 @@ void systemResetToBootloader(void)
|
||||||
if (mpuConfiguration.reset)
|
if (mpuConfiguration.reset)
|
||||||
mpuConfiguration.reset();
|
mpuConfiguration.reset();
|
||||||
|
|
||||||
*((uint32_t *)0x2001FFFC) = 0xDEADBEEF; // 128KB SRAM STM32F4XX
|
*((uint32_t *)0x2001FFFC) = 0xDEADBEEF; // 128KB SRAM STM32F4XX
|
||||||
|
|
||||||
__disable_irq();
|
__disable_irq();
|
||||||
NVIC_SystemReset();
|
NVIC_SystemReset();
|
||||||
}
|
}
|
||||||
|
|
||||||
void enableGPIOPowerUsageAndNoiseReductions(void)
|
void enableGPIOPowerUsageAndNoiseReductions(void)
|
||||||
|
@ -82,7 +82,7 @@ void enableGPIOPowerUsageAndNoiseReductions(void)
|
||||||
RCC_AHB1Periph_BKPSRAM |
|
RCC_AHB1Periph_BKPSRAM |
|
||||||
RCC_AHB1Periph_DMA1 |
|
RCC_AHB1Periph_DMA1 |
|
||||||
RCC_AHB1Periph_DMA2 |
|
RCC_AHB1Periph_DMA2 |
|
||||||
0, ENABLE
|
0, ENABLE
|
||||||
);
|
);
|
||||||
|
|
||||||
RCC_AHB2PeriphClockCmd(0, ENABLE);
|
RCC_AHB2PeriphClockCmd(0, ENABLE);
|
||||||
|
@ -172,25 +172,25 @@ void systemInit(void)
|
||||||
SetSysClock();
|
SetSysClock();
|
||||||
|
|
||||||
// Configure NVIC preempt/priority groups
|
// Configure NVIC preempt/priority groups
|
||||||
NVIC_PriorityGroupConfig(NVIC_PRIORITY_GROUPING);
|
NVIC_PriorityGroupConfig(NVIC_PRIORITY_GROUPING);
|
||||||
|
|
||||||
// cache RCC->CSR value to use it in isMPUSoftreset() and others
|
// cache RCC->CSR value to use it in isMPUSoftreset() and others
|
||||||
cachedRccCsrValue = RCC->CSR;
|
cachedRccCsrValue = RCC->CSR;
|
||||||
|
|
||||||
/* Accounts for OP Bootloader, set the Vector Table base address as specified in .ld file */
|
/* Accounts for OP Bootloader, set the Vector Table base address as specified in .ld file */
|
||||||
extern void *isr_vector_table_base;
|
extern void *isr_vector_table_base;
|
||||||
NVIC_SetVectorTable((uint32_t)&isr_vector_table_base, 0x0);
|
NVIC_SetVectorTable((uint32_t)&isr_vector_table_base, 0x0);
|
||||||
RCC_AHB2PeriphClockCmd(RCC_AHB2Periph_OTG_FS, DISABLE);
|
RCC_AHB2PeriphClockCmd(RCC_AHB2Periph_OTG_FS, DISABLE);
|
||||||
|
|
||||||
RCC_ClearFlag();
|
RCC_ClearFlag();
|
||||||
|
|
||||||
enableGPIOPowerUsageAndNoiseReductions();
|
enableGPIOPowerUsageAndNoiseReductions();
|
||||||
|
|
||||||
// Init cycle counter
|
// Init cycle counter
|
||||||
cycleCounterInit();
|
cycleCounterInit();
|
||||||
|
|
||||||
memset(extiHandlerConfigs, 0x00, sizeof(extiHandlerConfigs));
|
memset(extiHandlerConfigs, 0x00, sizeof(extiHandlerConfigs));
|
||||||
// SysTick
|
// SysTick
|
||||||
SysTick_Config(SystemCoreClock / 1000);
|
SysTick_Config(SystemCoreClock / 1000);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -17,7 +17,6 @@
|
||||||
|
|
||||||
#include <stdbool.h>
|
#include <stdbool.h>
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
#include <stdlib.h>
|
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
|
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
|
@ -26,7 +25,6 @@
|
||||||
|
|
||||||
#include "nvic.h"
|
#include "nvic.h"
|
||||||
|
|
||||||
#include "gpio.h"
|
|
||||||
#include "gpio.h"
|
#include "gpio.h"
|
||||||
#include "rcc.h"
|
#include "rcc.h"
|
||||||
#include "system.h"
|
#include "system.h"
|
||||||
|
@ -148,7 +146,7 @@ rccPeriphTag_t timerRCC(TIM_TypeDef *tim)
|
||||||
for (uint8_t i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) {
|
for (uint8_t i = 0; i < HARDWARE_TIMER_DEFINITION_COUNT; i++) {
|
||||||
if (timerDefinitions[i].TIMx == tim) {
|
if (timerDefinitions[i].TIMx == tim) {
|
||||||
return timerDefinitions[i].rcc;
|
return timerDefinitions[i].rcc;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
@ -190,7 +188,7 @@ void configTimeBase(TIM_TypeDef *tim, uint16_t period, uint8_t mhz)
|
||||||
#else
|
#else
|
||||||
TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / ((uint32_t)mhz * 1000000)) - 1;
|
TIM_TimeBaseStructure.TIM_Prescaler = (SystemCoreClock / ((uint32_t)mhz * 1000000)) - 1;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
|
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
|
||||||
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
|
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
|
||||||
TIM_TimeBaseInit(tim, &TIM_TimeBaseStructure);
|
TIM_TimeBaseInit(tim, &TIM_TimeBaseStructure);
|
||||||
|
@ -660,7 +658,7 @@ void timerInit(void)
|
||||||
IOConfigGPIOAF(IOGetByTag(timerHardwarePtr->tag), timerHardwarePtr->ioMode, timerHardwarePtr->alternateFunction);
|
IOConfigGPIOAF(IOGetByTag(timerHardwarePtr->tag), timerHardwarePtr->ioMode, timerHardwarePtr->alternateFunction);
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// initialize timer channel structures
|
// initialize timer channel structures
|
||||||
for(int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
|
for(int i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
|
||||||
timerChannelInfo[i].type = TYPE_FREE;
|
timerChannelInfo[i].type = TYPE_FREE;
|
||||||
|
|
|
@ -36,7 +36,6 @@
|
||||||
#include "drivers/sensor.h"
|
#include "drivers/sensor.h"
|
||||||
#include "drivers/accgyro.h"
|
#include "drivers/accgyro.h"
|
||||||
#include "drivers/system.h"
|
#include "drivers/system.h"
|
||||||
#include "drivers/gyro_sync.h"
|
|
||||||
|
|
||||||
#include "rx/rx.h"
|
#include "rx/rx.h"
|
||||||
|
|
||||||
|
|
|
@ -29,7 +29,6 @@
|
||||||
#include "common/filter.h"
|
#include "common/filter.h"
|
||||||
|
|
||||||
#include "drivers/sensor.h"
|
#include "drivers/sensor.h"
|
||||||
#include "drivers/gyro_sync.h"
|
|
||||||
|
|
||||||
#include "drivers/accgyro.h"
|
#include "drivers/accgyro.h"
|
||||||
#include "sensors/sensors.h"
|
#include "sensors/sensors.h"
|
||||||
|
@ -74,7 +73,7 @@ pidControllerFuncPtr pid_controller = pidInteger; // which pid controller are we
|
||||||
|
|
||||||
void setTargetPidLooptime(uint8_t pidProcessDenom)
|
void setTargetPidLooptime(uint8_t pidProcessDenom)
|
||||||
{
|
{
|
||||||
targetPidLooptime = targetLooptime * pidProcessDenom;
|
targetPidLooptime = gyro.targetLooptime * pidProcessDenom;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint16_t getDynamicKi(int axis, const pidProfile_t *pidProfile, int32_t angleRate)
|
uint16_t getDynamicKi(int axis, const pidProfile_t *pidProfile, int32_t angleRate)
|
||||||
|
|
|
@ -52,7 +52,6 @@
|
||||||
#include "drivers/inverter.h"
|
#include "drivers/inverter.h"
|
||||||
#include "drivers/flash_m25p16.h"
|
#include "drivers/flash_m25p16.h"
|
||||||
#include "drivers/sonar_hcsr04.h"
|
#include "drivers/sonar_hcsr04.h"
|
||||||
#include "drivers/gyro_sync.h"
|
|
||||||
#include "drivers/usb_io.h"
|
#include "drivers/usb_io.h"
|
||||||
#include "drivers/transponder_ir.h"
|
#include "drivers/transponder_ir.h"
|
||||||
#include "drivers/sdcard.h"
|
#include "drivers/sdcard.h"
|
||||||
|
|
|
@ -34,7 +34,6 @@
|
||||||
#include "drivers/system.h"
|
#include "drivers/system.h"
|
||||||
#include "drivers/sensor.h"
|
#include "drivers/sensor.h"
|
||||||
#include "drivers/accgyro.h"
|
#include "drivers/accgyro.h"
|
||||||
#include "drivers/gyro_sync.h"
|
|
||||||
|
|
||||||
#include "sensors/barometer.h"
|
#include "sensors/barometer.h"
|
||||||
#include "sensors/battery.h"
|
#include "sensors/battery.h"
|
||||||
|
@ -210,7 +209,7 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat
|
||||||
|
|
||||||
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
|
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
|
||||||
// GYRO calibration
|
// GYRO calibration
|
||||||
gyroSetCalibrationCycles(calculateCalibratingCycles());
|
gyroSetCalibrationCycles();
|
||||||
|
|
||||||
#ifdef GPS
|
#ifdef GPS
|
||||||
if (feature(FEATURE_GPS)) {
|
if (feature(FEATURE_GPS)) {
|
||||||
|
|
|
@ -48,7 +48,6 @@
|
||||||
#include "drivers/timer.h"
|
#include "drivers/timer.h"
|
||||||
#include "drivers/pwm_rx.h"
|
#include "drivers/pwm_rx.h"
|
||||||
#include "drivers/sdcard.h"
|
#include "drivers/sdcard.h"
|
||||||
#include "drivers/gyro_sync.h"
|
|
||||||
|
|
||||||
#include "drivers/buf_writer.h"
|
#include "drivers/buf_writer.h"
|
||||||
|
|
||||||
|
|
|
@ -40,7 +40,6 @@
|
||||||
#include "drivers/gpio.h"
|
#include "drivers/gpio.h"
|
||||||
#include "drivers/timer.h"
|
#include "drivers/timer.h"
|
||||||
#include "drivers/pwm_rx.h"
|
#include "drivers/pwm_rx.h"
|
||||||
#include "drivers/gyro_sync.h"
|
|
||||||
#include "drivers/sdcard.h"
|
#include "drivers/sdcard.h"
|
||||||
#include "drivers/buf_writer.h"
|
#include "drivers/buf_writer.h"
|
||||||
#include "drivers/max7456.h"
|
#include "drivers/max7456.h"
|
||||||
|
@ -109,8 +108,8 @@ void setGyroSamplingSpeed(uint16_t looptime) {
|
||||||
uint16_t gyroSampleRate = 1000;
|
uint16_t gyroSampleRate = 1000;
|
||||||
uint8_t maxDivider = 1;
|
uint8_t maxDivider = 1;
|
||||||
|
|
||||||
if (looptime != targetLooptime || looptime == 0) {
|
if (looptime != gyro.targetLooptime || looptime == 0) {
|
||||||
if (looptime == 0) looptime = targetLooptime; // needed for pid controller changes
|
if (looptime == 0) looptime = gyro.targetLooptime; // needed for pid controller changes
|
||||||
#ifdef STM32F303xC
|
#ifdef STM32F303xC
|
||||||
if (looptime < 1000) {
|
if (looptime < 1000) {
|
||||||
masterConfig.gyro_lpf = 0;
|
masterConfig.gyro_lpf = 0;
|
||||||
|
@ -854,7 +853,7 @@ static bool processOutCommand(uint8_t cmdMSP)
|
||||||
break;
|
break;
|
||||||
case MSP_LOOP_TIME:
|
case MSP_LOOP_TIME:
|
||||||
headSerialReply(2);
|
headSerialReply(2);
|
||||||
serialize16((uint16_t)targetLooptime);
|
serialize16((uint16_t)gyro.targetLooptime);
|
||||||
break;
|
break;
|
||||||
case MSP_RC_TUNING:
|
case MSP_RC_TUNING:
|
||||||
headSerialReply(11);
|
headSerialReply(11);
|
||||||
|
|
|
@ -17,11 +17,11 @@
|
||||||
|
|
||||||
#include <stdbool.h>
|
#include <stdbool.h>
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
#include <stdlib.h>
|
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
|
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
|
|
||||||
#include "common/axis.h"
|
#include "common/axis.h"
|
||||||
#include "common/color.h"
|
#include "common/color.h"
|
||||||
#include "common/maths.h"
|
#include "common/maths.h"
|
||||||
|
@ -49,7 +49,6 @@
|
||||||
#include "drivers/inverter.h"
|
#include "drivers/inverter.h"
|
||||||
#include "drivers/flash_m25p16.h"
|
#include "drivers/flash_m25p16.h"
|
||||||
#include "drivers/sonar_hcsr04.h"
|
#include "drivers/sonar_hcsr04.h"
|
||||||
#include "drivers/gyro_sync.h"
|
|
||||||
#include "drivers/sdcard.h"
|
#include "drivers/sdcard.h"
|
||||||
#include "drivers/usb_io.h"
|
#include "drivers/usb_io.h"
|
||||||
#include "drivers/transponder_ir.h"
|
#include "drivers/transponder_ir.h"
|
||||||
|
@ -153,9 +152,6 @@ static uint8_t systemState = SYSTEM_STATE_INITIALISING;
|
||||||
|
|
||||||
void init(void)
|
void init(void)
|
||||||
{
|
{
|
||||||
uint8_t i;
|
|
||||||
drv_pwm_config_t pwm_params;
|
|
||||||
|
|
||||||
printfSupportInit();
|
printfSupportInit();
|
||||||
|
|
||||||
initEEPROM();
|
initEEPROM();
|
||||||
|
@ -260,6 +256,7 @@ void init(void)
|
||||||
mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer);
|
mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
drv_pwm_config_t pwm_params;
|
||||||
memset(&pwm_params, 0, sizeof(pwm_params));
|
memset(&pwm_params, 0, sizeof(pwm_params));
|
||||||
|
|
||||||
#ifdef SONAR
|
#ifdef SONAR
|
||||||
|
@ -333,6 +330,7 @@ void init(void)
|
||||||
#endif
|
#endif
|
||||||
pwmRxInit(masterConfig.inputFilteringMode);
|
pwmRxInit(masterConfig.inputFilteringMode);
|
||||||
|
|
||||||
|
// pwmInit() needs to be called as soon as possible for ESC compatibility reasons
|
||||||
pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params);
|
pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params);
|
||||||
|
|
||||||
mixerUsePWMOutputConfiguration(pwmOutputConfiguration, use_unsyncedPwm);
|
mixerUsePWMOutputConfiguration(pwmOutputConfiguration, use_unsyncedPwm);
|
||||||
|
@ -488,12 +486,12 @@ void init(void)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,
|
if (!sensorsAutodetect(&masterConfig.sensorAlignmentConfig,
|
||||||
masterConfig.acc_hardware,
|
masterConfig.acc_hardware,
|
||||||
masterConfig.mag_hardware,
|
masterConfig.mag_hardware,
|
||||||
masterConfig.baro_hardware,
|
masterConfig.baro_hardware,
|
||||||
masterConfig.mag_declination,
|
masterConfig.mag_declination,
|
||||||
masterConfig.gyro_lpf,
|
masterConfig.gyro_lpf,
|
||||||
masterConfig.gyro_sync_denom)) {
|
masterConfig.gyro_sync_denom)) {
|
||||||
// if gyro was not detected due to whatever reason, we give up now.
|
// if gyro was not detected due to whatever reason, we give up now.
|
||||||
failureMode(FAILURE_MISSING_ACC);
|
failureMode(FAILURE_MISSING_ACC);
|
||||||
}
|
}
|
||||||
|
@ -504,7 +502,7 @@ void init(void)
|
||||||
LED0_OFF;
|
LED0_OFF;
|
||||||
LED2_OFF;
|
LED2_OFF;
|
||||||
|
|
||||||
for (i = 0; i < 10; i++) {
|
for (int i = 0; i < 10; i++) {
|
||||||
LED1_TOGGLE;
|
LED1_TOGGLE;
|
||||||
LED0_TOGGLE;
|
LED0_TOGGLE;
|
||||||
delay(25);
|
delay(25);
|
||||||
|
@ -626,7 +624,7 @@ void init(void)
|
||||||
if (masterConfig.mixerMode == MIXER_GIMBAL) {
|
if (masterConfig.mixerMode == MIXER_GIMBAL) {
|
||||||
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
|
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
|
||||||
}
|
}
|
||||||
gyroSetCalibrationCycles(calculateCalibratingCycles());
|
gyroSetCalibrationCycles();
|
||||||
#ifdef BARO
|
#ifdef BARO
|
||||||
baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
|
baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
|
||||||
#endif
|
#endif
|
||||||
|
@ -688,18 +686,18 @@ void processLoopback(void) {
|
||||||
#define processLoopback()
|
#define processLoopback()
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
void main_init(void)
|
void main_init(void)
|
||||||
{
|
{
|
||||||
init();
|
init();
|
||||||
|
|
||||||
/* Setup scheduler */
|
/* Setup scheduler */
|
||||||
schedulerInit();
|
schedulerInit();
|
||||||
rescheduleTask(TASK_GYROPID, targetLooptime);
|
rescheduleTask(TASK_GYROPID, gyro.targetLooptime);
|
||||||
setTaskEnabled(TASK_GYROPID, true);
|
setTaskEnabled(TASK_GYROPID, true);
|
||||||
|
|
||||||
if(sensors(SENSOR_ACC)) {
|
if(sensors(SENSOR_ACC)) {
|
||||||
setTaskEnabled(TASK_ACCEL, true);
|
setTaskEnabled(TASK_ACCEL, true);
|
||||||
switch(targetLooptime) { // Switch statement kept in place to change acc rates in the future
|
switch(gyro.targetLooptime) { // Switch statement kept in place to change acc rates in the future
|
||||||
case(500):
|
case(500):
|
||||||
case(375):
|
case(375):
|
||||||
case(250):
|
case(250):
|
||||||
|
|
|
@ -185,7 +185,7 @@ float calculateRate(int axis, int16_t rc) {
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
return constrainf(angleRate, -8190.0f, 8190.0f); // Rate limit protection
|
return constrainf(angleRate, -8190.0f, 8190.0f); // Rate limit protection
|
||||||
}
|
}
|
||||||
|
|
||||||
void processRcCommand(void)
|
void processRcCommand(void)
|
||||||
|
@ -377,7 +377,7 @@ void mwArm(void)
|
||||||
static bool firstArmingCalibrationWasCompleted;
|
static bool firstArmingCalibrationWasCompleted;
|
||||||
|
|
||||||
if (masterConfig.gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) {
|
if (masterConfig.gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) {
|
||||||
gyroSetCalibrationCycles(calculateCalibratingCycles());
|
gyroSetCalibrationCycles();
|
||||||
armingCalibrationWasInitialised = true;
|
armingCalibrationWasInitialised = true;
|
||||||
firstArmingCalibrationWasCompleted = true;
|
firstArmingCalibrationWasCompleted = true;
|
||||||
}
|
}
|
||||||
|
@ -778,7 +778,7 @@ void subTaskMotorUpdate(void)
|
||||||
|
|
||||||
uint8_t setPidUpdateCountDown(void) {
|
uint8_t setPidUpdateCountDown(void) {
|
||||||
if (masterConfig.gyro_soft_lpf_hz) {
|
if (masterConfig.gyro_soft_lpf_hz) {
|
||||||
return masterConfig.pid_process_denom - 1;
|
return masterConfig.pid_process_denom - 1;
|
||||||
} else {
|
} else {
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
@ -802,7 +802,7 @@ void taskMainPidLoopCheck(void)
|
||||||
|
|
||||||
const uint32_t startTime = micros();
|
const uint32_t startTime = micros();
|
||||||
while (true) {
|
while (true) {
|
||||||
if (gyroSyncCheckUpdate() || ((currentDeltaTime + (micros() - previousTime)) >= (targetLooptime + GYRO_WATCHDOG_DELAY))) {
|
if (gyroSyncCheckUpdate(&gyro) || ((currentDeltaTime + (micros() - previousTime)) >= (gyro.targetLooptime + GYRO_WATCHDOG_DELAY))) {
|
||||||
static uint8_t pidUpdateCountdown;
|
static uint8_t pidUpdateCountdown;
|
||||||
|
|
||||||
if (debugMode == DEBUG_PIDLOOP) {debug[0] = micros() - startTime;} // time spent busy waiting
|
if (debugMode == DEBUG_PIDLOOP) {debug[0] = micros() - startTime;} // time spent busy waiting
|
||||||
|
|
|
@ -41,7 +41,6 @@
|
||||||
#include "drivers/timer.h"
|
#include "drivers/timer.h"
|
||||||
#include "drivers/pwm_rx.h"
|
#include "drivers/pwm_rx.h"
|
||||||
#include "drivers/system.h"
|
#include "drivers/system.h"
|
||||||
#include "drivers/gyro_sync.h"
|
|
||||||
#include "rx/pwm.h"
|
#include "rx/pwm.h"
|
||||||
#include "rx/sbus.h"
|
#include "rx/sbus.h"
|
||||||
#include "rx/spektrum.h"
|
#include "rx/spektrum.h"
|
||||||
|
|
|
@ -28,7 +28,6 @@
|
||||||
#include "drivers/sensor.h"
|
#include "drivers/sensor.h"
|
||||||
#include "drivers/accgyro.h"
|
#include "drivers/accgyro.h"
|
||||||
#include "drivers/system.h"
|
#include "drivers/system.h"
|
||||||
#include "drivers/gyro_sync.h"
|
|
||||||
|
|
||||||
#include "sensors/battery.h"
|
#include "sensors/battery.h"
|
||||||
#include "sensors/sensors.h"
|
#include "sensors/sensors.h"
|
||||||
|
@ -45,7 +44,7 @@ acc_t acc; // acc access functions
|
||||||
sensor_align_e accAlign = 0;
|
sensor_align_e accAlign = 0;
|
||||||
uint32_t accTargetLooptime;
|
uint32_t accTargetLooptime;
|
||||||
|
|
||||||
uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
|
static uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
|
||||||
|
|
||||||
extern uint16_t InflightcalibratingA;
|
extern uint16_t InflightcalibratingA;
|
||||||
extern bool AccInflightCalibrationArmed;
|
extern bool AccInflightCalibrationArmed;
|
||||||
|
|
|
@ -20,6 +20,10 @@
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
|
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
|
|
||||||
|
int32_t BaroAlt = 0;
|
||||||
|
|
||||||
|
#ifdef BARO
|
||||||
#include "common/maths.h"
|
#include "common/maths.h"
|
||||||
|
|
||||||
#include "drivers/barometer.h"
|
#include "drivers/barometer.h"
|
||||||
|
@ -32,9 +36,6 @@ baro_t baro; // barometer access functions
|
||||||
uint16_t calibratingB = 0; // baro calibration = get new ground pressure value
|
uint16_t calibratingB = 0; // baro calibration = get new ground pressure value
|
||||||
int32_t baroPressure = 0;
|
int32_t baroPressure = 0;
|
||||||
int32_t baroTemperature = 0;
|
int32_t baroTemperature = 0;
|
||||||
int32_t BaroAlt = 0;
|
|
||||||
|
|
||||||
#ifdef BARO
|
|
||||||
|
|
||||||
static int32_t baroGroundAltitude = 0;
|
static int32_t baroGroundAltitude = 0;
|
||||||
static int32_t baroGroundPressure = 0;
|
static int32_t baroGroundPressure = 0;
|
||||||
|
|
|
@ -40,13 +40,14 @@
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
mag_t mag; // mag access functions
|
mag_t mag; // mag access functions
|
||||||
|
int32_t magADC[XYZ_AXIS_COUNT];
|
||||||
|
sensor_align_e magAlign = 0;
|
||||||
|
|
||||||
|
#ifdef MAG
|
||||||
|
|
||||||
extern uint32_t currentTime; // FIXME dependency on global variable, pass it in instead.
|
extern uint32_t currentTime; // FIXME dependency on global variable, pass it in instead.
|
||||||
|
|
||||||
int16_t magADCRaw[XYZ_AXIS_COUNT];
|
static int16_t magADCRaw[XYZ_AXIS_COUNT];
|
||||||
int32_t magADC[XYZ_AXIS_COUNT];
|
|
||||||
sensor_align_e magAlign = 0;
|
|
||||||
#ifdef MAG
|
|
||||||
static uint8_t magInit = 0;
|
static uint8_t magInit = 0;
|
||||||
|
|
||||||
void compassInit(void)
|
void compassInit(void)
|
||||||
|
|
|
@ -28,7 +28,6 @@
|
||||||
|
|
||||||
#include "drivers/sensor.h"
|
#include "drivers/sensor.h"
|
||||||
#include "drivers/accgyro.h"
|
#include "drivers/accgyro.h"
|
||||||
#include "drivers/gyro_sync.h"
|
|
||||||
#include "sensors/sensors.h"
|
#include "sensors/sensors.h"
|
||||||
#include "io/beeper.h"
|
#include "io/beeper.h"
|
||||||
#include "io/statusindicator.h"
|
#include "io/statusindicator.h"
|
||||||
|
@ -36,36 +35,31 @@
|
||||||
|
|
||||||
#include "sensors/gyro.h"
|
#include "sensors/gyro.h"
|
||||||
|
|
||||||
uint16_t calibratingG = 0;
|
|
||||||
int32_t gyroADC[XYZ_AXIS_COUNT];
|
|
||||||
float gyroADCf[XYZ_AXIS_COUNT];
|
|
||||||
int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
|
|
||||||
|
|
||||||
static gyroConfig_t *gyroConfig;
|
|
||||||
static biquad_t gyroFilterState[3];
|
|
||||||
static bool gyroFilterStateIsSet;
|
|
||||||
static float gyroLpfCutFreq;
|
|
||||||
|
|
||||||
gyro_t gyro; // gyro access functions
|
gyro_t gyro; // gyro access functions
|
||||||
sensor_align_e gyroAlign = 0;
|
sensor_align_e gyroAlign = 0;
|
||||||
|
|
||||||
void useGyroConfig(gyroConfig_t *gyroConfigToUse, float gyro_lpf_hz)
|
int32_t gyroADC[XYZ_AXIS_COUNT];
|
||||||
|
float gyroADCf[XYZ_AXIS_COUNT];
|
||||||
|
|
||||||
|
static int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
|
||||||
|
static const gyroConfig_t *gyroConfig;
|
||||||
|
static biquad_t gyroFilterState[XYZ_AXIS_COUNT];
|
||||||
|
static uint8_t gyroSoftLpfHz;
|
||||||
|
static uint16_t calibratingG = 0;
|
||||||
|
|
||||||
|
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz)
|
||||||
{
|
{
|
||||||
gyroConfig = gyroConfigToUse;
|
gyroConfig = gyroConfigToUse;
|
||||||
gyroLpfCutFreq = gyro_lpf_hz;
|
gyroSoftLpfHz = gyro_soft_lpf_hz;
|
||||||
}
|
}
|
||||||
|
|
||||||
void initGyroFilterCoefficients(void) {
|
void gyroInit(void)
|
||||||
int axis;
|
|
||||||
if (gyroLpfCutFreq && targetLooptime) { /* Initialisation needs to happen once samplingrate is known */
|
|
||||||
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(gyroLpfCutFreq, &gyroFilterState[axis], targetLooptime);
|
|
||||||
gyroFilterStateIsSet = true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
|
|
||||||
{
|
{
|
||||||
calibratingG = calibrationCyclesRequired;
|
if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known
|
||||||
|
for (int axis = 0; axis < 3; axis++) {
|
||||||
|
BiQuadNewLpf(gyroSoftLpfHz, &gyroFilterState[axis], gyro.targetLooptime);
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
bool isGyroCalibrationComplete(void)
|
bool isGyroCalibrationComplete(void)
|
||||||
|
@ -73,27 +67,32 @@ bool isGyroCalibrationComplete(void)
|
||||||
return calibratingG == 0;
|
return calibratingG == 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool isOnFinalGyroCalibrationCycle(void)
|
static bool isOnFinalGyroCalibrationCycle(void)
|
||||||
{
|
{
|
||||||
return calibratingG == 1;
|
return calibratingG == 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint16_t calculateCalibratingCycles(void) {
|
static uint16_t gyroCalculateCalibratingCycles(void)
|
||||||
return (CALIBRATING_GYRO_CYCLES / targetLooptime) * CALIBRATING_GYRO_CYCLES;
|
{
|
||||||
|
return (CALIBRATING_GYRO_CYCLES / gyro.targetLooptime) * CALIBRATING_GYRO_CYCLES;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool isOnFirstGyroCalibrationCycle(void)
|
static bool isOnFirstGyroCalibrationCycle(void)
|
||||||
{
|
{
|
||||||
return calibratingG == calculateCalibratingCycles();
|
return calibratingG == gyroCalculateCalibratingCycles();
|
||||||
|
}
|
||||||
|
|
||||||
|
void gyroSetCalibrationCycles(void)
|
||||||
|
{
|
||||||
|
calibratingG = gyroCalculateCalibratingCycles();
|
||||||
}
|
}
|
||||||
|
|
||||||
static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThreshold)
|
static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThreshold)
|
||||||
{
|
{
|
||||||
int8_t axis;
|
|
||||||
static int32_t g[3];
|
static int32_t g[3];
|
||||||
static stdev_t var[3];
|
static stdev_t var[3];
|
||||||
|
|
||||||
for (axis = 0; axis < 3; axis++) {
|
for (int axis = 0; axis < 3; axis++) {
|
||||||
|
|
||||||
// Reset g[axis] at start of calibration
|
// Reset g[axis] at start of calibration
|
||||||
if (isOnFirstGyroCalibrationCycle()) {
|
if (isOnFirstGyroCalibrationCycle()) {
|
||||||
|
@ -113,10 +112,10 @@ static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThresho
|
||||||
float dev = devStandardDeviation(&var[axis]);
|
float dev = devStandardDeviation(&var[axis]);
|
||||||
// check deviation and startover in case the model was moved
|
// check deviation and startover in case the model was moved
|
||||||
if (gyroMovementCalibrationThreshold && dev > gyroMovementCalibrationThreshold) {
|
if (gyroMovementCalibrationThreshold && dev > gyroMovementCalibrationThreshold) {
|
||||||
gyroSetCalibrationCycles(calculateCalibratingCycles());
|
gyroSetCalibrationCycles();
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
gyroZero[axis] = (g[axis] + (calculateCalibratingCycles() / 2)) / calculateCalibratingCycles();
|
gyroZero[axis] = (g[axis] + (gyroCalculateCalibratingCycles() / 2)) / gyroCalculateCalibratingCycles();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -129,8 +128,7 @@ static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThresho
|
||||||
|
|
||||||
static void applyGyroZero(void)
|
static void applyGyroZero(void)
|
||||||
{
|
{
|
||||||
int8_t axis;
|
for (int axis = 0; axis < 3; axis++) {
|
||||||
for (axis = 0; axis < 3; axis++) {
|
|
||||||
gyroADC[axis] -= gyroZero[axis];
|
gyroADC[axis] -= gyroZero[axis];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -138,14 +136,13 @@ static void applyGyroZero(void)
|
||||||
void gyroUpdate(void)
|
void gyroUpdate(void)
|
||||||
{
|
{
|
||||||
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
|
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
|
||||||
int axis;
|
|
||||||
|
|
||||||
// range: +/- 8192; +/- 2000 deg/sec
|
// range: +/- 8192; +/- 2000 deg/sec
|
||||||
if (!gyro.read(gyroADCRaw)) {
|
if (!gyro.read(gyroADCRaw)) {
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||||
if (debugMode == DEBUG_GYRO) debug[axis] = gyroADC[axis];
|
if (debugMode == DEBUG_GYRO) debug[axis] = gyroADC[axis];
|
||||||
gyroADC[axis] = gyroADCRaw[axis];
|
gyroADC[axis] = gyroADCRaw[axis];
|
||||||
}
|
}
|
||||||
|
@ -158,16 +155,10 @@ void gyroUpdate(void)
|
||||||
|
|
||||||
applyGyroZero();
|
applyGyroZero();
|
||||||
|
|
||||||
if (gyroLpfCutFreq) {
|
if (gyroSoftLpfHz) {
|
||||||
if (!gyroFilterStateIsSet) initGyroFilterCoefficients(); /* initialise filter coefficients */
|
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||||
|
gyroADCf[axis] = applyBiQuadFilter((float)gyroADC[axis], &gyroFilterState[axis]);
|
||||||
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
gyroADC[axis] = lrintf(gyroADCf[axis]);
|
||||||
if (gyroFilterStateIsSet) {
|
|
||||||
gyroADCf[axis] = applyBiQuadFilter((float) gyroADC[axis], &gyroFilterState[axis]);
|
|
||||||
gyroADC[axis] = lrintf(gyroADCf[axis]);
|
|
||||||
} else {
|
|
||||||
gyroADCf[axis] = gyroADC[axis]; // Otherwise float pid controller will not have gyro input when filter disabled
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -31,19 +31,17 @@ typedef enum {
|
||||||
} gyroSensor_e;
|
} gyroSensor_e;
|
||||||
|
|
||||||
extern gyro_t gyro;
|
extern gyro_t gyro;
|
||||||
extern sensor_align_e gyroAlign;
|
|
||||||
|
|
||||||
extern int32_t gyroADC[XYZ_AXIS_COUNT];
|
extern int32_t gyroADC[XYZ_AXIS_COUNT];
|
||||||
extern float gyroADCf[XYZ_AXIS_COUNT];
|
extern float gyroADCf[XYZ_AXIS_COUNT];
|
||||||
extern int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT];
|
|
||||||
|
|
||||||
typedef struct gyroConfig_s {
|
typedef struct gyroConfig_s {
|
||||||
uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
|
uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
|
||||||
} gyroConfig_t;
|
} gyroConfig_t;
|
||||||
|
|
||||||
void useGyroConfig(gyroConfig_t *gyroConfigToUse, float gyro_lpf_hz);
|
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_lpf_hz);
|
||||||
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
|
void gyroSetCalibrationCycles(void);
|
||||||
|
void gyroInit(void);
|
||||||
void gyroUpdate(void);
|
void gyroUpdate(void);
|
||||||
bool isGyroCalibrationComplete(void);
|
bool isGyroCalibrationComplete(void);
|
||||||
uint16_t calculateCalibratingCycles(void);
|
|
||||||
|
|
||||||
|
|
|
@ -79,6 +79,7 @@ extern float magneticDeclination;
|
||||||
extern gyro_t gyro;
|
extern gyro_t gyro;
|
||||||
extern baro_t baro;
|
extern baro_t baro;
|
||||||
extern acc_t acc;
|
extern acc_t acc;
|
||||||
|
extern sensor_align_e gyroAlign;
|
||||||
|
|
||||||
uint8_t detectedSensors[MAX_SENSORS_TO_DETECT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE };
|
uint8_t detectedSensors[MAX_SENSORS_TO_DETECT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE };
|
||||||
|
|
||||||
|
@ -632,8 +633,9 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t a
|
||||||
acc.init(&acc);
|
acc.init(&acc);
|
||||||
}
|
}
|
||||||
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
|
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
|
||||||
gyroUpdateSampleRate(gyroLpf, gyroSyncDenominator); // Set gyro refresh rate before initialisation
|
gyro.targetLooptime = gyroSetSampleRate(gyroLpf, gyroSyncDenominator); // Set gyro sample rate before initialisation
|
||||||
gyro.init(gyroLpf);
|
gyro.init(gyroLpf);
|
||||||
|
gyroInit();
|
||||||
|
|
||||||
detectMag(magHardwareToUse);
|
detectMag(magHardwareToUse);
|
||||||
|
|
||||||
|
|
|
@ -39,7 +39,7 @@ static IO_t HWDetectPin = IO_NONE;
|
||||||
|
|
||||||
void detectHardwareRevision(void)
|
void detectHardwareRevision(void)
|
||||||
{
|
{
|
||||||
HWDetectPin = IOGetByTag(IO_TAG(HW_PIN));
|
HWDetectPin = IOGetByTag(IO_TAG(HW_PIN));
|
||||||
IOInit(HWDetectPin, OWNER_SYSTEM, RESOURCE_INPUT);
|
IOInit(HWDetectPin, OWNER_SYSTEM, RESOURCE_INPUT);
|
||||||
IOConfigGPIO(HWDetectPin, IOCFG_IPU);
|
IOConfigGPIO(HWDetectPin, IOCFG_IPU);
|
||||||
|
|
||||||
|
@ -74,4 +74,4 @@ const extiConfig_t *selectMPUIntExtiConfigByHardwareRevision(void)
|
||||||
else {
|
else {
|
||||||
return &alienFlightF3V2MPUIntExtiConfig;
|
return &alienFlightF3V2MPUIntExtiConfig;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -28,12 +28,12 @@ const uint16_t multiPWM[] = {
|
||||||
PWM4 | (MAP_TO_PWM_INPUT << 8),
|
PWM4 | (MAP_TO_PWM_INPUT << 8),
|
||||||
PWM5 | (MAP_TO_PWM_INPUT << 8),
|
PWM5 | (MAP_TO_PWM_INPUT << 8),
|
||||||
PWM6 | (MAP_TO_PWM_INPUT << 8), // input #6
|
PWM6 | (MAP_TO_PWM_INPUT << 8), // input #6
|
||||||
PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed)
|
PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed)
|
||||||
PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed)
|
PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed)
|
||||||
PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or #3
|
PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or #3
|
||||||
PWM10 | (MAP_TO_MOTOR_OUTPUT << 8),
|
PWM10 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||||
PWM11 | (MAP_TO_MOTOR_OUTPUT << 8),
|
PWM11 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||||
PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 or #6
|
PWM12 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4 or #6
|
||||||
0xFFFF
|
0xFFFF
|
||||||
};
|
};
|
||||||
|
|
||||||
|
@ -59,26 +59,26 @@ const uint16_t airPWM[] = {
|
||||||
PWM4 | (MAP_TO_PWM_INPUT << 8),
|
PWM4 | (MAP_TO_PWM_INPUT << 8),
|
||||||
PWM5 | (MAP_TO_PWM_INPUT << 8),
|
PWM5 | (MAP_TO_PWM_INPUT << 8),
|
||||||
PWM6 | (MAP_TO_PWM_INPUT << 8), // input #6
|
PWM6 | (MAP_TO_PWM_INPUT << 8), // input #6
|
||||||
PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1
|
PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1
|
||||||
PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2
|
PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2
|
||||||
PWM9 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1
|
PWM9 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1
|
||||||
PWM10 | (MAP_TO_SERVO_OUTPUT << 8), // servo #2
|
PWM10 | (MAP_TO_SERVO_OUTPUT << 8), // servo #2
|
||||||
PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #3
|
PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #3
|
||||||
PWM12 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4
|
PWM12 | (MAP_TO_SERVO_OUTPUT << 8), // servo #4
|
||||||
0xFFFF
|
0xFFFF
|
||||||
};
|
};
|
||||||
|
|
||||||
const uint16_t multiPPM_BP6[] = {
|
const uint16_t multiPPM_BP6[] = {
|
||||||
PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input
|
PWM6 | (MAP_TO_PPM_INPUT << 8), // PPM input
|
||||||
PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1
|
PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1
|
||||||
PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2
|
PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2
|
||||||
PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #3
|
PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #3
|
||||||
PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4
|
PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #4
|
||||||
PWM11 | (MAP_TO_MOTOR_OUTPUT << 8),
|
PWM11 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||||
PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed
|
PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed
|
||||||
PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed
|
PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed
|
||||||
PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed
|
PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed
|
||||||
PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed
|
PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed
|
||||||
0xFFFF
|
0xFFFF
|
||||||
};
|
};
|
||||||
|
|
||||||
|
@ -89,9 +89,9 @@ const uint16_t multiPWM_BP6[] = {
|
||||||
PWM4 | (MAP_TO_PWM_INPUT << 8),
|
PWM4 | (MAP_TO_PWM_INPUT << 8),
|
||||||
PWM5 | (MAP_TO_PWM_INPUT << 8),
|
PWM5 | (MAP_TO_PWM_INPUT << 8),
|
||||||
PWM6 | (MAP_TO_PWM_INPUT << 8), // input #6
|
PWM6 | (MAP_TO_PWM_INPUT << 8), // input #6
|
||||||
PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed)
|
PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or servo #1 (swap to servo if needed)
|
||||||
PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed)
|
PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2 or servo #2 (swap to servo if needed)
|
||||||
PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or #3
|
PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1 or #3
|
||||||
PWM10 | (MAP_TO_MOTOR_OUTPUT << 8),
|
PWM10 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||||
PWM11 | (MAP_TO_MOTOR_OUTPUT << 8),
|
PWM11 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||||
0xFFFF
|
0xFFFF
|
||||||
|
@ -118,11 +118,11 @@ const uint16_t airPWM_BP6[] = {
|
||||||
PWM4 | (MAP_TO_PWM_INPUT << 8),
|
PWM4 | (MAP_TO_PWM_INPUT << 8),
|
||||||
PWM5 | (MAP_TO_PWM_INPUT << 8),
|
PWM5 | (MAP_TO_PWM_INPUT << 8),
|
||||||
PWM6 | (MAP_TO_PWM_INPUT << 8), // input #6
|
PWM6 | (MAP_TO_PWM_INPUT << 8), // input #6
|
||||||
PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1
|
PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #1
|
||||||
PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2
|
PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // motor #2
|
||||||
PWM9 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1
|
PWM9 | (MAP_TO_SERVO_OUTPUT << 8), // servo #1
|
||||||
PWM10 | (MAP_TO_SERVO_OUTPUT << 8), // servo #2
|
PWM10 | (MAP_TO_SERVO_OUTPUT << 8), // servo #2
|
||||||
PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #3
|
PWM11 | (MAP_TO_SERVO_OUTPUT << 8), // servo #3
|
||||||
0xFFFF
|
0xFFFF
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -55,5 +55,5 @@ void updateHardwareRevision(void)
|
||||||
|
|
||||||
const extiConfig_t *selectMPUIntExtiConfigByHardwareRevision(void)
|
const extiConfig_t *selectMPUIntExtiConfigByHardwareRevision(void)
|
||||||
{
|
{
|
||||||
return NULL;
|
return NULL;
|
||||||
}
|
}
|
||||||
|
|
|
@ -34,7 +34,7 @@
|
||||||
#define MPU6500_CS_PIN PA4
|
#define MPU6500_CS_PIN PA4
|
||||||
#define MPU6500_SPI_INSTANCE SPI1
|
#define MPU6500_SPI_INSTANCE SPI1
|
||||||
|
|
||||||
#define MPU6000_CS_PIN PA4
|
#define MPU6000_CS_PIN PA4
|
||||||
#define MPU6000_SPI_INSTANCE SPI1
|
#define MPU6000_SPI_INSTANCE SPI1
|
||||||
|
|
||||||
#define USE_SPI
|
#define USE_SPI
|
||||||
|
|
|
@ -25,9 +25,9 @@
|
||||||
#define USE_EXTI
|
#define USE_EXTI
|
||||||
#define CONFIG_PREFER_ACC_ON
|
#define CONFIG_PREFER_ACC_ON
|
||||||
|
|
||||||
#define LED0 PC14
|
#define LED0 PC14
|
||||||
|
|
||||||
#define BEEPER PC15
|
#define BEEPER PC15
|
||||||
#define BEEPER_INVERTED
|
#define BEEPER_INVERTED
|
||||||
|
|
||||||
#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU INT, SDCardDetect
|
#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU INT, SDCardDetect
|
||||||
|
@ -188,7 +188,7 @@
|
||||||
#define TARGET_IO_PORTB 0xffff
|
#define TARGET_IO_PORTB 0xffff
|
||||||
#define TARGET_IO_PORTC 0xffff
|
#define TARGET_IO_PORTC 0xffff
|
||||||
#define TARGET_IO_PORTD 0xffff
|
#define TARGET_IO_PORTD 0xffff
|
||||||
#define TARGET_IO_PORTF (BIT(4))
|
#define TARGET_IO_PORTF (BIT(4))
|
||||||
|
|
||||||
#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(16) |TIM_N(17))
|
#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(16) |TIM_N(17))
|
||||||
|
|
||||||
|
|
|
@ -86,6 +86,6 @@
|
||||||
#define TARGET_IO_PORTB 0xffff
|
#define TARGET_IO_PORTB 0xffff
|
||||||
#define TARGET_IO_PORTC 0xffff
|
#define TARGET_IO_PORTC 0xffff
|
||||||
#define TARGET_IO_PORTD 0xffff
|
#define TARGET_IO_PORTD 0xffff
|
||||||
#define TARGET_IO_PORTF (BIT(4))
|
#define TARGET_IO_PORTF (BIT(4))
|
||||||
|
|
||||||
#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(15) | TIM_N(16) | TIM_N(17))
|
#define USED_TIMERS (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(15) | TIM_N(16) | TIM_N(17))
|
||||||
|
|
|
@ -26,12 +26,12 @@ const uint16_t multiPWM[] = {
|
||||||
PWM3 | (MAP_TO_MOTOR_OUTPUT << 8),
|
PWM3 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||||
PWM4 | (MAP_TO_MOTOR_OUTPUT << 8),
|
PWM4 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||||
PWM5 | (MAP_TO_MOTOR_OUTPUT << 8),
|
PWM5 | (MAP_TO_MOTOR_OUTPUT << 8),
|
||||||
PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed
|
PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed
|
||||||
PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed
|
PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed
|
||||||
PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed
|
PWM8 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed
|
||||||
PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed
|
PWM9 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed
|
||||||
PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed
|
PWM10 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed
|
||||||
PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed
|
PWM11 | (MAP_TO_MOTOR_OUTPUT << 8), // Swap to servo if needed
|
||||||
0xFFFF
|
0xFFFF
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -23,7 +23,7 @@
|
||||||
|
|
||||||
#define USBD_PRODUCT_STRING "Revolution"
|
#define USBD_PRODUCT_STRING "Revolution"
|
||||||
#ifdef OPBL
|
#ifdef OPBL
|
||||||
#define USBD_SERIALNUMBER_STRING "0x8020000"
|
#define USBD_SERIALNUMBER_STRING "0x8020000"
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#define LED0 PB5
|
#define LED0 PB5
|
||||||
|
|
|
@ -22,7 +22,7 @@
|
||||||
|
|
||||||
#define USBD_PRODUCT_STRING "Revo Nano"
|
#define USBD_PRODUCT_STRING "Revo Nano"
|
||||||
#ifdef OPBL
|
#ifdef OPBL
|
||||||
#define USBD_SERIALNUMBER_STRING "0x8010000"
|
#define USBD_SERIALNUMBER_STRING "0x8010000"
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#define LED0 PC14
|
#define LED0 PC14
|
||||||
|
|
|
@ -92,15 +92,15 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
|
||||||
{ TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N
|
{ TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N
|
||||||
{ TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N
|
{ TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N
|
||||||
|
|
||||||
{ TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1
|
{ TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1
|
||||||
{ TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1
|
{ TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1
|
||||||
{ TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM3 - PA11
|
{ TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM3 - PA11
|
||||||
{ TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM4 - PA12
|
{ TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM4 - PA12
|
||||||
{ TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 - PB8
|
{ TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 - PB8
|
||||||
{ TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM6 - PB9
|
{ TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM6 - PB9
|
||||||
{ TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM7 - PA2
|
{ TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM7 - PA2
|
||||||
{ TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM8 - PA3
|
{ TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM8 - PA3
|
||||||
|
|
||||||
{ TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, 0}, // GPIO_TIMER / LED_STRIP
|
{ TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, 0}, // GPIO_TIMER / LED_STRIP
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -84,7 +84,6 @@ const uint16_t airPWM[] = {
|
||||||
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
|
const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
|
||||||
{ TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH1 - PA0 - *TIM2_CH1
|
{ TIM2, IO_TAG(PA0), TIM_Channel_1, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH1 - PA0 - *TIM2_CH1
|
||||||
{ TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N
|
{ TIM2, IO_TAG(PA1), TIM_Channel_2, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N
|
||||||
// Production boards swapped RC_CH3/4 swapped to make it easier to use SerialRX using supplied cables - compared to first prototype.
|
|
||||||
{ TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7)
|
{ TIM2, IO_TAG(PB11), TIM_Channel_4, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH3 - PB11 - *TIM2_CH4, USART3_RX (AF7)
|
||||||
{ TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH4 - PB10 - *TIM2_CH3, USART3_TX (AF7)
|
{ TIM2, IO_TAG(PB10), TIM_Channel_3, TIM2_IRQn, 0, IOCFG_AF_PP, GPIO_AF_1, 0}, // RC_CH4 - PB10 - *TIM2_CH3, USART3_TX (AF7)
|
||||||
{ TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH5 - PB4 - *TIM3_CH1
|
{ TIM3, IO_TAG(PB4), TIM_Channel_1, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH5 - PB4 - *TIM3_CH1
|
||||||
|
@ -92,15 +91,15 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {
|
||||||
{ TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N
|
{ TIM3, IO_TAG(PB0), TIM_Channel_3, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N
|
||||||
{ TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N
|
{ TIM3, IO_TAG(PB1), TIM_Channel_4, TIM3_IRQn, 0, IOCFG_AF_PP, GPIO_AF_2, 0}, // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N
|
||||||
|
|
||||||
{ TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1
|
{ TIM16, IO_TAG(PA6), TIM_Channel_1, TIM1_UP_TIM16_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1
|
||||||
{ TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1
|
{ TIM17, IO_TAG(PA7), TIM_Channel_1, TIM1_TRG_COM_TIM17_IRQn, 1, IOCFG_AF_PP, GPIO_AF_1, 0}, // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1
|
||||||
{ TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM3 - PA11
|
{ TIM4, IO_TAG(PA11), TIM_Channel_1, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM3 - PA11
|
||||||
{ TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM4 - PA12
|
{ TIM4, IO_TAG(PA12), TIM_Channel_2, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_10, 0}, // PWM4 - PA12
|
||||||
{ TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 - PB8
|
{ TIM4, IO_TAG(PB8), TIM_Channel_3, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM5 - PB8
|
||||||
{ TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM6 - PB9
|
{ TIM4, IO_TAG(PB9), TIM_Channel_4, TIM4_IRQn, 1, IOCFG_AF_PP, GPIO_AF_2, 0}, // PWM6 - PB9
|
||||||
{ TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM7 - PA2
|
{ TIM15, IO_TAG(PA2), TIM_Channel_1, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM7 - PA2
|
||||||
{ TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM8 - PA3
|
{ TIM15, IO_TAG(PA3), TIM_Channel_2, TIM1_BRK_TIM15_IRQn, 1, IOCFG_AF_PP, GPIO_AF_9, 0}, // PWM8 - PA3
|
||||||
|
|
||||||
{ TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, 0}, // GPIO_TIMER / LED_STRIP
|
{ TIM1, IO_TAG(PA8), TIM_Channel_1, TIM1_CC_IRQn, 1, IOCFG_AF_PP, GPIO_AF_6, 0}, // GPIO_TIMER / LED_STRIP
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -17,7 +17,7 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#define TARGET_BOARD_IDENTIFIER "X_RACERSPI"
|
#define TARGET_BOARD_IDENTIFIER "XRC3"
|
||||||
|
|
||||||
#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE
|
#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_NONE
|
||||||
|
|
||||||
|
|
|
@ -12,5 +12,5 @@ TARGET_SRC = \
|
||||||
drivers/light_ws2811strip.c \
|
drivers/light_ws2811strip.c \
|
||||||
drivers/light_ws2811strip_stm32f30x.c \
|
drivers/light_ws2811strip_stm32f30x.c \
|
||||||
drivers/serial_softserial.c \
|
drivers/serial_softserial.c \
|
||||||
drivers/sonar_hcsr04.c
|
drivers/sonar_hcsr04.c
|
||||||
|
|
||||||
|
|
|
@ -21,7 +21,7 @@
|
||||||
|
|
||||||
#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT
|
#define CONFIG_FASTLOOP_PREFERRED_ACC ACC_DEFAULT
|
||||||
|
|
||||||
#define LED0 PB8
|
#define LED0 PB8
|
||||||
|
|
||||||
#define BEEPER PC15
|
#define BEEPER PC15
|
||||||
#define BEEPER_INVERTED
|
#define BEEPER_INVERTED
|
||||||
|
@ -45,8 +45,8 @@
|
||||||
#define USE_ACC_MPU6500
|
#define USE_ACC_MPU6500
|
||||||
#define USE_ACC_SPI_MPU6500
|
#define USE_ACC_SPI_MPU6500
|
||||||
|
|
||||||
#define ACC_MPU6500_ALIGN CW180_DEG
|
#define ACC_MPU6500_ALIGN CW180_DEG
|
||||||
#define GYRO_MPU6500_ALIGN CW180_DEG
|
#define GYRO_MPU6500_ALIGN CW180_DEG
|
||||||
|
|
||||||
#define BARO
|
#define BARO
|
||||||
#define USE_BARO_BMP280
|
#define USE_BARO_BMP280
|
||||||
|
|
|
@ -424,8 +424,8 @@ static void hottCheckSerialData(uint32_t currentMicros)
|
||||||
}
|
}
|
||||||
|
|
||||||
static void workAroundForHottTelemetryOnUsart(serialPort_t *instance, portMode_t mode) {
|
static void workAroundForHottTelemetryOnUsart(serialPort_t *instance, portMode_t mode) {
|
||||||
closeSerialPort(hottPort);
|
closeSerialPort(hottPort);
|
||||||
hottPort = openSerialPort(instance->identifier, FUNCTION_TELEMETRY_HOTT, NULL, HOTT_BAUDRATE, mode, SERIAL_NOT_INVERTED);
|
hottPort = openSerialPort(instance->identifier, FUNCTION_TELEMETRY_HOTT, NULL, HOTT_BAUDRATE, mode, SERIAL_NOT_INVERTED);
|
||||||
}
|
}
|
||||||
|
|
||||||
static void hottSendTelemetryData(void) {
|
static void hottSendTelemetryData(void) {
|
||||||
|
@ -433,9 +433,9 @@ static void hottSendTelemetryData(void) {
|
||||||
hottIsSending = true;
|
hottIsSending = true;
|
||||||
// FIXME temorary workaround for HoTT not working on Hardware serial ports due to hardware/softserial serial port initialisation differences
|
// FIXME temorary workaround for HoTT not working on Hardware serial ports due to hardware/softserial serial port initialisation differences
|
||||||
if ((portConfig->identifier == SERIAL_PORT_USART1) || (portConfig->identifier == SERIAL_PORT_USART2) || (portConfig->identifier == SERIAL_PORT_USART3))
|
if ((portConfig->identifier == SERIAL_PORT_USART1) || (portConfig->identifier == SERIAL_PORT_USART2) || (portConfig->identifier == SERIAL_PORT_USART3))
|
||||||
workAroundForHottTelemetryOnUsart(hottPort, MODE_TX);
|
workAroundForHottTelemetryOnUsart(hottPort, MODE_TX);
|
||||||
else
|
else
|
||||||
serialSetMode(hottPort, MODE_TX);
|
serialSetMode(hottPort, MODE_TX);
|
||||||
hottMsgCrc = 0;
|
hottMsgCrc = 0;
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
@ -445,9 +445,9 @@ static void hottSendTelemetryData(void) {
|
||||||
hottIsSending = false;
|
hottIsSending = false;
|
||||||
// FIXME temorary workaround for HoTT not working on Hardware serial ports due to hardware/softserial serial port initialisation differences
|
// FIXME temorary workaround for HoTT not working on Hardware serial ports due to hardware/softserial serial port initialisation differences
|
||||||
if ((portConfig->identifier == SERIAL_PORT_USART1) || (portConfig->identifier == SERIAL_PORT_USART2) || (portConfig->identifier == SERIAL_PORT_USART3))
|
if ((portConfig->identifier == SERIAL_PORT_USART1) || (portConfig->identifier == SERIAL_PORT_USART2) || (portConfig->identifier == SERIAL_PORT_USART3))
|
||||||
workAroundForHottTelemetryOnUsart(hottPort, MODE_RX);
|
workAroundForHottTelemetryOnUsart(hottPort, MODE_RX);
|
||||||
else
|
else
|
||||||
serialSetMode(hottPort, MODE_RX);
|
serialSetMode(hottPort, MODE_RX);
|
||||||
flushHottRxBuffer();
|
flushHottRxBuffer();
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
|
@ -28,12 +28,12 @@
|
||||||
#include "../drivers/io.h"
|
#include "../drivers/io.h"
|
||||||
|
|
||||||
void USB_OTG_BSP_ConfigVBUS(USB_OTG_CORE_HANDLE *pdev) {
|
void USB_OTG_BSP_ConfigVBUS(USB_OTG_CORE_HANDLE *pdev) {
|
||||||
(void)pdev;
|
(void)pdev;
|
||||||
}
|
}
|
||||||
|
|
||||||
void USB_OTG_BSP_DriveVBUS(USB_OTG_CORE_HANDLE *pdev,uint8_t state) {
|
void USB_OTG_BSP_DriveVBUS(USB_OTG_CORE_HANDLE *pdev,uint8_t state) {
|
||||||
(void)pdev;
|
(void)pdev;
|
||||||
(void)state;
|
(void)state;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@ -46,7 +46,7 @@ void USB_OTG_BSP_DriveVBUS(USB_OTG_CORE_HANDLE *pdev,uint8_t state) {
|
||||||
|
|
||||||
void USB_OTG_BSP_Init(USB_OTG_CORE_HANDLE *pdev)
|
void USB_OTG_BSP_Init(USB_OTG_CORE_HANDLE *pdev)
|
||||||
{
|
{
|
||||||
(void)pdev;
|
(void)pdev;
|
||||||
GPIO_InitTypeDef GPIO_InitStructure;
|
GPIO_InitTypeDef GPIO_InitStructure;
|
||||||
|
|
||||||
#ifndef USE_ULPI_PHY
|
#ifndef USE_ULPI_PHY
|
||||||
|
@ -100,7 +100,7 @@ void USB_OTG_BSP_Init(USB_OTG_CORE_HANDLE *pdev)
|
||||||
*/
|
*/
|
||||||
void USB_OTG_BSP_EnableInterrupt(USB_OTG_CORE_HANDLE *pdev)
|
void USB_OTG_BSP_EnableInterrupt(USB_OTG_CORE_HANDLE *pdev)
|
||||||
{
|
{
|
||||||
(void)pdev;
|
(void)pdev;
|
||||||
NVIC_InitTypeDef NVIC_InitStructure;
|
NVIC_InitTypeDef NVIC_InitStructure;
|
||||||
|
|
||||||
NVIC_PriorityGroupConfig(NVIC_PRIORITY_GROUPING);
|
NVIC_PriorityGroupConfig(NVIC_PRIORITY_GROUPING);
|
||||||
|
|
|
@ -233,9 +233,9 @@
|
||||||
#elif defined (__ICCARM__) /* IAR Compiler */
|
#elif defined (__ICCARM__) /* IAR Compiler */
|
||||||
#define __packed __packed
|
#define __packed __packed
|
||||||
#elif defined ( __GNUC__ ) /* GNU Compiler */
|
#elif defined ( __GNUC__ ) /* GNU Compiler */
|
||||||
#ifndef __packed
|
#ifndef __packed
|
||||||
#define __packed __attribute__ ((__packed__))
|
#define __packed __attribute__ ((__packed__))
|
||||||
#endif
|
#endif
|
||||||
#elif defined (__TASKING__) /* TASKING Compiler */
|
#elif defined (__TASKING__) /* TASKING Compiler */
|
||||||
#define __packed __unaligned
|
#define __packed __unaligned
|
||||||
#endif /* __CC_ARM */
|
#endif /* __CC_ARM */
|
||||||
|
|
|
@ -34,7 +34,7 @@
|
||||||
#include "usbd_usr.h"
|
#include "usbd_usr.h"
|
||||||
#include "usbd_desc.h"
|
#include "usbd_desc.h"
|
||||||
|
|
||||||
#define USB_RX_BUFSIZE 1024
|
#define USB_RX_BUFSIZE 1024
|
||||||
|
|
||||||
__ALIGN_BEGIN USB_OTG_CORE_HANDLE USB_OTG_dev __ALIGN_END;
|
__ALIGN_BEGIN USB_OTG_CORE_HANDLE USB_OTG_dev __ALIGN_END;
|
||||||
|
|
||||||
|
|
|
@ -197,7 +197,7 @@ __ALIGN_BEGIN uint8_t USBD_LangIDDesc[USB_SIZ_STRING_LANGID] __ALIGN_END =
|
||||||
*/
|
*/
|
||||||
uint8_t * USBD_USR_DeviceDescriptor( uint8_t speed , uint16_t *length)
|
uint8_t * USBD_USR_DeviceDescriptor( uint8_t speed , uint16_t *length)
|
||||||
{
|
{
|
||||||
(void)speed;
|
(void)speed;
|
||||||
*length = sizeof(USBD_DeviceDesc);
|
*length = sizeof(USBD_DeviceDesc);
|
||||||
return USBD_DeviceDesc;
|
return USBD_DeviceDesc;
|
||||||
}
|
}
|
||||||
|
@ -211,7 +211,7 @@ uint8_t * USBD_USR_DeviceDescriptor( uint8_t speed , uint16_t *length)
|
||||||
*/
|
*/
|
||||||
uint8_t * USBD_USR_LangIDStrDescriptor( uint8_t speed , uint16_t *length)
|
uint8_t * USBD_USR_LangIDStrDescriptor( uint8_t speed , uint16_t *length)
|
||||||
{
|
{
|
||||||
(void)speed;
|
(void)speed;
|
||||||
*length = sizeof(USBD_LangIDDesc);
|
*length = sizeof(USBD_LangIDDesc);
|
||||||
return USBD_LangIDDesc;
|
return USBD_LangIDDesc;
|
||||||
}
|
}
|
||||||
|
@ -245,7 +245,7 @@ uint8_t * USBD_USR_ProductStrDescriptor( uint8_t speed , uint16_t *length)
|
||||||
*/
|
*/
|
||||||
uint8_t * USBD_USR_ManufacturerStrDescriptor( uint8_t speed , uint16_t *length)
|
uint8_t * USBD_USR_ManufacturerStrDescriptor( uint8_t speed , uint16_t *length)
|
||||||
{
|
{
|
||||||
(void)speed;
|
(void)speed;
|
||||||
USBD_GetString ((uint8_t*)USBD_MANUFACTURER_STRING, USBD_StrDesc, length);
|
USBD_GetString ((uint8_t*)USBD_MANUFACTURER_STRING, USBD_StrDesc, length);
|
||||||
return USBD_StrDesc;
|
return USBD_StrDesc;
|
||||||
}
|
}
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue