mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-24 16:55:36 +03:00
Merge pull request #10653 from klutvott123/i2c-configurable-clockspeed
This commit is contained in:
commit
e37619e559
17 changed files with 170 additions and 138 deletions
|
@ -80,6 +80,7 @@ MCU_COMMON_SRC = \
|
|||
startup/system_stm32f30x.c \
|
||||
drivers/adc_stm32f30x.c \
|
||||
drivers/bus_i2c_stm32f30x.c \
|
||||
drivers/bus_i2c_timing.c \
|
||||
drivers/bus_spi_stdperiph.c \
|
||||
drivers/dma.c \
|
||||
drivers/light_ws2811strip_stdperiph.c \
|
||||
|
|
|
@ -175,6 +175,7 @@ MCU_COMMON_SRC = \
|
|||
drivers/audio_stm32f7xx.c \
|
||||
drivers/bus_i2c_hal.c \
|
||||
drivers/bus_i2c_hal_init.c \
|
||||
drivers/bus_i2c_timing.c \
|
||||
drivers/dma_stm32f7xx.c \
|
||||
drivers/light_ws2811strip_hal.c \
|
||||
drivers/transponder_ir_io_hal.c \
|
||||
|
|
|
@ -160,6 +160,7 @@ MCU_COMMON_SRC = \
|
|||
drivers/adc_stm32g4xx.c \
|
||||
drivers/bus_i2c_hal.c \
|
||||
drivers/bus_i2c_hal_init.c \
|
||||
drivers/bus_i2c_timing.c \
|
||||
drivers/bus_spi_hal.c \
|
||||
drivers/dma_stm32g4xx.c \
|
||||
drivers/dshot_bitbang.c \
|
||||
|
|
|
@ -291,6 +291,7 @@ MCU_COMMON_SRC = \
|
|||
drivers/adc_stm32h7xx.c \
|
||||
drivers/bus_i2c_hal.c \
|
||||
drivers/bus_i2c_hal_init.c \
|
||||
drivers/bus_i2c_timing.c \
|
||||
drivers/pwm_output_dshot_hal.c \
|
||||
drivers/pwm_output_dshot_shared.c \
|
||||
drivers/persistent.c \
|
||||
|
|
|
@ -294,6 +294,7 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
|
|||
drivers/barometer/barometer_lps.c \
|
||||
drivers/barometer/barometer_qmp6988.c \
|
||||
drivers/bus_i2c_config.c \
|
||||
drivers/bus_i2c_timing.c \
|
||||
drivers/bus_spi_config.c \
|
||||
drivers/bus_spi_pinconfig.c \
|
||||
drivers/compass/compass_ak8963.c \
|
||||
|
|
|
@ -1631,19 +1631,19 @@ const clivalue_t valueTable[] = {
|
|||
#ifdef I2C_FULL_RECONFIGURABILITY
|
||||
#ifdef USE_I2C_DEVICE_1
|
||||
{ "i2c1_pullup", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_I2C_CONFIG, PG_ARRAY_ELEMENT_OFFSET(i2cConfig_t, 0, pullUp) },
|
||||
{ "i2c1_overclock", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_I2C_CONFIG, PG_ARRAY_ELEMENT_OFFSET(i2cConfig_t, 0, overClock) },
|
||||
{ "i2c1_clockspeed_khz", VAR_UINT16 | HARDWARE_VALUE, .config.minmax = { I2C_CLOCKSPEED_MIN_KHZ, I2C_CLOCKSPEED_MAX_KHZ }, PG_I2C_CONFIG, PG_ARRAY_ELEMENT_OFFSET(i2cConfig_t, 0, clockSpeed) },
|
||||
#endif
|
||||
#ifdef USE_I2C_DEVICE_2
|
||||
{ "i2c2_pullup", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_I2C_CONFIG, PG_ARRAY_ELEMENT_OFFSET(i2cConfig_t, 1, pullUp) },
|
||||
{ "i2c2_overclock", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_I2C_CONFIG, PG_ARRAY_ELEMENT_OFFSET(i2cConfig_t, 1, overClock) },
|
||||
{ "i2c2_clockspeed_khz", VAR_UINT16 | HARDWARE_VALUE, .config.minmax = { I2C_CLOCKSPEED_MIN_KHZ, I2C_CLOCKSPEED_MAX_KHZ }, PG_I2C_CONFIG, PG_ARRAY_ELEMENT_OFFSET(i2cConfig_t, 1, clockSpeed) },
|
||||
#endif
|
||||
#ifdef USE_I2C_DEVICE_3
|
||||
{ "i2c3_pullup", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_I2C_CONFIG, PG_ARRAY_ELEMENT_OFFSET(i2cConfig_t, 2, pullUp) },
|
||||
{ "i2c3_overclock", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_I2C_CONFIG, PG_ARRAY_ELEMENT_OFFSET(i2cConfig_t, 2, overClock) },
|
||||
{ "i2c3_clockspeed_khz", VAR_UINT16 | HARDWARE_VALUE, .config.minmax = { I2C_CLOCKSPEED_MIN_KHZ, I2C_CLOCKSPEED_MAX_KHZ }, PG_I2C_CONFIG, PG_ARRAY_ELEMENT_OFFSET(i2cConfig_t, 2, clockSpeed) },
|
||||
#endif
|
||||
#ifdef USE_I2C_DEVICE_4
|
||||
{ "i2c4_pullup", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_I2C_CONFIG, PG_ARRAY_ELEMENT_OFFSET(i2cConfig_t, 3, pullUp) },
|
||||
{ "i2c4_overclock", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_I2C_CONFIG, PG_ARRAY_ELEMENT_OFFSET(i2cConfig_t, 3, overClock) },
|
||||
{ "i2c4_clockspeed_khz", VAR_UINT16 | HARDWARE_VALUE, .config.minmax = { I2C_CLOCKSPEED_MIN_KHZ, I2C_CLOCKSPEED_MAX_KHZ }, PG_I2C_CONFIG, PG_ARRAY_ELEMENT_OFFSET(i2cConfig_t, 3, clockSpeed) },
|
||||
#endif
|
||||
#endif
|
||||
#ifdef USE_MCO
|
||||
|
|
|
@ -71,8 +71,8 @@ void i2cHardwareConfigure(const i2cConfig_t *i2cConfig)
|
|||
if (pDev->scl && pDev->sda) {
|
||||
pDev->hardware = hardware;
|
||||
pDev->reg = hardware->reg;
|
||||
pDev->overClock = i2cConfig[device].overClock;
|
||||
pDev->pullUp = i2cConfig[device].pullUp;
|
||||
pDev->clockSpeed = i2cConfig[device].clockSpeed;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -34,6 +34,7 @@
|
|||
|
||||
#include "drivers/bus_i2c.h"
|
||||
#include "drivers/bus_i2c_impl.h"
|
||||
#include "drivers/bus_i2c_timing.h"
|
||||
|
||||
// Number of bits in I2C protocol phase
|
||||
#define LEN_ADDR 7
|
||||
|
@ -202,99 +203,6 @@ const i2cHardware_t i2cHardware[I2CDEV_COUNT] = {
|
|||
|
||||
i2cDevice_t i2cDevice[I2CDEV_COUNT];
|
||||
|
||||
// Values from I2C-SMBus specification
|
||||
static uint16_t trmax; // Rise time (max)
|
||||
static uint16_t tfmax; // Fall time (max)
|
||||
static uint8_t tsuDATmin; // SDA setup time (min)
|
||||
static uint8_t thdDATmin; // SDA hold time (min)
|
||||
static uint16_t tHIGHmin; // High period of SCL clock (min)
|
||||
static uint16_t tLOWmin; // Low period of SCL clock (min)
|
||||
|
||||
// Silicon specific values, from datasheet
|
||||
static uint8_t tAFmin; // Analog filter delay (min)
|
||||
static uint16_t tAFmax; // Analog filter delay (max)
|
||||
|
||||
// Actual (estimated) values
|
||||
static uint16_t tr = 100; // Rise time
|
||||
static uint16_t tf = 10; // Fall time
|
||||
|
||||
/*
|
||||
* Compute SCLDEL, SDADEL, SCLH and SCLL for TIMINGR register according to reference manuals.
|
||||
*/
|
||||
static void i2cClockComputeRaw(uint32_t pclkFreq, int i2cFreqKhz, int presc, int dfcoeff,
|
||||
uint8_t *scldel, uint8_t *sdadel, uint16_t *sclh, uint16_t *scll)
|
||||
{
|
||||
if (i2cFreqKhz > 400) {
|
||||
// Fm+ (Fast mode plus)
|
||||
trmax = 120;
|
||||
tfmax = 120;
|
||||
tsuDATmin = 50;
|
||||
thdDATmin = 0;
|
||||
tHIGHmin = 260;
|
||||
tLOWmin = 500;
|
||||
} else {
|
||||
// Fm (Fast mode)
|
||||
trmax = 300;
|
||||
tfmax = 300;
|
||||
tsuDATmin = 100;
|
||||
thdDATmin = 0;
|
||||
tHIGHmin = 600;
|
||||
tLOWmin = 1300;
|
||||
}
|
||||
tAFmin = 50;
|
||||
tAFmax = 90;
|
||||
|
||||
// Convert pclkFreq into nsec
|
||||
float tI2cclk = 1000000000.0f / pclkFreq;
|
||||
|
||||
// Convert target i2cFreq into cycle time (nsec)
|
||||
float tSCL = 1000000.0f / i2cFreqKhz;
|
||||
|
||||
uint32_t SCLDELmin = (trmax + tsuDATmin)/((presc + 1) * tI2cclk) - 1;
|
||||
|
||||
uint32_t SDADELmin = (tfmax + thdDATmin - tAFmin - ((dfcoeff + 3) * tI2cclk)) / ((presc + 1) * tI2cclk);
|
||||
|
||||
float tsync1 = tf + tAFmin + dfcoeff * tI2cclk + 2 * tI2cclk;
|
||||
float tsync2 = tr + tAFmin + dfcoeff * tI2cclk + 2 * tI2cclk;
|
||||
|
||||
float tSCLH = tHIGHmin * tSCL / (tHIGHmin + tLOWmin) - tsync2;
|
||||
float tSCLL = tSCL - tSCLH - tsync1 - tsync2;
|
||||
|
||||
uint32_t SCLH = tSCLH / ((presc + 1) * tI2cclk) - 1;
|
||||
uint32_t SCLL = tSCLL / ((presc + 1) * tI2cclk) - 1;
|
||||
|
||||
while (tsync1 + tsync2 + ((SCLH + 1) + (SCLL + 1)) * ((presc + 1) * tI2cclk) < tSCL) {
|
||||
SCLH++;
|
||||
}
|
||||
|
||||
*scldel = SCLDELmin;
|
||||
*sdadel = SDADELmin;
|
||||
*sclh = SCLH;
|
||||
*scll = SCLL;
|
||||
}
|
||||
|
||||
static uint32_t i2cClockTIMINGR(uint32_t pclkFreq, int i2cFreqKhz, int dfcoeff)
|
||||
{
|
||||
#define TIMINGR(presc, scldel, sdadel, sclh, scll) \
|
||||
((presc << 28)|(scldel << 20)|(sdadel << 16)|(sclh << 8)|(scll << 0))
|
||||
|
||||
uint8_t scldel;
|
||||
uint8_t sdadel;
|
||||
uint16_t sclh;
|
||||
uint16_t scll;
|
||||
|
||||
for (int presc = 0; presc < 15; presc++) {
|
||||
i2cClockComputeRaw(pclkFreq, i2cFreqKhz, presc, dfcoeff, &scldel, &sdadel, &sclh, &scll);
|
||||
|
||||
// If all fields are not overflowing, return TIMINGR.
|
||||
// Otherwise, increase prescaler and try again.
|
||||
if ((scldel < 16) && (sdadel < 16) && (sclh < 256) && (scll < 256)) {
|
||||
return TIMINGR(presc, scldel, sdadel, sclh, scll);
|
||||
}
|
||||
}
|
||||
return 0; // Shouldn't reach here
|
||||
}
|
||||
|
||||
void i2cInit(I2CDevice device)
|
||||
{
|
||||
if (device == I2CINVALID) {
|
||||
|
@ -358,7 +266,7 @@ void i2cInit(I2CDevice device)
|
|||
#error Unknown MCU type
|
||||
#endif
|
||||
|
||||
pHandle->Init.Timing = i2cClockTIMINGR(i2cPclk, pDev->overClock ? 800 : 400, 0);
|
||||
pHandle->Init.Timing = i2cClockTIMINGR(i2cPclk, pDev->clockSpeed, 0);
|
||||
|
||||
pHandle->Init.OwnAddress1 = 0x0;
|
||||
pHandle->Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
|
||||
|
|
|
@ -82,8 +82,8 @@ typedef struct i2cDevice_s {
|
|||
uint8_t sclAF;
|
||||
uint8_t sdaAF;
|
||||
#endif
|
||||
bool overClock;
|
||||
bool pullUp;
|
||||
uint16_t clockSpeed;
|
||||
|
||||
// MCU/Driver dependent member follows
|
||||
#if defined(STM32F1) || defined(STM32F4)
|
||||
|
|
|
@ -35,8 +35,6 @@
|
|||
#include "drivers/bus_i2c.h"
|
||||
#include "drivers/bus_i2c_impl.h"
|
||||
|
||||
#define CLOCKSPEED 800000 // i2c clockspeed 400kHz default (conform specs), 800kHz and 1200kHz (Betaflight default)
|
||||
|
||||
// Number of bits in I2C protocol phase
|
||||
#define LEN_ADDR 7
|
||||
#define LEN_RW 1
|
||||
|
@ -483,12 +481,7 @@ void i2cInit(I2CDevice device)
|
|||
i2cInit.I2C_OwnAddress1 = 0;
|
||||
i2cInit.I2C_Ack = I2C_Ack_Enable;
|
||||
i2cInit.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
|
||||
|
||||
if (pDev->overClock) {
|
||||
i2cInit.I2C_ClockSpeed = 800000; // 800khz Maximum speed tested on various boards without issues
|
||||
} else {
|
||||
i2cInit.I2C_ClockSpeed = 400000; // 400khz Operation according specs
|
||||
}
|
||||
i2cInit.I2C_ClockSpeed = pDev->clockSpeed * 1000;
|
||||
|
||||
I2C_Cmd(I2Cx, ENABLE);
|
||||
I2C_Init(I2Cx, &i2cInit);
|
||||
|
|
|
@ -36,13 +36,11 @@
|
|||
|
||||
#include "drivers/bus_i2c.h"
|
||||
#include "drivers/bus_i2c_impl.h"
|
||||
#include "drivers/bus_i2c_timing.h"
|
||||
|
||||
#define IOCFG_I2C_PU IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_UP)
|
||||
#define IOCFG_I2C IO_CONFIG(GPIO_Mode_AF, GPIO_Speed_50MHz, GPIO_OType_OD, GPIO_PuPd_NOPULL)
|
||||
|
||||
#define I2C_HIGHSPEED_TIMING 0x00500E30 // 1000 Khz, 72Mhz Clock, Analog Filter Delay ON, Setup 40, Hold 4.
|
||||
#define I2C_STANDARD_TIMING 0x00E0257A // 400 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10.
|
||||
|
||||
#define I2C_GPIO_AF GPIO_AF_4
|
||||
|
||||
static volatile uint16_t i2cErrorCount = 0;
|
||||
|
@ -114,7 +112,7 @@ void i2cInit(I2CDevice device)
|
|||
.I2C_OwnAddress1 = 0x00,
|
||||
.I2C_Ack = I2C_Ack_Enable,
|
||||
.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
|
||||
.I2C_Timing = (pDev->overClock ? I2C_HIGHSPEED_TIMING : I2C_STANDARD_TIMING)
|
||||
.I2C_Timing = i2cClockTIMINGR(SystemCoreClock, pDev->clockSpeed, 0)
|
||||
};
|
||||
|
||||
I2C_Init(I2Cx, &i2cInit);
|
||||
|
|
112
src/main/drivers/bus_i2c_timing.c
Normal file
112
src/main/drivers/bus_i2c_timing.c
Normal file
|
@ -0,0 +1,112 @@
|
|||
/*
|
||||
* This file is part of Cleanflight and Betaflight.
|
||||
*
|
||||
* Cleanflight and Betaflight are free software. You can redistribute
|
||||
* this software and/or modify this software under the terms of the
|
||||
* GNU General Public License as published by the Free Software
|
||||
* Foundation, either version 3 of the License, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* Cleanflight and Betaflight are distributed in the hope that they
|
||||
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
|
||||
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||
* See the GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this software.
|
||||
*
|
||||
* If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
/*
|
||||
* Compute SCLDEL, SDADEL, SCLH and SCLL for TIMINGR register according to reference manuals.
|
||||
*/
|
||||
static void i2cClockComputeRaw(uint32_t pclkFreq, int i2cFreqKhz, int presc, int dfcoeff,
|
||||
uint8_t *scldel, uint8_t *sdadel, uint16_t *sclh, uint16_t *scll)
|
||||
{
|
||||
// Values from I2C-SMBus specification
|
||||
uint16_t trmax; // Rise time (max)
|
||||
uint16_t tfmax; // Fall time (max)
|
||||
uint8_t tsuDATmin; // SDA setup time (min)
|
||||
uint8_t thdDATmin; // SDA hold time (min)
|
||||
uint16_t tHIGHmin; // High period of SCL clock (min)
|
||||
uint16_t tLOWmin; // Low period of SCL clock (min)
|
||||
|
||||
// Silicon specific values, from datasheet
|
||||
uint8_t tAFmin = 50; // Analog filter delay (min)
|
||||
|
||||
// Actual (estimated) values
|
||||
uint8_t tr = 100; // Rise time
|
||||
uint8_t tf = 10; // Fall time
|
||||
|
||||
if (i2cFreqKhz > 400) {
|
||||
// Fm+ (Fast mode plus)
|
||||
trmax = 120;
|
||||
tfmax = 120;
|
||||
tsuDATmin = 50;
|
||||
thdDATmin = 0;
|
||||
tHIGHmin = 260;
|
||||
tLOWmin = 500;
|
||||
} else {
|
||||
// Fm (Fast mode)
|
||||
trmax = 300;
|
||||
tfmax = 300;
|
||||
tsuDATmin = 100;
|
||||
thdDATmin = 0;
|
||||
tHIGHmin = 600;
|
||||
tLOWmin = 1300;
|
||||
}
|
||||
|
||||
// Convert pclkFreq into nsec
|
||||
float tI2cclk = 1000000000.0f / pclkFreq;
|
||||
|
||||
// Convert target i2cFreq into cycle time (nsec)
|
||||
float tSCL = 1000000.0f / i2cFreqKhz;
|
||||
|
||||
uint32_t SCLDELmin = (trmax + tsuDATmin) / ((presc + 1) * tI2cclk) - 1;
|
||||
uint32_t SDADELmin = (tfmax + thdDATmin - tAFmin - ((dfcoeff + 3) * tI2cclk)) / ((presc + 1) * tI2cclk);
|
||||
|
||||
float tsync1 = tf + tAFmin + dfcoeff * tI2cclk + 2 * tI2cclk;
|
||||
float tsync2 = tr + tAFmin + dfcoeff * tI2cclk + 2 * tI2cclk;
|
||||
|
||||
float tSCLH = tHIGHmin * tSCL / (tHIGHmin + tLOWmin) - tsync2;
|
||||
float tSCLL = tSCL - tSCLH - tsync1 - tsync2;
|
||||
|
||||
uint32_t SCLH = tSCLH / ((presc + 1) * tI2cclk) - 1;
|
||||
uint32_t SCLL = tSCLL / ((presc + 1) * tI2cclk) - 1;
|
||||
|
||||
while (tsync1 + tsync2 + ((SCLH + 1) + (SCLL + 1)) * ((presc + 1) * tI2cclk) < tSCL) {
|
||||
SCLH++;
|
||||
}
|
||||
|
||||
*scldel = SCLDELmin;
|
||||
*sdadel = SDADELmin;
|
||||
*sclh = SCLH;
|
||||
*scll = SCLL;
|
||||
}
|
||||
|
||||
uint32_t i2cClockTIMINGR(uint32_t pclkFreq, int i2cFreqKhz, int dfcoeff)
|
||||
{
|
||||
#define TIMINGR(presc, scldel, sdadel, sclh, scll) \
|
||||
((presc << 28)|(scldel << 20)|(sdadel << 16)|(sclh << 8)|(scll << 0))
|
||||
|
||||
uint8_t scldel;
|
||||
uint8_t sdadel;
|
||||
uint16_t sclh;
|
||||
uint16_t scll;
|
||||
|
||||
for (int presc = 0; presc < 15; presc++) {
|
||||
i2cClockComputeRaw(pclkFreq, i2cFreqKhz, presc, dfcoeff, &scldel, &sdadel, &sclh, &scll);
|
||||
|
||||
// If all fields are not overflowing, return TIMINGR.
|
||||
// Otherwise, increase prescaler and try again.
|
||||
if ((scldel < 16) && (sdadel < 16) && (sclh < 256) && (scll < 256)) {
|
||||
return TIMINGR(presc, scldel, sdadel, sclh, scll);
|
||||
}
|
||||
}
|
||||
return 0; // Shouldn't reach here
|
||||
}
|
23
src/main/drivers/bus_i2c_timing.h
Normal file
23
src/main/drivers/bus_i2c_timing.h
Normal file
|
@ -0,0 +1,23 @@
|
|||
/*
|
||||
* This file is part of Cleanflight and Betaflight.
|
||||
*
|
||||
* Cleanflight and Betaflight are free software. You can redistribute
|
||||
* this software and/or modify this software under the terms of the
|
||||
* GNU General Public License as published by the Free Software
|
||||
* Foundation, either version 3 of the License, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* Cleanflight and Betaflight are distributed in the hope that they
|
||||
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
|
||||
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
||||
* See the GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this software.
|
||||
*
|
||||
* If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
uint32_t i2cClockTIMINGR(uint32_t pclkFreq, int i2cFreqKhz, int dfcoeff);
|
|
@ -42,22 +42,22 @@
|
|||
typedef struct i2cDefaultConfig_s {
|
||||
I2CDevice device;
|
||||
ioTag_t ioTagScl, ioTagSda;
|
||||
bool overClock;
|
||||
bool pullUp;
|
||||
uint16_t clockSpeed;
|
||||
} i2cDefaultConfig_t;
|
||||
|
||||
static const i2cDefaultConfig_t i2cDefaultConfig[] = {
|
||||
#ifdef USE_I2C_DEVICE_1
|
||||
{ I2CDEV_1, IO_TAG(I2C1_SCL), IO_TAG(I2C1_SDA), I2C1_OVERCLOCK, I2C1_PULLUP },
|
||||
{ I2CDEV_1, IO_TAG(I2C1_SCL), IO_TAG(I2C1_SDA), I2C1_PULLUP, I2C1_CLOCKSPEED },
|
||||
#endif
|
||||
#ifdef USE_I2C_DEVICE_2
|
||||
{ I2CDEV_2, IO_TAG(I2C2_SCL), IO_TAG(I2C2_SDA), I2C2_OVERCLOCK, I2C2_PULLUP },
|
||||
{ I2CDEV_2, IO_TAG(I2C2_SCL), IO_TAG(I2C2_SDA), I2C2_PULLUP, I2C2_CLOCKSPEED },
|
||||
#endif
|
||||
#ifdef USE_I2C_DEVICE_3
|
||||
{ I2CDEV_3, IO_TAG(I2C3_SCL), IO_TAG(I2C3_SDA), I2C3_OVERCLOCK, I2C3_PULLUP },
|
||||
{ I2CDEV_3, IO_TAG(I2C3_SCL), IO_TAG(I2C3_SDA), I2C3_PULLUP, I2C3_CLOCKSPEED },
|
||||
#endif
|
||||
#ifdef USE_I2C_DEVICE_4
|
||||
{ I2CDEV_4, IO_TAG(I2C4_SCL), IO_TAG(I2C4_SDA), I2C4_OVERCLOCK, I2C4_PULLUP },
|
||||
{ I2CDEV_4, IO_TAG(I2C4_SCL), IO_TAG(I2C4_SDA), I2C4_PULLUP, I2C4_CLOCKSPEED },
|
||||
#endif
|
||||
};
|
||||
|
||||
|
@ -70,11 +70,11 @@ void pgResetFn_i2cConfig(i2cConfig_t *i2cConfig)
|
|||
int device = defconf->device;
|
||||
i2cConfig[device].ioTagScl = defconf->ioTagScl;
|
||||
i2cConfig[device].ioTagSda = defconf->ioTagSda;
|
||||
i2cConfig[device].overClock = defconf->overClock;
|
||||
i2cConfig[device].pullUp = defconf->pullUp;
|
||||
i2cConfig[device].clockSpeed = defconf->clockSpeed;
|
||||
}
|
||||
}
|
||||
|
||||
PG_REGISTER_ARRAY_WITH_RESET_FN(i2cConfig_t, I2CDEV_COUNT, i2cConfig, PG_I2C_CONFIG, 0);
|
||||
PG_REGISTER_ARRAY_WITH_RESET_FN(i2cConfig_t, I2CDEV_COUNT, i2cConfig, PG_I2C_CONFIG, 1);
|
||||
|
||||
#endif // defined(USE_I2C) && !defined(USE_SOFT_I2C)
|
||||
|
|
|
@ -26,11 +26,14 @@
|
|||
|
||||
#include "pg/pg.h"
|
||||
|
||||
#define I2C_CLOCKSPEED_MIN_KHZ 100
|
||||
#define I2C_CLOCKSPEED_MAX_KHZ 1300
|
||||
|
||||
typedef struct i2cConfig_s {
|
||||
ioTag_t ioTagScl;
|
||||
ioTag_t ioTagSda;
|
||||
bool overClock;
|
||||
bool pullUp;
|
||||
uint16_t clockSpeed;
|
||||
} i2cConfig_t;
|
||||
|
||||
PG_DECLARE_ARRAY(i2cConfig_t, I2CDEV_COUNT, i2cConfig);
|
||||
|
|
|
@ -169,17 +169,17 @@
|
|||
|
||||
#endif // I2C_FULL_RECONFIGURABILITY
|
||||
|
||||
#ifndef I2C1_OVERCLOCK
|
||||
#define I2C1_OVERCLOCK false
|
||||
#ifndef I2C1_CLOCKSPEED
|
||||
#define I2C1_CLOCKSPEED 800
|
||||
#endif
|
||||
#ifndef I2C2_OVERCLOCK
|
||||
#define I2C2_OVERCLOCK false
|
||||
#ifndef I2C2_CLOCKSPEED
|
||||
#define I2C2_CLOCKSPEED 800
|
||||
#endif
|
||||
#ifndef I2C3_OVERCLOCK
|
||||
#define I2C3_OVERCLOCK false
|
||||
#ifndef I2C3_CLOCKSPEED
|
||||
#define I2C3_CLOCKSPEED 800
|
||||
#endif
|
||||
#ifndef I2C4_OVERCLOCK
|
||||
#define I2C4_OVERCLOCK false
|
||||
#ifndef I2C4_CLOCKSPEED
|
||||
#define I2C4_CLOCKSPEED 800
|
||||
#endif
|
||||
|
||||
// Default values for internal pullup
|
||||
|
|
|
@ -30,9 +30,6 @@
|
|||
|
||||
//#define SCHEDULER_DEBUG // define this to use scheduler debug[] values. Undefined by default for performance reasons
|
||||
|
||||
#define I2C1_OVERCLOCK true
|
||||
#define I2C2_OVERCLOCK true
|
||||
|
||||
#ifdef STM32F1
|
||||
#define MINIMAL_CLI
|
||||
// Using RX DMA disables the use of receive callbacks
|
||||
|
@ -57,7 +54,6 @@
|
|||
#define USE_DSHOT_TELEMETRY_STATS
|
||||
#define USE_RPM_FILTER
|
||||
#define USE_DYN_IDLE
|
||||
#define I2C3_OVERCLOCK true
|
||||
#define USE_GYRO_DATA_ANALYSE
|
||||
#define USE_ADC
|
||||
#define USE_ADC_INTERNAL
|
||||
|
@ -86,8 +82,6 @@
|
|||
#define USE_DSHOT_TELEMETRY_STATS
|
||||
#define USE_RPM_FILTER
|
||||
#define USE_DYN_IDLE
|
||||
#define I2C3_OVERCLOCK true
|
||||
#define I2C4_OVERCLOCK true
|
||||
#define USE_GYRO_DATA_ANALYSE
|
||||
#define USE_OVERCLOCK
|
||||
#define USE_ADC_INTERNAL
|
||||
|
@ -111,8 +105,6 @@
|
|||
#define USE_DSHOT_TELEMETRY_STATS
|
||||
#define USE_RPM_FILTER
|
||||
#define USE_DYN_IDLE
|
||||
#define I2C3_OVERCLOCK true
|
||||
#define I2C4_OVERCLOCK true
|
||||
#define USE_GYRO_DATA_ANALYSE
|
||||
#define USE_ADC_INTERNAL
|
||||
#define USE_USB_CDC_HID
|
||||
|
@ -135,8 +127,6 @@
|
|||
#define USE_DSHOT_TELEMETRY_STATS
|
||||
#define USE_RPM_FILTER
|
||||
#define USE_DYN_IDLE
|
||||
#define I2C3_OVERCLOCK true
|
||||
#define I2C4_OVERCLOCK true
|
||||
#define USE_OVERCLOCK
|
||||
#define USE_GYRO_DATA_ANALYSE
|
||||
#define USE_ADC_INTERNAL
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue