1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-23 00:05:33 +03:00

Moving EXTI to new io

Updated i2c to use new io
This commit is contained in:
blckmn 2016-06-02 22:23:52 +10:00
parent 7db5445bf7
commit 928609e2bb
32 changed files with 559 additions and 676 deletions

View file

@ -191,110 +191,50 @@ static void mpu6050FindRevision(void)
} }
} }
void MPU_DATA_READY_EXTI_Handler(void) extiCallbackRec_t mpuIntCallbackRec;
void mpuIntExtiHandler(extiCallbackRec_t *cb)
{ {
if (EXTI_GetITStatus(mpuIntExtiConfig->exti_line) == RESET) { UNUSED(cb);
return;
}
EXTI_ClearITPendingBit(mpuIntExtiConfig->exti_line);
mpuDataReady = true; mpuDataReady = true;
#ifdef DEBUG_MPU_DATA_READY_INTERRUPT #ifdef DEBUG_MPU_DATA_READY_INTERRUPT
// Measure the delta in micro seconds between calls to the interrupt handler
static uint32_t lastCalledAt = 0; static uint32_t lastCalledAt = 0;
static int32_t callDelta = 0;
uint32_t now = micros(); uint32_t now = micros();
callDelta = now - lastCalledAt; uint32_t callDelta = now - lastCalledAt;
//UNUSED(callDelta);
debug[0] = callDelta; debug[0] = callDelta;
lastCalledAt = now; lastCalledAt = now;
#endif #endif
} }
void configureMPUDataReadyInterruptHandling(void)
{
#ifdef USE_MPU_DATA_READY_SIGNAL
#ifdef STM32F10X
// enable AFIO for EXTI support
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
#endif
#ifdef STM32F303xC
/* Enable SYSCFG clock otherwise the EXTI irq handlers are not called */
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
#endif
#ifdef STM32F10X
gpioExtiLineConfig(mpuIntExtiConfig->exti_port_source, mpuIntExtiConfig->exti_pin_source);
#endif
#ifdef STM32F303xC
gpioExtiLineConfig(mpuIntExtiConfig->exti_port_source, mpuIntExtiConfig->exti_pin_source);
#endif
#ifdef ENSURE_MPU_DATA_READY_IS_LOW
uint8_t status = GPIO_ReadInputDataBit(mpuIntExtiConfig->gpioPort, mpuIntExtiConfig->gpioPin);
if (status) {
return;
}
#endif
registerExtiCallbackHandler(mpuIntExtiConfig->exti_irqn, MPU_DATA_READY_EXTI_Handler);
EXTI_ClearITPendingBit(mpuIntExtiConfig->exti_line);
EXTI_InitTypeDef EXTIInit;
EXTIInit.EXTI_Line = mpuIntExtiConfig->exti_line;
EXTIInit.EXTI_Mode = EXTI_Mode_Interrupt;
EXTIInit.EXTI_Trigger = EXTI_Trigger_Rising;
EXTIInit.EXTI_LineCmd = ENABLE;
EXTI_Init(&EXTIInit);
NVIC_InitTypeDef NVIC_InitStructure;
NVIC_InitStructure.NVIC_IRQChannel = mpuIntExtiConfig->exti_irqn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_MPU_DATA_READY);
NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_MPU_DATA_READY);
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
#endif
}
void mpuIntExtiInit(void) void mpuIntExtiInit(void)
{ {
gpio_config_t gpio; static bool mpuExtiInitDone = false;
static bool mpuExtiInitDone = false; if (mpuExtiInitDone || !mpuIntExtiConfig) {
return;
}
if (mpuExtiInitDone || !mpuIntExtiConfig) { #if defined(USE_MPU_DATA_READY_SIGNAL) && defined(USE_EXTI)
return;
}
#ifdef STM32F303 IO_t mpuIntIO = IOGetByTag(mpuIntExtiConfig->io);
if (mpuIntExtiConfig->gpioAHBPeripherals) {
RCC_AHBPeriphClockCmd(mpuIntExtiConfig->gpioAHBPeripherals, ENABLE); #ifdef ENSURE_MPU_DATA_READY_IS_LOW
} uint8_t status = IORead(mpuIntIO);
#endif if (status) {
#ifdef STM32F10X return;
if (mpuIntExtiConfig->gpioAPB2Peripherals) { }
RCC_APB2PeriphClockCmd(mpuIntExtiConfig->gpioAPB2Peripherals, ENABLE);
}
#endif #endif
gpio.pin = mpuIntExtiConfig->gpioPin; IOInit(mpuIntIO, OWNER_SYSTEM, RESOURCE_INPUT | RESOURCE_EXTI);
gpio.speed = Speed_2MHz; IOConfigGPIO(mpuIntIO, IOCFG_IN_FLOATING); // TODO - maybe pullup / pulldown ?
gpio.mode = Mode_IN_FLOATING;
gpioInit(mpuIntExtiConfig->gpioPort, &gpio);
configureMPUDataReadyInterruptHandling(); EXTIHandlerInit(&mpuIntCallbackRec, mpuIntExtiHandler);
EXTIConfig(mpuIntIO, &mpuIntCallbackRec, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising);
mpuExtiInitDone = true; EXTIEnable(mpuIntIO, true);
#endif
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)

View file

@ -117,11 +117,15 @@
typedef bool (*mpuReadRegisterFunc)(uint8_t reg, uint8_t length, uint8_t* data); typedef bool (*mpuReadRegisterFunc)(uint8_t reg, uint8_t length, uint8_t* data);
typedef bool (*mpuWriteRegisterFunc)(uint8_t reg, uint8_t data); typedef bool (*mpuWriteRegisterFunc)(uint8_t reg, uint8_t data);
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;
mpuWriteRegisterFunc verifywrite;
mpuResetFuncPtr reset;
} mpuConfiguration_t; } mpuConfiguration_t;
extern mpuConfiguration_t mpuConfiguration; extern mpuConfiguration_t mpuConfiguration;

View file

@ -28,24 +28,31 @@
#include "system.h" #include "system.h"
#include "bus_i2c.h" #include "bus_i2c.h"
#include "nvic.h" #include "nvic.h"
#include "exti.h"
#include "io.h"
#include "barometer_bmp085.h" #include "barometer_bmp085.h"
#ifdef BARO #ifdef BARO
#if defined(BARO_EOC_GPIO) #if defined(BARO_EOC_GPIO)
static IO_t eocIO;
static bool isConversionComplete = false; static bool isConversionComplete = false;
static bool isEOCConnected = true; static bool isEOCConnected = true;
// EXTI14 for BMP085 End of Conversion Interrupt // EXTI14 for BMP085 End of Conversion Interrupt
void BMP085_EOC_EXTI_Handler(void) { void bmp085_extiHandler(extiCallbackRec_t* cb)
if (EXTI_GetITStatus(EXTI_Line14) == SET) { {
EXTI_ClearITPendingBit(EXTI_Line14); UNUSED(cb);
isConversionComplete = true; isConversionComplete = true;
}
} }
#endif static extiCallbackRec_t bmp085_extiCallbackRec;
bool bmp085TestEOCConnected(const bmp085Config_t *config);
# endif
typedef struct { typedef struct {
int16_t ac1; int16_t ac1;
@ -123,28 +130,29 @@ static int32_t bmp085_get_temperature(uint32_t ut);
static int32_t bmp085_get_pressure(uint32_t up); static int32_t bmp085_get_pressure(uint32_t up);
STATIC_UNIT_TESTED void bmp085_calculate(int32_t *pressure, int32_t *temperature); STATIC_UNIT_TESTED void bmp085_calculate(int32_t *pressure, int32_t *temperature);
static IO_t xclrIO;
#ifdef BARO_XCLR_PIN #ifdef BARO_XCLR_PIN
#define BMP085_OFF digitalLo(BARO_XCLR_GPIO, BARO_XCLR_PIN); #define BMP085_OFF IOLo(xclrIO);
#define BMP085_ON digitalHi(BARO_XCLR_GPIO, BARO_XCLR_PIN); #define BMP085_ON IOHi(xclrIO);
#else #else
#define BMP085_OFF #define BMP085_OFF
#define BMP085_ON #define BMP085_ON
#endif #endif
void bmp085InitXCLRGpio(const bmp085Config_t *config) {
gpio_config_t gpio;
RCC_APB2PeriphClockCmd(config->gpioAPB2Peripherals, ENABLE); void bmp085InitXclrIO(const bmp085Config_t *config)
{
gpio.pin = config->xclrGpioPin; if (!xclrIO && config && config->xclrIO) {
gpio.speed = Speed_2MHz; xclrIO = IOGetByTag(config->xclrIO);
gpio.mode = Mode_Out_PP; IOInit(xclrIO, OWNER_SYSTEM, RESOURCE_OUTPUT);
gpioInit(config->xclrGpioPort, &gpio); IOConfigGPIO(xclrIO, IOCFG_OUT_PP);
}
} }
void bmp085Disable(const bmp085Config_t *config) void bmp085Disable(const bmp085Config_t *config)
{ {
bmp085InitXCLRGpio(config); bmp085InitXclrIO(config);
BMP085_OFF; BMP085_OFF;
} }
@ -152,39 +160,25 @@ bool bmp085Detect(const bmp085Config_t *config, baro_t *baro)
{ {
uint8_t data; uint8_t data;
bool ack; bool ack;
#if defined(BARO_EOC_GPIO)
IO_t eocIO = IO_NONE;
#endif
if (bmp085InitDone) if (bmp085InitDone)
return true; return true;
#if defined(BARO_XCLR_GPIO) && defined(BARO_EOC_GPIO) bmp085InitXclrIO(config);
if (config) { BMP085_ON; // enable baro
EXTI_InitTypeDef EXTI_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
gpio_config_t gpio;
bmp085InitXCLRGpio(config);
gpio.pin = config->eocGpioPin;
gpio.mode = Mode_IPD;
gpioInit(config->eocGpioPort, &gpio);
BMP085_ON;
registerExtiCallbackHandler(EXTI15_10_IRQn, BMP085_EOC_EXTI_Handler);
#if defined(BARO_EOC_GPIO) && defined(USE_EXTI)
if (config && config->eocIO) {
eocIO = IOGetByTag(config->eocIO);
// EXTI interrupt for barometer EOC // EXTI interrupt for barometer EOC
gpioExtiLineConfig(GPIO_PortSourceGPIOC, GPIO_PinSource14); IOInit(eocIO, OWNER_SYSTEM, RESOURCE_INPUT | RESOURCE_EXTI);
EXTI_InitStructure.EXTI_Line = EXTI_Line14; IOConfigGPIO(eocIO, Mode_IN_FLOATING);
EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt; EXTIHandlerInit(&bmp085_extiCallbackRec, bmp085_extiHandler);
EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising; EXTIConfig(eocIO, &bmp085_extiCallbackRec, NVIC_PRIO_BARO_EXTI, EXTI_Trigger_Rising);
EXTI_InitStructure.EXTI_LineCmd = ENABLE; EXTIEnable(eocIO, true);
EXTI_Init(&EXTI_InitStructure);
// Enable and set EXTI10-15 Interrupt to the lowest priority
NVIC_InitStructure.NVIC_IRQChannel = EXTI15_10_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_BARO_EXT);
NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_BARO_EXT);
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
} }
#else #else
UNUSED(config); UNUSED(config);
@ -218,13 +212,8 @@ bool bmp085Detect(const bmp085Config_t *config, baro_t *baro)
} }
#if defined(BARO_EOC_GPIO) #if defined(BARO_EOC_GPIO)
EXTI_InitTypeDef EXTI_InitStructure; if (eocIO)
EXTI_StructInit(&EXTI_InitStructure); EXTIRelease(eocIO);
EXTI_InitStructure.EXTI_Line = EXTI_Line14;
EXTI_InitStructure.EXTI_LineCmd = DISABLE;
EXTI_Init(&EXTI_InitStructure);
unregisterExtiCallbackHandler(EXTI15_10_IRQn, BMP085_EOC_EXTI_Handler);
#endif #endif
BMP085_OFF; BMP085_OFF;
@ -379,16 +368,18 @@ static void bmp085_get_cal_param(void)
#if defined(BARO_EOC_GPIO) #if defined(BARO_EOC_GPIO)
bool bmp085TestEOCConnected(const bmp085Config_t *config) bool bmp085TestEOCConnected(const bmp085Config_t *config)
{ {
if (!bmp085InitDone) { UNUSED(config);
if (!bmp085InitDone && eocIO) {
bmp085_start_ut(); bmp085_start_ut();
delayMicroseconds(UT_DELAY * 2); // wait twice as long as normal, just to be sure delayMicroseconds(UT_DELAY * 2); // wait twice as long as normal, just to be sure
// conversion should have finished now so check if EOC is high // conversion should have finished now so check if EOC is high
uint8_t status = GPIO_ReadInputDataBit(config->eocGpioPort, config->eocGpioPin); uint8_t status = IORead(eocIO);
if (status) { if (status) {
return true; return true;
} }
} }
return false; // assume EOC is not connected return false; // assume EOC is not connected
} }
#endif #endif

View file

@ -18,11 +18,8 @@
#pragma once #pragma once
typedef struct bmp085Config_s { typedef struct bmp085Config_s {
uint32_t gpioAPB2Peripherals; ioTag_t xclrIO;
uint16_t xclrGpioPin; ioTag_t eocIO;
GPIO_TypeDef *xclrGpioPort;
uint16_t eocGpioPin;
GPIO_TypeDef *eocGpioPort;
} bmp085Config_t; } bmp085Config_t;
bool bmp085Detect(const bmp085Config_t *config, baro_t *baro); bool bmp085Detect(const bmp085Config_t *config, baro_t *baro);

View file

@ -21,13 +21,50 @@
#define I2C_LONG_TIMEOUT ((uint32_t)(10 * I2C_SHORT_TIMEOUT)) #define I2C_LONG_TIMEOUT ((uint32_t)(10 * I2C_SHORT_TIMEOUT))
#define I2C_DEFAULT_TIMEOUT I2C_SHORT_TIMEOUT #define I2C_DEFAULT_TIMEOUT I2C_SHORT_TIMEOUT
#include "drivers/io.h"
#include "drivers/rcc.h"
#ifndef I2C_DEVICE
#define I2C_DEVICE I2CINVALID
#endif
typedef enum I2CDevice { typedef enum I2CDevice {
I2CDEV_1, I2CINVALID = -1,
I2CDEV_1 = 0,
I2CDEV_2, I2CDEV_2,
I2CDEV_MAX = I2CDEV_2, I2CDEV_3,
I2CDEV_MAX = I2CDEV_3,
} I2CDevice; } I2CDevice;
void i2cInit(I2CDevice index); typedef struct i2cDevice_s {
I2C_TypeDef *dev;
ioTag_t scl;
ioTag_t sda;
rccPeriphTag_t rcc;
bool overClock;
#if !defined(STM32F303xC)
uint8_t ev_irq;
uint8_t er_irq;
#endif
} i2cDevice_t;
typedef struct i2cState_s {
volatile bool error;
volatile bool busy;
volatile uint8_t addr;
volatile uint8_t reg;
volatile uint8_t bytes;
volatile uint8_t writing;
volatile uint8_t reading;
volatile uint8_t* write_p;
volatile uint8_t* read_p;
} i2cState_t;
void i2cInit(I2CDevice device);
bool i2cWriteBufferByDevice(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data);
bool i2cWriteByDevice(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t data);
bool i2cReadByDevice(I2CDevice device, uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* buf);
bool i2cWriteBuffer(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data); bool i2cWriteBuffer(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data);
bool i2cWrite(uint8_t addr_, uint8_t reg, uint8_t data); bool i2cWrite(uint8_t addr_, uint8_t reg, uint8_t data);
bool i2cRead(uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* buf); bool i2cRead(uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* buf);

View file

@ -23,16 +23,22 @@
#include "build_config.h" #include "build_config.h"
#include "gpio.h" #include "io.h"
#include "system.h" #include "system.h"
#include "bus_i2c.h" #include "bus_i2c.h"
#include "nvic.h" #include "nvic.h"
#include "io_impl.h"
#include "rcc.h"
#ifndef SOFT_I2C #ifndef SOFT_I2C
#define CLOCKSPEED 800000 // i2c clockspeed 400kHz default (conform specs), 800kHz and 1200kHz (Betaflight default) #define CLOCKSPEED 800000 // i2c clockspeed 400kHz default (conform specs), 800kHz and 1200kHz (Betaflight default)
static void i2c_er_handler(I2CDevice device);
static void i2c_ev_handler(I2CDevice device);
static void i2cUnstick(IO_t scl, IO_t sda);
// I2C2 // I2C2
// SCL PB10 // SCL PB10
// SDA PB11 // SDA PB11
@ -40,329 +46,375 @@
// SCL PB6 // SCL PB6
// SDA PB7 // SDA PB7
static void i2c_er_handler(void); #ifndef I2C1_SCL
static void i2c_ev_handler(void); #define I2C1_SCL PB6
static void i2cUnstick(void); #endif
#ifndef I2C1_SDA
#define I2C1_SDA PB7
#endif
typedef struct i2cDevice_s { #ifndef I2C2_SCL
I2C_TypeDef *dev; #define I2C2_SCL PB10
GPIO_TypeDef *gpio; #endif
uint16_t scl; #ifndef I2C2_SDA
uint16_t sda; #define I2C2_SDA PB11
uint8_t ev_irq; #endif
uint8_t er_irq;
uint32_t peripheral;
} i2cDevice_t;
static const i2cDevice_t i2cHardwareMap[] = { static i2cDevice_t i2cHardwareMap[] = {
{ I2C1, GPIOB, Pin_6, Pin_7, I2C1_EV_IRQn, I2C1_ER_IRQn, RCC_APB1Periph_I2C1 }, { .dev = I2C1, .scl = IO_TAG(I2C1_SCL), .sda = IO_TAG(I2C1_SDA), .rcc = RCC_APB1(I2C1), .overClock = false, .ev_irq = I2C1_EV_IRQn, .er_irq = I2C1_ER_IRQn },
{ I2C2, GPIOB, Pin_10, Pin_11, I2C2_EV_IRQn, I2C2_ER_IRQn, RCC_APB1Periph_I2C2 }, { .dev = I2C2, .scl = IO_TAG(I2C2_SCL), .sda = IO_TAG(I2C2_SDA), .rcc = RCC_APB1(I2C2), .overClock = false, .ev_irq = I2C2_EV_IRQn, .er_irq = I2C2_ER_IRQn },
#ifdef STM32F4
{ .dev = I2C3, .scl = IO_TAG(I2C3_SCL), .sda = IO_TAG(I2C3_SDA), .rcc = RCC_APB1(I2C3), .overClock = false, .ev_irq = I2C3_EV_IRQn, .er_irq = I2C3_ER_IRQn }
#endif
}; };
// Copy of peripheral address for IRQ routines
static I2C_TypeDef *I2Cx = NULL;
// Copy of device index for reinit, etc purposes // Copy of device index for reinit, etc purposes
static I2CDevice I2Cx_index; static I2CDevice I2Cx_device;
void I2C1_ER_IRQHandler(void)
{
i2c_er_handler();
}
void I2C1_EV_IRQHandler(void)
{
i2c_ev_handler();
}
void I2C2_ER_IRQHandler(void)
{
i2c_er_handler();
}
void I2C2_EV_IRQHandler(void)
{
i2c_ev_handler();
}
//#define I2C_DEFAULT_TIMEOUT 30000
static volatile uint16_t i2cErrorCount = 0; static volatile uint16_t i2cErrorCount = 0;
static volatile bool error = false; static i2cState_t i2cState[] = {
static volatile bool busy; { false, false, 0, 0, 0, 0, 0, 0, 0 },
{ false, false, 0, 0, 0, 0, 0, 0, 0 },
{ false, false, 0, 0, 0, 0, 0, 0, 0 }
};
static volatile uint8_t addr; void I2C1_ER_IRQHandler(void) {
static volatile uint8_t reg; i2c_er_handler(I2CDEV_1);
static volatile uint8_t bytes; }
static volatile uint8_t writing;
static volatile uint8_t reading;
static volatile uint8_t* write_p;
static volatile uint8_t* read_p;
static bool i2cHandleHardwareFailure(void) void I2C1_EV_IRQHandler(void) {
i2c_ev_handler(I2CDEV_1);
}
void I2C2_ER_IRQHandler(void) {
i2c_er_handler(I2CDEV_2);
}
void I2C2_EV_IRQHandler(void) {
i2c_ev_handler(I2CDEV_2);
}
#ifdef STM32F4
void I2C3_ER_IRQHandler(void) {
i2c_er_handler(I2CDEV_3);
}
void I2C3_EV_IRQHandler(void) {
i2c_ev_handler(I2CDEV_3);
}
#endif
static bool i2cHandleHardwareFailure(I2CDevice device)
{ {
i2cErrorCount++; i2cErrorCount++;
// reinit peripheral + clock out garbage // reinit peripheral + clock out garbage
i2cInit(I2Cx_index); i2cInit(device);
return false; return false;
} }
bool i2cWriteBuffer(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data) bool i2cWriteBufferByDevice(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data)
{ {
uint32_t timeout = I2C_DEFAULT_TIMEOUT;
if (device == I2CINVALID)
addr = addr_ << 1;
reg = reg_;
writing = 1;
reading = 0;
write_p = data;
read_p = data;
bytes = len_;
busy = 1;
error = false;
if (!I2Cx)
return false; return false;
if (!(I2Cx->CR2 & I2C_IT_EVT)) { // if we are restarting the driver
if (!(I2Cx->CR1 & 0x0100)) { // ensure sending a start
while (I2Cx->CR1 & 0x0200 && --timeout > 0) { ; } // wait for any stop to finish sending
if (timeout == 0) {
return i2cHandleHardwareFailure();
}
I2C_GenerateSTART(I2Cx, ENABLE); // send the start for the new job
}
I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, ENABLE); // allow the interrupts to fire off again
}
timeout = I2C_DEFAULT_TIMEOUT;
while (busy && --timeout > 0) { ; }
if (timeout == 0) {
return i2cHandleHardwareFailure();
}
return !error;
}
bool i2cWrite(uint8_t addr_, uint8_t reg_, uint8_t data)
{
return i2cWriteBuffer(addr_, reg_, 1, &data);
}
bool i2cRead(uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf)
{
uint32_t timeout = I2C_DEFAULT_TIMEOUT; uint32_t timeout = I2C_DEFAULT_TIMEOUT;
addr = addr_ << 1; I2C_TypeDef *I2Cx;
reg = reg_; I2Cx = i2cHardwareMap[device].dev;
writing = 0;
reading = 1; i2cState_t *state;
read_p = buf; state = &(i2cState[device]);
write_p = buf;
bytes = len; state->addr = addr_ << 1;
busy = 1; state->reg = reg_;
error = false; state->writing = 1;
state->reading = 0;
if (!I2Cx) state->write_p = data;
return false; state->read_p = data;
state->bytes = len_;
state->busy = 1;
state->error = false;
if (!(I2Cx->CR2 & I2C_IT_EVT)) { // if we are restarting the driver if (!(I2Cx->CR2 & I2C_IT_EVT)) { // if we are restarting the driver
if (!(I2Cx->CR1 & 0x0100)) { // ensure sending a start if (!(I2Cx->CR1 & I2C_CR1_START)) { // ensure sending a start
while (I2Cx->CR1 & 0x0200 && --timeout > 0) { ; } // wait for any stop to finish sending while (I2Cx->CR1 & I2C_CR1_STOP && --timeout > 0) {; } // wait for any stop to finish sending
if (timeout == 0) if (timeout == 0)
return i2cHandleHardwareFailure(); return i2cHandleHardwareFailure(device);
I2C_GenerateSTART(I2Cx, ENABLE); // send the start for the new job I2C_GenerateSTART(I2Cx, ENABLE); // send the start for the new job
} }
I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, ENABLE); // allow the interrupts to fire off again I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, ENABLE); // allow the interrupts to fire off again
} }
timeout = I2C_DEFAULT_TIMEOUT; timeout = I2C_DEFAULT_TIMEOUT;
while (busy && --timeout > 0) { ; } while (state->busy && --timeout > 0) {; }
if (timeout == 0) if (timeout == 0)
return i2cHandleHardwareFailure(); return i2cHandleHardwareFailure(device);
return !error; return !(state->error);
} }
static void i2c_er_handler(void) bool i2cWriteByDevice(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t data)
{ {
// Read the I2Cx status register return i2cWriteBufferByDevice(device, addr_, reg_, 1, &data);
uint32_t SR1Register = I2Cx->SR1; }
if (SR1Register & 0x0F00) { // an error bool i2cReadByDevice(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf)
error = true; {
if (device == I2CINVALID)
return false;
uint32_t timeout = I2C_DEFAULT_TIMEOUT;
I2C_TypeDef *I2Cx;
I2Cx = i2cHardwareMap[device].dev;
i2cState_t *state;
state = &(i2cState[device]);
state->addr = addr_ << 1;
state->reg = reg_;
state->writing = 0;
state->reading = 1;
state->read_p = buf;
state->write_p = buf;
state->bytes = len;
state->busy = 1;
state->error = false;
if (!(I2Cx->CR2 & I2C_IT_EVT)) { // if we are restarting the driver
if (!(I2Cx->CR1 & I2C_CR1_START)) { // ensure sending a start
while (I2Cx->CR1 & I2C_CR1_STOP && --timeout > 0) {; } // wait for any stop to finish sending
if (timeout == 0)
return i2cHandleHardwareFailure(device);
I2C_GenerateSTART(I2Cx, ENABLE); // send the start for the new job
}
I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, ENABLE); // allow the interrupts to fire off again
} }
timeout = I2C_DEFAULT_TIMEOUT;
while (state->busy && --timeout > 0) {; }
if (timeout == 0)
return i2cHandleHardwareFailure(device);
return !(state->error);
}
static void i2c_er_handler(I2CDevice device) {
I2C_TypeDef *I2Cx;
I2Cx = i2cHardwareMap[device].dev;
i2cState_t *state;
state = &(i2cState[device]);
// Read the I2C1 status register
volatile uint32_t SR1Register = I2Cx->SR1;
if (SR1Register & 0x0F00) // an error
state->error = true;
// If AF, BERR or ARLO, abandon the current job and commence new if there are jobs // If AF, BERR or ARLO, abandon the current job and commence new if there are jobs
if (SR1Register & 0x0700) { if (SR1Register & 0x0700) {
(void)I2Cx->SR2; // read second status register to clear ADDR if it is set (note that BTF will not be set after a NACK) (void)I2Cx->SR2; // read second status register to clear ADDR if it is set (note that BTF will not be set after a NACK)
I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // disable the RXNE/TXE interrupt - prevent the ISR tailchaining onto the ER (hopefully) I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // disable the RXNE/TXE interrupt - prevent the ISR tailchaining onto the ER (hopefully)
if (!(SR1Register & 0x0200) && !(I2Cx->CR1 & 0x0200)) { // if we dont have an ARLO error, ensure sending of a stop if (!(SR1Register & I2C_SR1_ARLO) && !(I2Cx->CR1 & I2C_CR1_STOP)) { // if we dont have an ARLO error, ensure sending of a stop
if (I2Cx->CR1 & 0x0100) { // We are currently trying to send a start, this is very bad as start, stop will hang the peripheral if (I2Cx->CR1 & I2C_CR1_START) { // We are currently trying to send a start, this is very bad as start, stop will hang the peripheral
// TODO - busy waiting in highest priority IRQ. Maybe only set flag and handle it from main loop while (I2Cx->CR1 & I2C_CR1_START) {; } // wait for any start to finish sending
while (I2Cx->CR1 & 0x0100) { ; } // wait for any start to finish sending I2C_GenerateSTOP(I2Cx, ENABLE); // send stop to finalise bus transaction
I2C_GenerateSTOP(I2Cx, ENABLE); // send stop to finalise bus transaction while (I2Cx->CR1 & I2C_CR1_STOP) {; } // wait for stop to finish sending
while (I2Cx->CR1 & 0x0200) { ; } // wait for stop to finish sending i2cInit(device); // reset and configure the hardware
i2cInit(I2Cx_index); // reset and configure the hardware }
} else { else {
I2C_GenerateSTOP(I2Cx, ENABLE); // stop to free up the bus I2C_GenerateSTOP(I2Cx, ENABLE); // stop to free up the bus
I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE); // Disable EVT and ERR interrupts while bus inactive I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE); // Disable EVT and ERR interrupts while bus inactive
} }
} }
} }
I2Cx->SR1 &= ~0x0F00; // reset all the error bits to clear the interrupt I2Cx->SR1 &= ~0x0F00; // reset all the error bits to clear the interrupt
busy = 0; state->busy = 0;
} }
void i2c_ev_handler(void) void i2c_ev_handler(I2CDevice device) {
{
static uint8_t subaddress_sent, final_stop; // flag to indicate if subaddess sent, flag to indicate final bus condition I2C_TypeDef *I2Cx;
static int8_t index; // index is signed -1 == send the subaddress I2Cx = i2cHardwareMap[device].dev;
uint8_t SReg_1 = I2Cx->SR1; // read the status register here
i2cState_t *state;
state = &(i2cState[device]);
static uint8_t subaddress_sent, final_stop; // flag to indicate if subaddess sent, flag to indicate final bus condition
static int8_t index; // index is signed -1 == send the subaddress
uint8_t SReg_1 = I2Cx->SR1; // read the status register here
if (SReg_1 & 0x0001) { // we just sent a start - EV5 in ref manual if (SReg_1 & I2C_SR1_SB) { // we just sent a start - EV5 in ref manual
I2Cx->CR1 &= ~0x0800; // reset the POS bit so ACK/NACK applied to the current byte I2Cx->CR1 &= ~I2C_CR1_POS; // reset the POS bit so ACK/NACK applied to the current byte
I2C_AcknowledgeConfig(I2Cx, ENABLE); // make sure ACK is on I2C_AcknowledgeConfig(I2Cx, ENABLE); // make sure ACK is on
index = 0; // reset the index index = 0; // reset the index
if (reading && (subaddress_sent || 0xFF == reg)) { // we have sent the subaddr if (state->reading && (subaddress_sent || 0xFF == state->reg)) { // we have sent the subaddr
subaddress_sent = 1; // make sure this is set in case of no subaddress, so following code runs correctly subaddress_sent = 1; // make sure this is set in case of no subaddress, so following code runs correctly
if (bytes == 2) if (state->bytes == 2)
I2Cx->CR1 |= 0x0800; // set the POS bit so NACK applied to the final byte in the two byte read I2Cx->CR1 |= I2C_CR1_POS; // set the POS bit so NACK applied to the final byte in the two byte read
I2C_Send7bitAddress(I2Cx, addr, I2C_Direction_Receiver); // send the address and set hardware mode I2C_Send7bitAddress(I2Cx, state->addr, I2C_Direction_Receiver); // send the address and set hardware mode
} else { // direction is Tx, or we havent sent the sub and rep start
I2C_Send7bitAddress(I2Cx, addr, I2C_Direction_Transmitter); // send the address and set hardware mode
if (reg != 0xFF) // 0xFF as subaddress means it will be ignored, in Tx or Rx mode
index = -1; // send a subaddress
} }
} else if (SReg_1 & 0x0002) { // we just sent the address - EV6 in ref manual else { // direction is Tx, or we havent sent the sub and rep start
I2C_Send7bitAddress(I2Cx, state->addr, I2C_Direction_Transmitter); // send the address and set hardware mode
if (state->reg != 0xFF) // 0xFF as subaddress means it will be ignored, in Tx or Rx mode
index = -1; // send a subaddress
}
}
else if (SReg_1 & I2C_SR1_ADDR) { // we just sent the address - EV6 in ref manual
// Read SR1,2 to clear ADDR // Read SR1,2 to clear ADDR
__DMB(); // memory fence to control hardware __DMB(); // memory fence to control hardware
if (bytes == 1 && reading && subaddress_sent) { // we are receiving 1 byte - EV6_3 if (state->bytes == 1 && state->reading && subaddress_sent) { // we are receiving 1 byte - EV6_3
I2C_AcknowledgeConfig(I2Cx, DISABLE); // turn off ACK I2C_AcknowledgeConfig(I2Cx, DISABLE); // turn off ACK
__DMB(); __DMB();
(void)I2Cx->SR2; // clear ADDR after ACK is turned off (void)I2Cx->SR2; // clear ADDR after ACK is turned off
I2C_GenerateSTOP(I2Cx, ENABLE); // program the stop I2C_GenerateSTOP(I2Cx, ENABLE); // program the stop
final_stop = 1; final_stop = 1;
I2C_ITConfig(I2Cx, I2C_IT_BUF, ENABLE); // allow us to have an EV7 I2C_ITConfig(I2Cx, I2C_IT_BUF, ENABLE); // allow us to have an EV7
} else { // EV6 and EV6_1 }
else { // EV6 and EV6_1
(void)I2Cx->SR2; // clear the ADDR here (void)I2Cx->SR2; // clear the ADDR here
__DMB(); __DMB();
if (bytes == 2 && reading && subaddress_sent) { // rx 2 bytes - EV6_1 if (state->bytes == 2 && state->reading && subaddress_sent) { // rx 2 bytes - EV6_1
I2C_AcknowledgeConfig(I2Cx, DISABLE); // turn off ACK I2C_AcknowledgeConfig(I2Cx, DISABLE); // turn off ACK
I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // disable TXE to allow the buffer to fill I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // disable TXE to allow the buffer to fill
} else if (bytes == 3 && reading && subaddress_sent) // rx 3 bytes }
I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // make sure RXNE disabled so we get a BTF in two bytes time else if (state->bytes == 3 && state->reading && subaddress_sent) // rx 3 bytes
else // receiving greater than three bytes, sending subaddress, or transmitting I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // make sure RXNE disabled so we get a BTF in two bytes time
else // receiving greater than three bytes, sending subaddress, or transmitting
I2C_ITConfig(I2Cx, I2C_IT_BUF, ENABLE); I2C_ITConfig(I2Cx, I2C_IT_BUF, ENABLE);
} }
} else if (SReg_1 & 0x004) { // Byte transfer finished - EV7_2, EV7_3 or EV8_2 }
else if (SReg_1 & I2C_SR1_BTF) { // Byte transfer finished - EV7_2, EV7_3 or EV8_2
final_stop = 1; final_stop = 1;
if (reading && subaddress_sent) { // EV7_2, EV7_3 if (state->reading && subaddress_sent) { // EV7_2, EV7_3
if (bytes > 2) { // EV7_2 if (state->bytes > 2) { // EV7_2
I2C_AcknowledgeConfig(I2Cx, DISABLE); // turn off ACK I2C_AcknowledgeConfig(I2Cx, DISABLE); // turn off ACK
read_p[index++] = (uint8_t)I2Cx->DR; // read data N-2 state->read_p[index++] = (uint8_t)I2Cx->DR; // read data N-2
I2C_GenerateSTOP(I2Cx, ENABLE); // program the Stop I2C_GenerateSTOP(I2Cx, ENABLE); // program the Stop
final_stop = 1; // required to fix hardware final_stop = 1; // required to fix hardware
read_p[index++] = (uint8_t)I2Cx->DR; // read data N - 1 state->read_p[index++] = (uint8_t)I2Cx->DR; // read data N - 1
I2C_ITConfig(I2Cx, I2C_IT_BUF, ENABLE); // enable TXE to allow the final EV7 I2C_ITConfig(I2Cx, I2C_IT_BUF, ENABLE); // enable TXE to allow the final EV7
} else { // EV7_3 }
else { // EV7_3
if (final_stop) if (final_stop)
I2C_GenerateSTOP(I2Cx, ENABLE); // program the Stop I2C_GenerateSTOP(I2Cx, ENABLE); // program the Stop
else else
I2C_GenerateSTART(I2Cx, ENABLE); // program a rep start I2C_GenerateSTART(I2Cx, ENABLE); // program a rep start
read_p[index++] = (uint8_t)I2Cx->DR; // read data N - 1 state->read_p[index++] = (uint8_t)I2Cx->DR; // read data N - 1
read_p[index++] = (uint8_t)I2Cx->DR; // read data N state->read_p[index++] = (uint8_t)I2Cx->DR; // read data N
index++; // to show job completed index++; // to show job completed
} }
} else { // EV8_2, which may be due to a subaddress sent or a write completion }
if (subaddress_sent || (writing)) { else { // EV8_2, which may be due to a subaddress sent or a write completion
if (subaddress_sent || (state->writing)) {
if (final_stop) if (final_stop)
I2C_GenerateSTOP(I2Cx, ENABLE); // program the Stop I2C_GenerateSTOP(I2Cx, ENABLE); // program the Stop
else else
I2C_GenerateSTART(I2Cx, ENABLE); // program a rep start I2C_GenerateSTART(I2Cx, ENABLE); // program a rep start
index++; // to show that the job is complete index++; // to show that the job is complete
} else { // We need to send a subaddress }
else { // We need to send a subaddress
I2C_GenerateSTART(I2Cx, ENABLE); // program the repeated Start I2C_GenerateSTART(I2Cx, ENABLE); // program the repeated Start
subaddress_sent = 1; // this is set back to zero upon completion of the current task subaddress_sent = 1; // this is set back to zero upon completion of the current task
} }
} }
// TODO - busy waiting in ISR
// we must wait for the start to clear, otherwise we get constant BTF // we must wait for the start to clear, otherwise we get constant BTF
while (I2Cx->CR1 & 0x0100) { ; } while (I2Cx->CR1 & 0x0100) {; }
} else if (SReg_1 & 0x0040) { // Byte received - EV7 }
read_p[index++] = (uint8_t)I2Cx->DR; else if (SReg_1 & I2C_SR1_RXNE) { // Byte received - EV7
if (bytes == (index + 3)) state->read_p[index++] = (uint8_t)I2Cx->DR;
if (state->bytes == (index + 3))
I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // disable TXE to allow the buffer to flush so we can get an EV7_2 I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // disable TXE to allow the buffer to flush so we can get an EV7_2
if (bytes == index) // We have completed a final EV7 if (state->bytes == index) // We have completed a final EV7
index++; // to show job is complete index++; // to show job is complete
} else if (SReg_1 & 0x0080) { // Byte transmitted EV8 / EV8_1 }
else if (SReg_1 & I2C_SR1_TXE) { // Byte transmitted EV8 / EV8_1
if (index != -1) { // we dont have a subaddress to send if (index != -1) { // we dont have a subaddress to send
I2Cx->DR = write_p[index++]; I2Cx->DR = state->write_p[index++];
if (bytes == index) // we have sent all the data if (state->bytes == index) // we have sent all the data
I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // disable TXE to allow the buffer to flush I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // disable TXE to allow the buffer to flush
} else { }
else {
index++; index++;
I2Cx->DR = reg; // send the subaddress I2Cx->DR = state->reg; // send the subaddress
if (reading || !bytes) // if receiving or sending 0 bytes, flush now if (state->reading || !(state->bytes)) // if receiving or sending 0 bytes, flush now
I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // disable TXE to allow the buffer to flush I2C_ITConfig(I2Cx, I2C_IT_BUF, DISABLE); // disable TXE to allow the buffer to flush
} }
} }
if (index == bytes + 1) { // we have completed the current job if (index == state->bytes + 1) { // we have completed the current job
subaddress_sent = 0; // reset this here subaddress_sent = 0; // reset this here
if (final_stop) // If there is a final stop and no more jobs, bus is inactive, disable interrupts to prevent BTF if (final_stop) // If there is a final stop and no more jobs, bus is inactive, disable interrupts to prevent BTF
I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE); // Disable EVT and ERR interrupts while bus inactive I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE); // Disable EVT and ERR interrupts while bus inactive
busy = 0; state->busy = 0;
} }
} }
void i2cInit(I2CDevice index) void i2cInit(I2CDevice device)
{ {
if (device == I2CINVALID)
return;
i2cDevice_t *i2c;
i2c = &(i2cHardwareMap[device]);
NVIC_InitTypeDef nvic; NVIC_InitTypeDef nvic;
I2C_InitTypeDef i2c; I2C_InitTypeDef i2cInit;
if (index > I2CDEV_MAX) IO_t scl = IOGetByTag(i2c->scl);
index = I2CDEV_MAX; IO_t sda = IOGetByTag(i2c->sda);
IOInit(scl, OWNER_SYSTEM, RESOURCE_I2C);
IOInit(sda, OWNER_SYSTEM, RESOURCE_I2C);
// Turn on peripheral clock, save device and index // Enable RCC
I2Cx = i2cHardwareMap[index].dev; RCC_ClockCmd(i2c->rcc, ENABLE);
I2Cx_index = index;
RCC_APB1PeriphClockCmd(i2cHardwareMap[index].peripheral, ENABLE); I2C_ITConfig(i2c->dev, I2C_IT_EVT | I2C_IT_ERR, DISABLE);
i2cUnstick(scl, sda);
// Init pins
#if defined(STM32F40_41xxx) || defined(STM32F411xE)
IOConfigGPIOAF(scl, IOCFG_I2C, GPIO_AF_I2C);
IOConfigGPIOAF(sda, IOCFG_I2C, GPIO_AF_I2C);
#else
IOConfigGPIO(scl, IOCFG_AF_OD);
IOConfigGPIO(sda, IOCFG_AF_OD);
#endif
I2C_DeInit(i2c->dev);
I2C_StructInit(&i2cInit);
I2C_ITConfig(i2c->dev, I2C_IT_EVT | I2C_IT_ERR, DISABLE); // Disable EVT and ERR interrupts - they are enabled by the first request
i2cInit.I2C_Mode = I2C_Mode_I2C;
i2cInit.I2C_DutyCycle = I2C_DutyCycle_2;
i2cInit.I2C_OwnAddress1 = 0;
i2cInit.I2C_Ack = I2C_Ack_Enable;
i2cInit.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
// diable I2C interrrupts first to avoid ER handler triggering if (i2c->overClock) {
I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE); i2cInit.I2C_ClockSpeed = 800000; // 800khz Maximum speed tested on various boards without issues
}
// clock out stuff to make sure slaves arent stuck else {
// This will also configure GPIO as AF_OD at the end i2cInit.I2C_ClockSpeed = 400000; // 400khz Operation according specs
i2cUnstick(); }
// Init I2C peripheral
I2C_DeInit(I2Cx);
I2C_StructInit(&i2c);
I2C_ITConfig(I2Cx, I2C_IT_EVT | I2C_IT_ERR, DISABLE); // Disable EVT and ERR interrupts - they are enabled by the first request
i2c.I2C_Mode = I2C_Mode_I2C;
i2c.I2C_DutyCycle = I2C_DutyCycle_2;
i2c.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
// Overclocking i2c, test results
// Default speed, conform specs is 400000 (400 kHz)
// 2.0* : 800kHz - worked without errors
// 3.0* : 1200kHz - worked without errors
// 3.5* : 1400kHz - failed, hangup, bootpin recovery needed
// 4.0* : 1600kHz - failed, hangup, bootpin recovery needed
i2c.I2C_ClockSpeed = CLOCKSPEED;
I2C_Cmd(I2Cx, ENABLE);
I2C_Init(I2Cx, &i2c);
I2C_Cmd(i2c->dev, ENABLE);
I2C_Init(i2c->dev, &i2cInit);
// I2C ER Interrupt // I2C ER Interrupt
nvic.NVIC_IRQChannel = i2cHardwareMap[index].er_irq; nvic.NVIC_IRQChannel = i2c->er_irq;
nvic.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_ER); nvic.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_ER);
nvic.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_ER); nvic.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_ER);
nvic.NVIC_IRQChannelCmd = ENABLE; nvic.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&nvic); NVIC_Init(&nvic);
// I2C EV Interrupt // I2C EV Interrupt
nvic.NVIC_IRQChannel = i2cHardwareMap[index].ev_irq; nvic.NVIC_IRQChannel = i2c->ev_irq;
nvic.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_EV); nvic.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_I2C_EV);
nvic.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_EV); nvic.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_I2C_EV);
NVIC_Init(&nvic); NVIC_Init(&nvic);
@ -373,54 +425,54 @@ uint16_t i2cGetErrorCounter(void)
return i2cErrorCount; return i2cErrorCount;
} }
static void i2cUnstick(void) static void i2cUnstick(IO_t scl, IO_t sda)
{ {
GPIO_TypeDef *gpio;
gpio_config_t cfg;
uint16_t scl, sda;
int i; int i;
int timeout = 100;
// prepare pins IOHi(scl);
gpio = i2cHardwareMap[I2Cx_index].gpio; IOHi(sda);
scl = i2cHardwareMap[I2Cx_index].scl;
sda = i2cHardwareMap[I2Cx_index].sda;
digitalHi(gpio, scl | sda); IOConfigGPIO(scl, IOCFG_OUT_OD);
IOConfigGPIO(sda, IOCFG_OUT_OD);
cfg.pin = scl | sda;
cfg.speed = Speed_2MHz;
cfg.mode = Mode_Out_OD;
gpioInit(gpio, &cfg);
for (i = 0; i < 8; i++) { for (i = 0; i < 8; i++) {
// Wait for any clock stretching to finish // Wait for any clock stretching to finish
while (!digitalIn(gpio, scl)) while (!IORead(scl) && timeout) {
delayMicroseconds(10); delayMicroseconds(10);
timeout--;
}
// Pull low // Pull low
digitalLo(gpio, scl); // Set bus low IOLo(scl); // Set bus low
delayMicroseconds(10); delayMicroseconds(10);
// Release high again IOHi(scl); // Set bus high
digitalHi(gpio, scl); // Set bus high
delayMicroseconds(10); delayMicroseconds(10);
} }
// Generate a start then stop condition // Generate a start then stop condition
// SCL PB10 IOLo(sda); // Set bus data low
// SDA PB11
digitalLo(gpio, sda); // Set bus data low
delayMicroseconds(10); delayMicroseconds(10);
digitalLo(gpio, scl); // Set bus scl low IOLo(scl); // Set bus scl low
delayMicroseconds(10); delayMicroseconds(10);
digitalHi(gpio, scl); // Set bus scl high IOHi(scl); // Set bus scl high
delayMicroseconds(10); delayMicroseconds(10);
digitalHi(gpio, sda); // Set bus sda high IOHi(sda); // Set bus sda high
}
// Init pins bool i2cWriteBuffer(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data)
cfg.pin = scl | sda; {
cfg.speed = Speed_2MHz; return i2cWriteBufferByDevice(I2Cx_device, addr_, reg_, len_, data);
cfg.mode = Mode_AF_OD; }
gpioInit(gpio, &cfg);
bool i2cWrite(uint8_t addr_, uint8_t reg, uint8_t data)
{
return i2cWriteByDevice(I2Cx_device, addr_, reg, data);
}
bool i2cRead(uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* buf)
{
return i2cReadByDevice(I2Cx_device, addr_, reg, len, buf);
} }
#endif #endif

View file

@ -18,25 +18,13 @@
#pragma once #pragma once
#include "drivers/io.h"
// old EXTI interface, to be replaced // old EXTI interface, to be replaced
typedef struct extiConfig_s { typedef struct extiConfig_s {
#ifdef STM32F303 ioTag_t io;
uint32_t gpioAHBPeripherals;
#endif
#ifdef STM32F10X
uint32_t gpioAPB2Peripherals;
#endif
uint16_t gpioPin;
GPIO_TypeDef *gpioPort;
uint8_t exti_port_source;
uint32_t exti_line;
uint8_t exti_pin_source;
IRQn_Type exti_irqn;
} extiConfig_t; } extiConfig_t;
// new io EXTI interface
#include "drivers/io.h"
typedef struct extiCallbackRec_s extiCallbackRec_t; typedef struct extiCallbackRec_s extiCallbackRec_t;
typedef void extiHandlerCallback(extiCallbackRec_t *self); typedef void extiHandlerCallback(extiCallbackRec_t *self);

View file

@ -1,23 +1,24 @@
#pragma once #pragma once
#include <stdbool.h> #include <stdbool.h>
#include <stdint.h>
#include "drivers/gpio.h" #include "resource.h"
#include "drivers/resource.h"
// IO pin identification // IO pin identification
// make sure that ioTag_t can't be assigned into IO_t without warning // make sure that ioTag_t can't be assigned into IO_t without warning
typedef uint8_t ioTag_t; // packet tag to specify IO pin typedef uint8_t ioTag_t; // packet tag to specify IO pin
typedef void* IO_t; // type specifying IO pin. Currently ioRec_t pointer, but this may change typedef void* IO_t; // type specifying IO pin. Currently ioRec_t pointer, but this may change
// NONE initializer for IO_t variable
#define IO_NONE ((IO_t)0)
// preprocessor is used to convert pinid to requested C data value // preprocessor is used to convert pinid to requested C data value
// compile-time error is generated if requested pin is not available (not set in TARGET_IO_PORTx) // compile-time error is generated if requested pin is not available (not set in TARGET_IO_PORTx)
// ioTag_t and IO_t is supported, but ioTag_t is preferred // ioTag_t and IO_t is supported, but ioTag_t is preferred
// expand pinid to ioTag_t, generate compilation error if pin is not supported // expand pinid to to ioTag_t
#define IO_TAG(pinid) DEFIO_TAG(pinid) #define IO_TAG(pinid) DEFIO_TAG(pinid)
// expand pinid to ioTag_t, expand to NONE if pin is not supported
#define IO_TAG_E(pinid) DEFIO_TAG_E(pinid)
// both ioTag_t and IO_t are guarantied to be zero if pinid is NONE (no pin) // both ioTag_t and IO_t are guarantied to be zero if pinid is NONE (no pin)
// this simplifies initialization (globals are zeroed on start) and allows // this simplifies initialization (globals are zeroed on start) and allows
@ -44,7 +45,7 @@ typedef uint8_t ioConfig_t; // packed IO configuration
# define IOCFG_IPD IO_CONFIG(GPIO_Mode_IPD, GPIO_Speed_2MHz) # define IOCFG_IPD IO_CONFIG(GPIO_Mode_IPD, GPIO_Speed_2MHz)
# define IOCFG_IPU IO_CONFIG(GPIO_Mode_IPU, GPIO_Speed_2MHz) # define IOCFG_IPU IO_CONFIG(GPIO_Mode_IPU, GPIO_Speed_2MHz)
# define IOCFG_IN_FLOATING IO_CONFIG(GPIO_Mode_IN_FLOATING, GPIO_Speed_2MHz) # define IOCFG_IN_FLOATING IO_CONFIG(GPIO_Mode_IN_FLOATING, GPIO_Speed_2MHz)
# define IOCFG_ANALOG IO_CONFIG(GPIO_Mode_AIN, GPIO_Speed_2MHz)
#elif defined(STM32F303xC) #elif defined(STM32F303xC)
# define IO_CONFIG(mode, speed, otype, pupd) ((mode) | ((speed) << 2) | ((otype) << 4) | ((pupd) << 5)) # define IO_CONFIG(mode, speed, otype, pupd) ((mode) | ((speed) << 2) | ((otype) << 4) | ((pupd) << 5))
@ -56,7 +57,18 @@ typedef uint8_t ioConfig_t; // packed IO configuration
# define IOCFG_IPD IO_CONFIG(GPIO_Mode_IN, 0, 0, GPIO_PuPd_DOWN) # define IOCFG_IPD IO_CONFIG(GPIO_Mode_IN, 0, 0, GPIO_PuPd_DOWN)
# define IOCFG_IPU IO_CONFIG(GPIO_Mode_IN, 0, 0, GPIO_PuPd_UP) # define IOCFG_IPU IO_CONFIG(GPIO_Mode_IN, 0, 0, GPIO_PuPd_UP)
# define IOCFG_IN_FLOATING IO_CONFIG(GPIO_Mode_IN, 0, 0, GPIO_PuPd_NOPULL) # define IOCFG_IN_FLOATING IO_CONFIG(GPIO_Mode_IN, 0, 0, GPIO_PuPd_NOPULL)
# define IOCFG_ANALOG IO_CONFIG(GPIO_Mode_AN, 0, 0, GPIO_PuPd_NOPULL)
#elif defined(STM32F40_41xxx) || defined(STM32F411xE)
# define IO_CONFIG(mode, speed, otype, pupd) ((mode) | ((speed) << 2) | ((otype) << 4) | ((pupd) << 5))
# define IOCFG_OUT_PP IO_CONFIG(GPIO_Mode_OUT, 0, GPIO_OType_PP, GPIO_PuPd_NOPULL) // TODO
# define IOCFG_OUT_OD IO_CONFIG(GPIO_Mode_OUT, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)
# define IOCFG_AF_PP IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_PP, GPIO_PuPd_NOPULL)
# define IOCFG_AF_OD IO_CONFIG(GPIO_Mode_AF, 0, GPIO_OType_OD, GPIO_PuPd_NOPULL)
# define IOCFG_IPD IO_CONFIG(GPIO_Mode_IN, 0, 0, GPIO_PuPd_DOWN)
# define IOCFG_IPU IO_CONFIG(GPIO_Mode_IN, 0, 0, GPIO_PuPd_UP)
# define IOCFG_IN_FLOATING IO_CONFIG(GPIO_Mode_IN, 0, 0, GPIO_PuPd_NOPULL)
#elif defined(UNIT_TEST) #elif defined(UNIT_TEST)
@ -88,7 +100,7 @@ resourceType_t IOGetResources(IO_t io);
IO_t IOGetByTag(ioTag_t tag); IO_t IOGetByTag(ioTag_t tag);
void IOConfigGPIO(IO_t io, ioConfig_t cfg); void IOConfigGPIO(IO_t io, ioConfig_t cfg);
#if defined(STM32F303xC) #if defined(STM32F303xC) || defined(STM32F40_41xxx) || defined(STM32F411xE)
void IOConfigGPIOAF(IO_t io, ioConfig_t cfg, uint8_t af); void IOConfigGPIOAF(IO_t io, ioConfig_t cfg, uint8_t af);
#endif #endif

View file

@ -6,10 +6,11 @@
// can't use 0 // can't use 0
#define NVIC_PRIO_MAX NVIC_BUILD_PRIORITY(0, 1) #define NVIC_PRIO_MAX NVIC_BUILD_PRIORITY(0, 1)
#define NVIC_PRIO_TIMER NVIC_BUILD_PRIORITY(1, 1) #define NVIC_PRIO_TIMER NVIC_BUILD_PRIORITY(1, 1)
#define NVIC_PRIO_BARO_EXT NVIC_BUILD_PRIORITY(0x0f, 0x0f) #define NVIC_PRIO_BARO_EXTI NVIC_BUILD_PRIORITY(0x0f, 0x0f)
#define NVIC_PRIO_SONAR_EXTI NVIC_BUILD_PRIORITY(2, 0) // maybe increate slightly
#define NVIC_PRIO_MPU_INT_EXTI NVIC_BUILD_PRIORITY(0x0f, 0x0f)
#define NVIC_PRIO_MAG_INT_EXTI NVIC_BUILD_PRIORITY(0x0f, 0x0f)
#define NVIC_PRIO_WS2811_DMA NVIC_BUILD_PRIORITY(1, 2) // TODO - is there some reason to use high priority? (or to use DMA IRQ at all?) #define NVIC_PRIO_WS2811_DMA NVIC_BUILD_PRIORITY(1, 2) // TODO - is there some reason to use high priority? (or to use DMA IRQ at all?)
#define NVIC_PRIO_BST_READ_DATA NVIC_BUILD_PRIORITY(0x0f, 0x0f)
#define NVIC_PRIO_TRANSPONDER_DMA NVIC_BUILD_PRIORITY(3, 0)
#define NVIC_PRIO_SERIALUART1_TXDMA NVIC_BUILD_PRIORITY(1, 1) #define NVIC_PRIO_SERIALUART1_TXDMA NVIC_BUILD_PRIORITY(1, 1)
#define NVIC_PRIO_SERIALUART1_RXDMA NVIC_BUILD_PRIORITY(1, 1) #define NVIC_PRIO_SERIALUART1_RXDMA NVIC_BUILD_PRIORITY(1, 1)
#define NVIC_PRIO_SERIALUART1 NVIC_BUILD_PRIORITY(1, 1) #define NVIC_PRIO_SERIALUART1 NVIC_BUILD_PRIORITY(1, 1)
@ -19,6 +20,15 @@
#define NVIC_PRIO_SERIALUART3_TXDMA NVIC_BUILD_PRIORITY(1, 0) #define NVIC_PRIO_SERIALUART3_TXDMA NVIC_BUILD_PRIORITY(1, 0)
#define NVIC_PRIO_SERIALUART3_RXDMA NVIC_BUILD_PRIORITY(1, 1) #define NVIC_PRIO_SERIALUART3_RXDMA NVIC_BUILD_PRIORITY(1, 1)
#define NVIC_PRIO_SERIALUART3 NVIC_BUILD_PRIORITY(1, 2) #define NVIC_PRIO_SERIALUART3 NVIC_BUILD_PRIORITY(1, 2)
#define NVIC_PRIO_SERIALUART4_TXDMA NVIC_BUILD_PRIORITY(1, 0)
#define NVIC_PRIO_SERIALUART4_RXDMA NVIC_BUILD_PRIORITY(1, 1)
#define NVIC_PRIO_SERIALUART4 NVIC_BUILD_PRIORITY(1, 2)
#define NVIC_PRIO_SERIALUART5_TXDMA NVIC_BUILD_PRIORITY(1, 0)
#define NVIC_PRIO_SERIALUART5_RXDMA NVIC_BUILD_PRIORITY(1, 1)
#define NVIC_PRIO_SERIALUART5 NVIC_BUILD_PRIORITY(1, 2)
#define NVIC_PRIO_SERIALUART6_TXDMA NVIC_BUILD_PRIORITY(1, 0)
#define NVIC_PRIO_SERIALUART6_RXDMA NVIC_BUILD_PRIORITY(1, 1)
#define NVIC_PRIO_SERIALUART6 NVIC_BUILD_PRIORITY(1, 2)
#define NVIC_PRIO_I2C_ER NVIC_BUILD_PRIORITY(0, 0) #define NVIC_PRIO_I2C_ER NVIC_BUILD_PRIORITY(0, 0)
#define NVIC_PRIO_I2C_EV NVIC_BUILD_PRIORITY(0, 0) #define NVIC_PRIO_I2C_EV NVIC_BUILD_PRIORITY(0, 0)
#define NVIC_PRIO_USB NVIC_BUILD_PRIORITY(2, 0) #define NVIC_PRIO_USB NVIC_BUILD_PRIORITY(2, 0)

View file

@ -35,4 +35,6 @@ typedef enum {
RESOURCE_USART = 1 << 4, RESOURCE_USART = 1 << 4,
RESOURCE_ADC = 1 << 5, RESOURCE_ADC = 1 << 5,
RESOURCE_EXTI = 1 << 6, RESOURCE_EXTI = 1 << 6,
RESOURCE_I2C = 1 << 7,
RESOURCE_SPI = 1 << 8,
} resourceType_t; } resourceType_t;

View file

@ -24,7 +24,8 @@
#include "system.h" #include "system.h"
#include "gpio.h" #include "gpio.h"
#include "nvic.h" #include "nvic.h"
#include "io.h"
#include "exti.h"
#include "sonar_hcsr04.h" #include "sonar_hcsr04.h"
/* HC-SR04 consists of ultrasonic transmitter, receiver, and control circuits. /* HC-SR04 consists of ultrasonic transmitter, receiver, and control circuits.
@ -41,40 +42,27 @@ STATIC_UNIT_TESTED volatile int32_t measurement = -1;
static uint32_t lastMeasurementAt; static uint32_t lastMeasurementAt;
static sonarHardware_t const *sonarHardware; static sonarHardware_t const *sonarHardware;
#if !defined(UNIT_TEST) extiCallbackRec_t hcsr04_extiCallbackRec;
static void ECHO_EXTI_IRQHandler(void) static IO_t echoIO;
//static IO_t triggerIO;
void hcsr04_extiHandler(extiCallbackRec_t* cb)
{ {
static uint32_t timing_start; static uint32_t timing_start;
uint32_t timing_stop; uint32_t timing_stop;
UNUSED(cb);
if (digitalIn(sonarHardware->echo_gpio, sonarHardware->echo_pin) != 0) { if (digitalIn(sonarHardware->echo_gpio, sonarHardware->echo_pin) != 0) {
timing_start = micros(); timing_start = micros();
} else { }
else {
timing_stop = micros(); timing_stop = micros();
if (timing_stop > timing_start) { if (timing_stop > timing_start) {
measurement = timing_stop - timing_start; measurement = timing_stop - timing_start;
} }
} }
EXTI_ClearITPendingBit(sonarHardware->exti_line);
} }
void EXTI0_IRQHandler(void)
{
ECHO_EXTI_IRQHandler();
}
void EXTI1_IRQHandler(void)
{
ECHO_EXTI_IRQHandler();
}
void EXTI9_5_IRQHandler(void)
{
ECHO_EXTI_IRQHandler();
}
#endif
void hcsr04_init(const sonarHardware_t *initialSonarHardware, sonarRange_t *sonarRange) void hcsr04_init(const sonarHardware_t *initialSonarHardware, sonarRange_t *sonarRange)
{ {
sonarHardware = initialSonarHardware; sonarHardware = initialSonarHardware;
@ -84,7 +72,6 @@ void hcsr04_init(const sonarHardware_t *initialSonarHardware, sonarRange_t *sona
#if !defined(UNIT_TEST) #if !defined(UNIT_TEST)
gpio_config_t gpio; gpio_config_t gpio;
EXTI_InitTypeDef EXTIInit;
#ifdef STM32F10X #ifdef STM32F10X
// enable AFIO for EXTI support // enable AFIO for EXTI support
@ -108,35 +95,17 @@ void hcsr04_init(const sonarHardware_t *initialSonarHardware, sonarRange_t *sona
gpio.pin = sonarHardware->echo_pin; gpio.pin = sonarHardware->echo_pin;
gpio.mode = Mode_IN_FLOATING; gpio.mode = Mode_IN_FLOATING;
gpioInit(sonarHardware->echo_gpio, &gpio); gpioInit(sonarHardware->echo_gpio, &gpio);
#ifdef STM32F10X echoIO = IOGetByTag(sonarHardware->echoIO);
// setup external interrupt on echo pin #ifdef USE_EXTI
gpioExtiLineConfig(GPIO_PortSourceGPIOB, sonarHardware->exti_pin_source); EXTIHandlerInit(&hcsr04_extiCallbackRec, hcsr04_extiHandler);
EXTIConfig(echoIO, &hcsr04_extiCallbackRec, NVIC_PRIO_SONAR_EXTI, EXTI_Trigger_Rising_Falling); // TODO - priority!
EXTIEnable(echoIO, true);
#endif #endif
#ifdef STM32F303xC
gpioExtiLineConfig(EXTI_PortSourceGPIOB, sonarHardware->exti_pin_source);
#endif
EXTI_ClearITPendingBit(sonarHardware->exti_line);
EXTIInit.EXTI_Line = sonarHardware->exti_line;
EXTIInit.EXTI_Mode = EXTI_Mode_Interrupt;
EXTIInit.EXTI_Trigger = EXTI_Trigger_Rising_Falling;
EXTIInit.EXTI_LineCmd = ENABLE;
EXTI_Init(&EXTIInit);
NVIC_InitTypeDef NVIC_InitStructure;
NVIC_InitStructure.NVIC_IRQChannel = sonarHardware->exti_irqn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_SONAR_ECHO);
NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_SONAR_ECHO);
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
lastMeasurementAt = millis() - 60; // force 1st measurement in hcsr04_get_distance() lastMeasurementAt = millis() - 60; // force 1st measurement in hcsr04_get_distance()
#else #else
lastMeasurementAt = 0; // to avoid "unused" compiler warning UNUSED(lastMeasurementAt); // to avoid "unused" compiler warning
#endif #endif
} }

View file

@ -18,15 +18,15 @@
#pragma once #pragma once
#include "platform.h" #include "platform.h"
#include "io.h"
typedef struct sonarHardware_s { typedef struct sonarHardware_s {
uint16_t trigger_pin; uint16_t trigger_pin;
GPIO_TypeDef* trigger_gpio; GPIO_TypeDef* trigger_gpio;
uint16_t echo_pin; uint16_t echo_pin;
GPIO_TypeDef* echo_gpio; GPIO_TypeDef* echo_gpio;
uint32_t exti_line; ioTag_t triggerIO;
uint8_t exti_pin_source; ioTag_t echoIO;
IRQn_Type exti_irqn;
} sonarHardware_t; } sonarHardware_t;
typedef struct sonarRange_s { typedef struct sonarRange_s {

View file

@ -55,48 +55,6 @@ void registerExtiCallbackHandler(IRQn_Type irqn, extiCallbackHandlerFunc *fn)
failureMode(FAILURE_DEVELOPER); // EXTI_CALLBACK_HANDLER_COUNT is too low for the amount of handlers required. failureMode(FAILURE_DEVELOPER); // EXTI_CALLBACK_HANDLER_COUNT is too low for the amount of handlers required.
} }
void unregisterExtiCallbackHandler(IRQn_Type irqn, extiCallbackHandlerFunc *fn)
{
for (int index = 0; index < EXTI_CALLBACK_HANDLER_COUNT; index++) {
extiCallbackHandlerConfig_t *candidate = &extiHandlerConfigs[index];
if (candidate->fn == fn && candidate->irqn == irqn) {
candidate->fn = NULL;
candidate->irqn = 0;
return;
}
}
}
static void extiHandler(IRQn_Type irqn)
{
for (int index = 0; index < EXTI_CALLBACK_HANDLER_COUNT; index++) {
extiCallbackHandlerConfig_t *candidate = &extiHandlerConfigs[index];
if (candidate->fn && candidate->irqn == irqn) {
candidate->fn();
}
}
}
void EXTI15_10_IRQHandler(void)
{
extiHandler(EXTI15_10_IRQn);
}
#if defined(CC3D)
void EXTI3_IRQHandler(void)
{
extiHandler(EXTI3_IRQn);
}
#endif
#if defined(COLIBRI_RACE) || defined(LUX_RACE)
void EXTI9_5_IRQHandler(void)
{
extiHandler(EXTI9_5_IRQn);
}
#endif
// cycles per microsecond // cycles per microsecond
static uint32_t usTicks = 0; static uint32_t usTicks = 0;
// current uptime for 1kHz systick timer. will rollover after 49 days. hopefully we won't care. // current uptime for 1kHz systick timer. will rollover after 49 days. hopefully we won't care.

View file

@ -24,8 +24,18 @@ void delay(uint32_t ms);
uint32_t micros(void); uint32_t micros(void);
uint32_t millis(void); uint32_t millis(void);
typedef enum {
FAILURE_DEVELOPER = 0,
FAILURE_MISSING_ACC,
FAILURE_ACC_INIT,
FAILURE_ACC_INCOMPATIBLE,
FAILURE_INVALID_EEPROM_CONTENTS,
FAILURE_FLASH_WRITE_FAILED,
FAILURE_GYRO_INIT_FAILED
} failureMode_e;
// failure // failure
void failureMode(uint8_t mode); void failureMode(failureMode_e mode);
// bootloader/IAP // bootloader/IAP
void systemReset(void); void systemReset(void);
@ -43,13 +53,5 @@ void unregisterExtiCallbackHandler(IRQn_Type irqn, extiCallbackHandlerFunc *fn);
extern uint32_t cachedRccCsrValue; extern uint32_t cachedRccCsrValue;
typedef enum {
FAILURE_DEVELOPER = 0,
FAILURE_MISSING_ACC,
FAILURE_ACC_INIT,
FAILURE_ACC_INCOMPATIBLE,
FAILURE_INVALID_EEPROM_CONTENTS,
FAILURE_FLASH_WRITE_FAILED,
FAILURE_GYRO_INIT_FAILED
} failureMode_e;

View file

@ -2692,7 +2692,7 @@ static void cliSet(char *cmdline)
if (strncasecmp(cmdline, valueTable[i].name, strlen(valueTable[i].name)) == 0 && variableNameLength == strlen(valueTable[i].name)) { if (strncasecmp(cmdline, valueTable[i].name, strlen(valueTable[i].name)) == 0 && variableNameLength == strlen(valueTable[i].name)) {
bool changeValue = false; bool changeValue = false;
int_float_value_t tmp; int_float_value_t tmp = { 0 };
switch (valueTable[i].type & VALUE_MODE_MASK) { switch (valueTable[i].type & VALUE_MODE_MASK) {
case MODE_DIRECT: { case MODE_DIRECT: {
int32_t value = 0; int32_t value = 0;

View file

@ -369,7 +369,7 @@ void init(void)
#ifdef NAZE #ifdef NAZE
if (hardwareRevision >= NAZE32_REV5) { if (hardwareRevision >= NAZE32_REV5) {
// naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN. // naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN.
beeperConfig.gpioMode = Mode_Out_PP; beeperConfig.isOD = true;
beeperConfig.isInverted = true; beeperConfig.isInverted = true;
} }
#endif #endif

View file

@ -28,6 +28,7 @@
#define U_ID_1 (*(uint32_t*)0x1FFFF7B0) #define U_ID_1 (*(uint32_t*)0x1FFFF7B0)
#define U_ID_2 (*(uint32_t*)0x1FFFF7B4) #define U_ID_2 (*(uint32_t*)0x1FFFF7B4)
#define STM32F3
#endif #endif
#ifdef STM32F10X #ifdef STM32F10X
@ -41,6 +42,7 @@
#define U_ID_1 (*(uint32_t*)0x1FFFF7EC) #define U_ID_1 (*(uint32_t*)0x1FFFF7EC)
#define U_ID_2 (*(uint32_t*)0x1FFFF7F0) #define U_ID_2 (*(uint32_t*)0x1FFFF7F0)
#define STM32F1
#endif // STM32F10X #endif // STM32F10X
#include "target.h" #include "target.h"

View file

@ -84,26 +84,19 @@ uint8_t detectedSensors[MAX_SENSORS_TO_DETECT] = { GYRO_NONE, ACC_NONE, BARO_NON
const extiConfig_t *selectMPUIntExtiConfig(void) const extiConfig_t *selectMPUIntExtiConfig(void)
{ {
#if defined(MPU_INT_EXTI)
static const extiConfig_t mpuIntExtiConfig = { .io = IO_TAG(MPU_INT_EXTI) };
return &mpuIntExtiConfig;
#endif
#ifdef NAZE #ifdef NAZE
// MPU_INT output on rev4 PB13 // MPU_INT output on rev4 PB13
static const extiConfig_t nazeRev4MPUIntExtiConfig = { static const extiConfig_t nazeRev4MPUIntExtiConfig = {
.gpioAPB2Peripherals = RCC_APB2Periph_GPIOB, .io = IO_TAG(PB13)
.gpioPin = Pin_13,
.gpioPort = GPIOB,
.exti_port_source = GPIO_PortSourceGPIOB,
.exti_line = EXTI_Line13,
.exti_pin_source = GPIO_PinSource13,
.exti_irqn = EXTI15_10_IRQn
}; };
// MPU_INT output on rev5 hardware PC13 // MPU_INT output on rev5 hardware PC13
static const extiConfig_t nazeRev5MPUIntExtiConfig = { static const extiConfig_t nazeRev5MPUIntExtiConfig = {
.gpioAPB2Peripherals = RCC_APB2Periph_GPIOC, .io = IO_TAG(PC13)
.gpioPin = Pin_13,
.gpioPort = GPIOC,
.exti_port_source = GPIO_PortSourceGPIOC,
.exti_line = EXTI_Line13,
.exti_pin_source = GPIO_PinSource13,
.exti_irqn = EXTI15_10_IRQn
}; };
#ifdef AFROMINI #ifdef AFROMINI
@ -111,118 +104,29 @@ const extiConfig_t *selectMPUIntExtiConfig(void)
#else #else
if (hardwareRevision < NAZE32_REV5) { if (hardwareRevision < NAZE32_REV5) {
return &nazeRev4MPUIntExtiConfig; return &nazeRev4MPUIntExtiConfig;
} else { }
else {
return &nazeRev5MPUIntExtiConfig; return &nazeRev5MPUIntExtiConfig;
} }
#endif #endif
#endif #endif
#if defined(SPRACINGF3) || defined(SPRACINGF3MINI) || defined(SPRACINGF3EVO)
static const extiConfig_t spRacingF3MPUIntExtiConfig = {
.gpioAHBPeripherals = RCC_AHBPeriph_GPIOC,
.gpioPort = GPIOC,
.gpioPin = Pin_13,
.exti_port_source = EXTI_PortSourceGPIOC,
.exti_pin_source = EXTI_PinSource13,
.exti_line = EXTI_Line13,
.exti_irqn = EXTI15_10_IRQn
};
return &spRacingF3MPUIntExtiConfig;
#endif
#if defined(CC3D)
static const extiConfig_t cc3dMPUIntExtiConfig = {
.gpioAPB2Peripherals = RCC_APB2Periph_GPIOA,
.gpioPort = GPIOA,
.gpioPin = Pin_3,
.exti_port_source = GPIO_PortSourceGPIOA,
.exti_pin_source = GPIO_PinSource3,
.exti_line = EXTI_Line3,
.exti_irqn = EXTI3_IRQn
};
return &cc3dMPUIntExtiConfig;
#endif
#if defined(COLIBRI_RACE) || defined(LUX_RACE)
static const extiConfig_t RaceMPUIntExtiConfig = {
.gpioAHBPeripherals = RCC_AHBPeriph_GPIOA,
.gpioPort = GPIOA,
.gpioPin = Pin_5,
.exti_port_source = EXTI_PortSourceGPIOA,
.exti_pin_source = EXTI_PinSource5,
.exti_line = EXTI_Line5,
.exti_irqn = EXTI9_5_IRQn
};
return &RaceMPUIntExtiConfig;
#endif
#if defined(DOGE)
static const extiConfig_t dogeMPUIntExtiConfig = {
.gpioAHBPeripherals = RCC_AHBPeriph_GPIOC,
.gpioPort = GPIOC,
.gpioPin = Pin_13,
.exti_port_source = EXTI_PortSourceGPIOC,
.exti_pin_source = EXTI_PinSource13,
.exti_line = EXTI_Line13,
.exti_irqn = EXTI15_10_IRQn
};
return &dogeMPUIntExtiConfig;
#endif
#if defined(MOTOLAB) || defined(SPARKY)
static const extiConfig_t MotolabF3MPU6050Config = {
.gpioAHBPeripherals = RCC_AHBPeriph_GPIOA,
.gpioPort = GPIOA,
.gpioPin = Pin_15,
.exti_port_source = EXTI_PortSourceGPIOA,
.exti_pin_source = EXTI_PinSource15,
.exti_line = EXTI_Line15,
.exti_irqn = EXTI15_10_IRQn
};
return &MotolabF3MPU6050Config;
#endif
#ifdef SINGULARITY
static const extiConfig_t singularityMPU6050Config = {
.gpioAHBPeripherals = RCC_AHBPeriph_GPIOC,
.gpioPort = GPIOC,
.gpioPin = Pin_13,
.exti_port_source = EXTI_PortSourceGPIOC,
.exti_pin_source = EXTI_PinSource13,
.exti_line = EXTI_Line13,
.exti_irqn = EXTI15_10_IRQn
};
return &singularityMPU6050Config;
#endif
#ifdef ALIENFLIGHTF3 #ifdef ALIENFLIGHTF3
// MPU_INT output on V1 PA15 // MPU_INT output on V1 PA15
static const extiConfig_t alienFlightF3V1MPUIntExtiConfig = { static const extiConfig_t alienFlightF3V1MPUIntExtiConfig = {
.gpioAHBPeripherals = RCC_AHBPeriph_GPIOA, .io = IO_TAG(PA15)
.gpioPort = GPIOA,
.gpioPin = Pin_15,
.exti_port_source = EXTI_PortSourceGPIOA,
.exti_pin_source = EXTI_PinSource15,
.exti_line = EXTI_Line15,
.exti_irqn = EXTI15_10_IRQn
}; };
// MPU_INT output on V2 PB13 // MPU_INT output on V2 PB13
static const extiConfig_t alienFlightF3V2MPUIntExtiConfig = { static const extiConfig_t alienFlightF3V2MPUIntExtiConfig = {
.gpioAHBPeripherals = RCC_AHBPeriph_GPIOB, .io = IO_TAG(PB13)
.gpioPort = GPIOB,
.gpioPin = Pin_13,
.exti_port_source = EXTI_PortSourceGPIOB,
.exti_pin_source = EXTI_PinSource13,
.exti_line = EXTI_Line13,
.exti_irqn = EXTI15_10_IRQn
}; };
if (hardwareRevision == AFF3_REV_1) { if (hardwareRevision == AFF3_REV_1) {
return &alienFlightF3V1MPUIntExtiConfig; return &alienFlightF3V1MPUIntExtiConfig;
} else { }
else {
return &alienFlightF3V2MPUIntExtiConfig; return &alienFlightF3V2MPUIntExtiConfig;
} }
#endif #endif
return NULL; return NULL;
} }
@ -534,23 +438,20 @@ static void detectBaro(baroSensor_e baroHardwareToUse)
#ifdef USE_BARO_BMP085 #ifdef USE_BARO_BMP085
const bmp085Config_t *bmp085Config = NULL; const bmp085Config_t *bmp085Config = NULL;
#if defined(BARO_XCLR_GPIO) && defined(BARO_EOC_GPIO) #if defined(BARO_XCLR_GPIO) && defined(BARO_EOC_GPIO)
static const bmp085Config_t defaultBMP085Config = { static const bmp085Config_t defaultBMP085Config = {
.gpioAPB2Peripherals = BARO_APB2_PERIPHERALS, .xclrIO = IO_TAG(BARO_XCLR_PIN),
.xclrGpioPin = BARO_XCLR_PIN, .eocIO = IO_TAG(BARO_EOC_PIN),
.xclrGpioPort = BARO_XCLR_GPIO, };
.eocGpioPin = BARO_EOC_PIN, bmp085Config = &defaultBMP085Config;
.eocGpioPort = BARO_EOC_GPIO
};
bmp085Config = &defaultBMP085Config;
#endif #endif
#ifdef NAZE #ifdef NAZE
if (hardwareRevision == NAZE32) { if (hardwareRevision == NAZE32) {
bmp085Disable(bmp085Config); bmp085Disable(bmp085Config);
} }
#endif #endif
#endif #endif

View file

@ -56,18 +56,16 @@ const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryCon
.trigger_gpio = GPIOB, .trigger_gpio = GPIOB,
.echo_pin = Pin_9, // PWM6 (PB9) - 5v tolerant .echo_pin = Pin_9, // PWM6 (PB9) - 5v tolerant
.echo_gpio = GPIOB, .echo_gpio = GPIOB,
.exti_line = EXTI_Line9, .triggerIO = IO_TAG(PB8),
.exti_pin_source = GPIO_PinSource9, .echoIO = IO_TAG(PB9),
.exti_irqn = EXTI9_5_IRQn
}; };
static const sonarHardware_t sonarRC78 = { static const sonarHardware_t sonarRC78 = {
.trigger_pin = Pin_0, // RX7 (PB0) - only 3.3v ( add a 1K Ohms resistor ) .trigger_pin = Pin_0, // RX7 (PB0) - only 3.3v ( add a 1K Ohms resistor )
.trigger_gpio = GPIOB, .trigger_gpio = GPIOB,
.echo_pin = Pin_1, // RX8 (PB1) - only 3.3v ( add a 1K Ohms resistor ) .echo_pin = Pin_1, // RX8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
.echo_gpio = GPIOB, .echo_gpio = GPIOB,
.exti_line = EXTI_Line1, .triggerIO = IO_TAG(PB0),
.exti_pin_source = GPIO_PinSource1, .echoIO = IO_TAG(PB1),
.exti_irqn = EXTI1_IRQn
}; };
// If we are using softserial, parallel PWM or ADC current sensor, then use motor pins 5 and 6 for sonar, otherwise use rc pins 7 and 8 // If we are using softserial, parallel PWM or ADC current sensor, then use motor pins 5 and 6 for sonar, otherwise use rc pins 7 and 8
if (feature(FEATURE_SOFTSERIAL) if (feature(FEATURE_SOFTSERIAL)
@ -84,9 +82,8 @@ const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryCon
.trigger_gpio = GPIOB, .trigger_gpio = GPIOB,
.echo_pin = Pin_1, // RX8 (PB1) - only 3.3v ( add a 1K Ohms resistor ) .echo_pin = Pin_1, // RX8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
.echo_gpio = GPIOB, .echo_gpio = GPIOB,
.exti_line = EXTI_Line1, .triggerIO = IO_TAG(PB0),
.exti_pin_source = GPIO_PinSource1, .echoIO = IO_TAG(PB1),
.exti_irqn = EXTI1_IRQn
}; };
return &sonarHardware; return &sonarHardware;
#elif defined(CC3D) #elif defined(CC3D)
@ -96,9 +93,8 @@ const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryCon
.trigger_gpio = GPIOB, .trigger_gpio = GPIOB,
.echo_pin = Pin_0, // (PB0) - only 3.3v ( add a 1K Ohms resistor ) .echo_pin = Pin_0, // (PB0) - only 3.3v ( add a 1K Ohms resistor )
.echo_gpio = GPIOB, .echo_gpio = GPIOB,
.exti_line = EXTI_Line0, .triggerIO = IO_TAG(PB5),
.exti_pin_source = GPIO_PinSource0, .echoIO = IO_TAG(PB0),
.exti_irqn = EXTI0_IRQn
}; };
return &sonarHardware; return &sonarHardware;
#elif defined(SPRACINGF3) || defined(SPRACINGF3MINI) #elif defined(SPRACINGF3) || defined(SPRACINGF3MINI)
@ -108,9 +104,8 @@ const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryCon
.trigger_gpio = GPIOB, .trigger_gpio = GPIOB,
.echo_pin = Pin_1, // RC_CH8 (PB1) - only 3.3v ( add a 1K Ohms resistor ) .echo_pin = Pin_1, // RC_CH8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
.echo_gpio = GPIOB, .echo_gpio = GPIOB,
.exti_line = EXTI_Line1, .triggerIO = IO_TAG(PB0),
.exti_pin_source = EXTI_PinSource1, .echoIO = IO_TAG(PB1),
.exti_irqn = EXTI1_IRQn
}; };
return &sonarHardware; return &sonarHardware;
#elif defined(SPARKY) #elif defined(SPARKY)
@ -120,9 +115,8 @@ const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryCon
.trigger_gpio = GPIOA, .trigger_gpio = GPIOA,
.echo_pin = Pin_1, // PWM7 (PB1) - only 3.3v ( add a 1K Ohms resistor ) .echo_pin = Pin_1, // PWM7 (PB1) - only 3.3v ( add a 1K Ohms resistor )
.echo_gpio = GPIOB, .echo_gpio = GPIOB,
.exti_line = EXTI_Line1, .triggerIO = IO_TAG(PA2),
.exti_pin_source = EXTI_PinSource1, .echoIO = IO_TAG(PB1),
.exti_irqn = EXTI1_IRQn
}; };
return &sonarHardware; return &sonarHardware;
#elif defined(UNIT_TEST) #elif defined(UNIT_TEST)

View file

@ -36,6 +36,7 @@
#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready #define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready
#define USE_EXTI
//#define DEBUG_MPU_DATA_READY_INTERRUPT //#define DEBUG_MPU_DATA_READY_INTERRUPT
#define USE_MPU_DATA_READY_SIGNAL #define USE_MPU_DATA_READY_SIGNAL

View file

@ -24,6 +24,10 @@
#define BEEPER PB15 // PA15 (Beeper) #define BEEPER PB15 // PA15 (Beeper)
#define USE_EXTI
#define USE_MPU_DATA_READY_SIGNAL
#define MPU_INT_EXTI PA3
#define MPU6000_CS_GPIO GPIOA #define MPU6000_CS_GPIO GPIOA
#define MPU6000_CS_PIN GPIO_Pin_4 #define MPU6000_CS_PIN GPIO_Pin_4
#define MPU6000_SPI_INSTANCE SPI1 #define MPU6000_SPI_INSTANCE SPI1

View file

@ -28,6 +28,7 @@
#define BEEPER PB13 #define BEEPER PB13
#define BEEPER_INVERTED #define BEEPER_INVERTED
#define USE_EXTI
#define MPU6500_CS_GPIO_CLK_PERIPHERAL RCC_AHBPeriph_GPIOA #define MPU6500_CS_GPIO_CLK_PERIPHERAL RCC_AHBPeriph_GPIOA
#define MPU6500_CS_GPIO GPIOA #define MPU6500_CS_GPIO GPIOA
#define MPU6500_CS_PIN GPIO_Pin_4 #define MPU6500_CS_PIN GPIO_Pin_4
@ -169,6 +170,8 @@
// MPU6500 interrupt // MPU6500 interrupt
#define USE_EXTI
#define MPU_INT_EXTI PA5
//#define DEBUG_MPU_DATA_READY_INTERRUPT //#define DEBUG_MPU_DATA_READY_INTERRUPT
#define USE_MPU_DATA_READY_SIGNAL #define USE_MPU_DATA_READY_SIGNAL
#define ENSURE_MPU_DATA_READY_IS_LOW #define ENSURE_MPU_DATA_READY_IS_LOW

View file

@ -147,6 +147,8 @@
#define CURRENT_METER_ADC_CHANNEL ADC_Channel_2 #define CURRENT_METER_ADC_CHANNEL ADC_Channel_2
// mpu_int definition in sensors/initialisation.c // mpu_int definition in sensors/initialisation.c
#define USE_EXTI
#define MPU_INT_EXTI PC13
//#define DEBUG_MPU_DATA_READY_INTERRUPT //#define DEBUG_MPU_DATA_READY_INTERRUPT
#define USE_MPU_DATA_READY_SIGNAL #define USE_MPU_DATA_READY_SIGNAL
#define ENSURE_MPU_DATA_READY_IS_LOW #define ENSURE_MPU_DATA_READY_IS_LOW

View file

@ -25,6 +25,7 @@
#define INVERTER PB2 // PB2 (BOOT1) abused as inverter select GPIO #define INVERTER PB2 // PB2 (BOOT1) abused as inverter select GPIO
#define INVERTER_USART USART2 #define INVERTER_USART USART2
#define USE_EXTI
#define MPU6000_CS_GPIO GPIOB #define MPU6000_CS_GPIO GPIOB
#define MPU6000_CS_PIN GPIO_Pin_12 #define MPU6000_CS_PIN GPIO_Pin_12
#define MPU6000_SPI_INSTANCE SPI2 #define MPU6000_SPI_INSTANCE SPI2

View file

@ -133,6 +133,8 @@
// MPU6500 interrupt // MPU6500 interrupt
#define USE_EXTI
#define MPU_INT_EXTI PA5
//#define DEBUG_MPU_DATA_READY_INTERRUPT //#define DEBUG_MPU_DATA_READY_INTERRUPT
#define USE_MPU_DATA_READY_SIGNAL #define USE_MPU_DATA_READY_SIGNAL
#define ENSURE_MPU_DATA_READY_IS_LOW #define ENSURE_MPU_DATA_READY_IS_LOW

View file

@ -29,6 +29,8 @@
#define USABLE_TIMER_CHANNEL_COUNT 9 #define USABLE_TIMER_CHANNEL_COUNT 9
// MPU6050 interrupts // MPU6050 interrupts
#define USE_EXTI
#define MPU_INT_EXTI PA15
#define EXTI15_10_CALLBACK_HANDLER_COUNT 1 // MPU data ready #define EXTI15_10_CALLBACK_HANDLER_COUNT 1 // MPU data ready
#define USE_MPU_DATA_READY_SIGNAL #define USE_MPU_DATA_READY_SIGNAL
//#define ENSURE_MPU_DATA_READY_IS_LOW //#define ENSURE_MPU_DATA_READY_IS_LOW

View file

@ -27,15 +27,14 @@
#define BEEPER PA12 // PA12 (Beeper) #define BEEPER PA12 // PA12 (Beeper)
#define BARO_XCLR_GPIO GPIOC #define BARO_XCLR_PIN PC13
#define BARO_XCLR_PIN Pin_13 #define BARO_EOC_PIN PC14
#define BARO_EOC_GPIO GPIOC
#define BARO_EOC_PIN Pin_14
#define BARO_APB2_PERIPHERALS RCC_APB2Periph_GPIOC
#define INVERTER PB2 // PB2 (BOOT1) abused as inverter select GPIO #define INVERTER PB2 // PB2 (BOOT1) abused as inverter select GPIO
#define INVERTER_USART USART2 #define INVERTER_USART USART2
#define USE_EXTI
// SPI2 // SPI2
// PB15 28 SPI2_MOSI // PB15 28 SPI2_MOSI
// PB14 27 SPI2_MISO // PB14 27 SPI2_MISO

View file

@ -25,6 +25,8 @@
#define USABLE_TIMER_CHANNEL_COUNT 10 #define USABLE_TIMER_CHANNEL_COUNT 10
#define USE_EXTI
#define MPU_INT_EXTI PC13
#define USE_MPU_DATA_READY_SIGNAL #define USE_MPU_DATA_READY_SIGNAL
#define GYRO #define GYRO

View file

@ -28,6 +28,8 @@
#define USABLE_TIMER_CHANNEL_COUNT 11 #define USABLE_TIMER_CHANNEL_COUNT 11
// MPU6050 interrupts // MPU6050 interrupts
#define USE_EXTI
#define MPU_INT_EXTI PA15
#define EXTI15_10_CALLBACK_HANDLER_COUNT 1 // MPU data ready #define EXTI15_10_CALLBACK_HANDLER_COUNT 1 // MPU data ready
#define USE_MPU_DATA_READY_SIGNAL #define USE_MPU_DATA_READY_SIGNAL

View file

@ -28,6 +28,8 @@
#define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready and MAG data ready #define EXTI_CALLBACK_HANDLER_COUNT 2 // MPU data ready and MAG data ready
#define USE_EXTI
#define MPU_INT_EXTI PC13
#define USE_MPU_DATA_READY_SIGNAL #define USE_MPU_DATA_READY_SIGNAL
#define ENSURE_MPU_DATA_READY_IS_LOW #define ENSURE_MPU_DATA_READY_IS_LOW

View file

@ -28,6 +28,8 @@
#define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU_INT, SDCardDetect #define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU_INT, SDCardDetect
#define USE_EXTI
#define MPU_INT_EXTI PC13
#define USE_MPU_DATA_READY_SIGNAL #define USE_MPU_DATA_READY_SIGNAL
#define ENSURE_MPU_DATA_READY_IS_LOW #define ENSURE_MPU_DATA_READY_IS_LOW

View file

@ -31,6 +31,8 @@
#define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU_INT, SDCardDetect #define EXTI15_10_CALLBACK_HANDLER_COUNT 2 // MPU_INT, SDCardDetect
#define USE_EXTI
#define MPU_INT_EXTI PC13
#define USE_MPU_DATA_READY_SIGNAL #define USE_MPU_DATA_READY_SIGNAL
#define ENSURE_MPU_DATA_READY_IS_LOW #define ENSURE_MPU_DATA_READY_IS_LOW