rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

New SPI API supporting DMA 

Call targetConfiguration() once before config is loaded and again afterwards in case the config needs to be changed to load from SD card etc

Drop SPI clock during binding

Remove debug

Add per device SPI DMA enable

Fix sdioPinConfigure() declaration warning

Reduce clock speed during SPI RX initialisation
This commit is contained in:
Steve Evans 2021-04-20 19:45:56 +01:00 committed by Michael Keller
parent 6d286e25f1
commit 87c8847c13
136 changed files with 3585 additions and 2721 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
make/mcu/STM32H7.mk
View File

@ -60,6 +60,7 @@ EXCLUDES = \
stm32h7xx_hal_smartcard_ex.c \
stm32h7xx_hal_smbus.c \
stm32h7xx_hal_spdifrx.c \
stm32h7xx_hal_spi.c \
stm32h7xx_hal_sram.c \
stm32h7xx_hal_swpmi.c \
stm32h7xx_hal_usart.c \
@ -85,7 +86,6 @@ EXCLUDES = \
stm32h7xx_ll_rcc.c \
stm32h7xx_ll_rng.c \
stm32h7xx_ll_rtc.c \
stm32h7xx_ll_spi.c \
stm32h7xx_ll_swpmi.c \
stm32h7xx_ll_usart.c \
stm32h7xx_ll_utils.c
@ -282,7 +282,7 @@ MCU_COMMON_SRC = \
drivers/serial_uart_hal.c \
drivers/serial_uart_stm32h7xx.c \
drivers/bus_quadspi_hal.c \
drivers/bus_spi_hal.c \
drivers/bus_spi_ll.c \
drivers/dma_stm32h7xx.c \
drivers/dshot_bitbang.c \
drivers/dshot_bitbang_decode.c \

4
src/link/stm32_flash_f7_split.ld
View File

@ -120,7 +120,7 @@ SECTIONS
. = ALIGN(4);
_edata = .; /* define a global symbol at data end */
} >RAM AT >AXIM_FLASH
} >RAM AT >AXIM_FLASH1
/* Uninitialized data section */
. = ALIGN(4);
@ -166,7 +166,7 @@ SECTIONS
. = ALIGN(4);
_efastram_data = .; /* define a global symbol at data end */
} >FASTRAM AT >AXIM_FLASH
} >FASTRAM AT >AXIM_FLASH1
. = ALIGN(4);
.fastram_bss (NOLOAD) :

35
src/link/stm32_flash_h723_1m.ld
View File

@ -162,7 +162,7 @@ SECTIONS
. = ALIGN(4);
_edata = .; /* define a global symbol at data end */
} >RAM AT >FLASH
} >FASTRAM AT >FLASH
/* Uninitialized data section */
. = ALIGN(4);
@ -178,7 +178,7 @@ SECTIONS
. = ALIGN(4);
_ebss = .; /* define a global symbol at bss end */
__bss_end__ = _ebss;
} >RAM
} >FASTRAM
/* Uninitialized data section */
. = ALIGN(4);
@ -223,17 +223,42 @@ SECTIONS
__fastram_bss_end__ = _efastram_bss;
} >FASTRAM
.DMA_RAM (NOLOAD) :
{
/* used during startup to initialized dmaram_data */
_sdmaram_idata = LOADADDR(.dmaram_data);
. = ALIGN(32);
.dmaram_data :
{
PROVIDE(dmaram_start = .);
_sdmaram = .;
_dmaram_start__ = _sdmaram;
_sdmaram_data = .; /* create a global symbol at data start */
*(.dmaram_data) /* .data sections */
*(.dmaram_data*) /* .data* sections */
. = ALIGN(32);
_edmaram_data = .; /* define a global symbol at data end */
} >RAM AT >FLASH
. = ALIGN(32);
.dmaram_bss (NOLOAD) :
{
_sdmaram_bss = .;
__dmaram_bss_start__ = _sdmaram_bss;
*(.dmaram_bss)
*(SORT_BY_ALIGNMENT(.dmaram_bss*))
. = ALIGN(32);
_edmaram_bss = .;
__dmaram_bss_end__ = _edmaram_bss;
} >RAM
. = ALIGN(32);
.DMA_RAM (NOLOAD) :
{
KEEP(*(.DMA_RAM))
PROVIDE(dmaram_end = .);
_edmaram = .;
_dmaram_end__ = _edmaram;
} >D2_RAM
} >RAM
.DMA_RW_D2 (NOLOAD) :
{

31
src/link/stm32_flash_h743_2m.ld
View File

@ -239,17 +239,42 @@ SECTIONS
__fastram_bss_end__ = _efastram_bss;
} >FASTRAM
.DMA_RAM (NOLOAD) :
{
/* used during startup to initialized dmaram_data */
_sdmaram_idata = LOADADDR(.dmaram_data);
. = ALIGN(32);
.dmaram_data :
{
PROVIDE(dmaram_start = .);
_sdmaram = .;
_dmaram_start__ = _sdmaram;
_sdmaram_data = .; /* create a global symbol at data start */
*(.dmaram_data) /* .data sections */
*(.dmaram_data*) /* .data* sections */
. = ALIGN(32);
_edmaram_data = .; /* define a global symbol at data end */
} >RAM AT >FLASH
. = ALIGN(32);
.dmaram_bss (NOLOAD) :
{
_sdmaram_bss = .;
__dmaram_bss_start__ = _sdmaram_bss;
*(.dmaram_bss)
*(SORT_BY_ALIGNMENT(.dmaram_bss*))
. = ALIGN(32);
_edmaram_bss = .;
__dmaram_bss_end__ = _edmaram_bss;
} >RAM
. = ALIGN(32);
.DMA_RAM (NOLOAD) :
{
KEEP(*(.DMA_RAM))
PROVIDE(dmaram_end = .);
_edmaram = .;
_dmaram_end__ = _edmaram;
} >D2_RAM
} >RAM
.DMA_RW_D2 (NOLOAD) :
{

34
src/link/stm32_flash_h750_128k.ld
View File

@ -50,19 +50,46 @@ REGION_ALIAS("STACKRAM", DTCM_RAM)
REGION_ALIAS("FASTRAM", DTCM_RAM)
REGION_ALIAS("MAIN", FLASH)
INCLUDE "stm32_h750_common.ld"
SECTIONS
{
.DMA_RAM (NOLOAD) :
{
/* used during startup to initialized dmaram_data */
_sdmaram_idata = LOADADDR(.dmaram_data);
. = ALIGN(32);
.dmaram_data :
{
PROVIDE(dmaram_start = .);
_sdmaram = .;
_dmaram_start__ = _sdmaram;
_sdmaram_data = .; /* create a global symbol at data start */
*(.dmaram_data) /* .data sections */
*(.dmaram_data*) /* .data* sections */
. = ALIGN(32);
_edmaram_data = .; /* define a global symbol at data end */
} >RAM AT >FLASH
. = ALIGN(32);
.dmaram_bss (NOLOAD) :
{
_sdmaram_bss = .;
__dmaram_bss_start__ = _sdmaram_bss;
*(.dmaram_bss)
*(SORT_BY_ALIGNMENT(.dmaram_bss*))
. = ALIGN(32);
_edmaram_bss = .;
__dmaram_bss_end__ = _edmaram_bss;
} >RAM
. = ALIGN(32);
.DMA_RAM (NOLOAD) :
{
KEEP(*(.DMA_RAM))
PROVIDE(dmaram_end = .);
_edmaram = .;
_dmaram_end__ = _edmaram;
} >D2_RAM
} >RAM
.DMA_RW_D2 (NOLOAD) :
{
@ -89,5 +116,4 @@ SECTIONS
} >RAM
}
INCLUDE "stm32_h750_common.ld"
INCLUDE "stm32_h750_common_post.ld"

34
src/link/stm32_flash_h750_1m.ld
View File

@ -50,19 +50,46 @@ REGION_ALIAS("STACKRAM", DTCM_RAM)
REGION_ALIAS("FASTRAM", DTCM_RAM)
REGION_ALIAS("MAIN", FLASH)
INCLUDE "stm32_h750_common.ld"
SECTIONS
{
.DMA_RAM (NOLOAD) :
{
/* used during startup to initialized dmaram_data */
_sdmaram_idata = LOADADDR(.dmaram_data);
. = ALIGN(32);
.dmaram_data :
{
PROVIDE(dmaram_start = .);
_sdmaram = .;
_dmaram_start__ = _sdmaram;
_sdmaram_data = .; /* create a global symbol at data start */
*(.dmaram_data) /* .data sections */
*(.dmaram_data*) /* .data* sections */
. = ALIGN(32);
_edmaram_data = .; /* define a global symbol at data end */
} >RAM AT >FLASH
. = ALIGN(32);
.dmaram_bss (NOLOAD) :
{
_sdmaram_bss = .;
__dmaram_bss_start__ = _sdmaram_bss;
*(.dmaram_bss)
*(SORT_BY_ALIGNMENT(.dmaram_bss*))
. = ALIGN(32);
_edmaram_bss = .;
__dmaram_bss_end__ = _edmaram_bss;
} >RAM
. = ALIGN(32);
.DMA_RAM (NOLOAD) :
{
KEEP(*(.DMA_RAM))
PROVIDE(dmaram_end = .);
_edmaram = .;
_dmaram_end__ = _edmaram;
} >D2_RAM
} >RAM
.DMA_RW_D2 (NOLOAD) :
{
@ -89,5 +116,4 @@ SECTIONS
} >RAM
}
INCLUDE "stm32_h750_common.ld"
INCLUDE "stm32_h750_common_post.ld"

35
src/link/stm32_flash_h7a3_2m.ld
View File

@ -147,7 +147,7 @@ SECTIONS
. = ALIGN(4);
_edata = .; /* define a global symbol at data end */
} >RAM AT >FLASH
} >FASTRAM AT >FLASH
/* Uninitialized data section */
. = ALIGN(4);
@ -163,7 +163,7 @@ SECTIONS
. = ALIGN(4);
_ebss = .; /* define a global symbol at bss end */
__bss_end__ = _ebss;
} >RAM
} >FASTRAM
/* Uninitialized data section */
. = ALIGN(4);
@ -208,17 +208,42 @@ SECTIONS
__fastram_bss_end__ = _efastram_bss;
} >FASTRAM
.DMA_RAM (NOLOAD) :
{
/* used during startup to initialized dmaram_data */
_sdmaram_idata = LOADADDR(.dmaram_data);
. = ALIGN(32);
.dmaram_data :
{
PROVIDE(dmaram_start = .);
_sdmaram = .;
_dmaram_start__ = _sdmaram;
_sdmaram_data = .; /* create a global symbol at data start */
*(.dmaram_data) /* .data sections */
*(.dmaram_data*) /* .data* sections */
. = ALIGN(32);
_edmaram_data = .; /* define a global symbol at data end */
} >RAM AT >FLASH
. = ALIGN(32);
.dmaram_bss (NOLOAD) :
{
_sdmaram_bss = .;
__dmaram_bss_start__ = _sdmaram_bss;
*(.dmaram_bss)
*(SORT_BY_ALIGNMENT(.dmaram_bss*))
. = ALIGN(32);
_edmaram_bss = .;
__dmaram_bss_end__ = _edmaram_bss;
} >RAM
. = ALIGN(32);
.DMA_RAM (NOLOAD) :
{
KEEP(*(.DMA_RAM))
PROVIDE(dmaram_end = .);
_edmaram = .;
_dmaram_end__ = _edmaram;
} >D2_RAM
} >RAM
.DMA_RW_D2 (NOLOAD) :
{

4
src/link/stm32_h750_common.ld
View File

@ -15,7 +15,7 @@ _Min_Stack_Size = 0x800; /* required amount of stack */
/* Define output sections */
SECTIONS
{
/* The startup code goes first into MAIN */
/* The vector table goes first into MAIN */
.isr_vector :
{
. = ALIGN(512);
@ -111,7 +111,7 @@ SECTIONS
. = ALIGN(4);
_ebss = .; /* define a global symbol at bss end */
__bss_end__ = _ebss;
} >D2_RAM
} >DTCM_RAM
/* Uninitialized data section */
. = ALIGN(4);

31
src/link/stm32_ram_h743.ld
View File

@ -214,17 +214,42 @@ SECTIONS
__fastram_bss_end__ = _efastram_bss;
} >FASTRAM
.DMA_RAM (NOLOAD) :
{
/* used during startup to initialized dmaram_data */
_sdmaram_idata = LOADADDR(.dmaram_data);
. = ALIGN(32);
.dmaram_data :
{
PROVIDE(dmaram_start = .);
_sdmaram = .;
_dmaram_start__ = _sdmaram;
_sdmaram_data = .; /* create a global symbol at data start */
*(.dmaram_data) /* .data sections */
*(.dmaram_data*) /* .data* sections */
. = ALIGN(32);
_edmaram_data = .; /* define a global symbol at data end */
} >RAM
. = ALIGN(32);
.dmaram_bss (NOLOAD) :
{
_sdmaram_bss = .;
__dmaram_bss_start__ = _sdmaram_bss;
*(.dmaram_bss)
*(SORT_BY_ALIGNMENT(.dmaram_bss*))
. = ALIGN(32);
_edmaram_bss = .;
__dmaram_bss_end__ = _edmaram_bss;
} >RAM
. = ALIGN(32);
.DMA_RAM (NOLOAD) :
{
KEEP(*(.DMA_RAM))
PROVIDE(dmaram_end = .);
_edmaram = .;
_dmaram_end__ = _edmaram;
} >D2_RAM
} >RAM
.DMA_RW_D2 (NOLOAD) :
{

34
src/link/stm32_ram_h750_exst.ld
View File

@ -73,19 +73,46 @@ REGION_ALIAS("STACKRAM", DTCM_RAM)
REGION_ALIAS("FASTRAM", DTCM_RAM)
REGION_ALIAS("MAIN", CODE_RAM)
INCLUDE "stm32_h750_common.ld"
SECTIONS
{
.DMA_RAM (NOLOAD) :
{
/* used during startup to initialized dmaram_data */
_sdmaram_idata = LOADADDR(.dmaram_data);
. = ALIGN(32);
.dmaram_data :
{
PROVIDE(dmaram_start = .);
_sdmaram = .;
_dmaram_start__ = _sdmaram;
_sdmaram_data = .; /* create a global symbol at data start */
*(.dmaram_data) /* .data sections */
*(.dmaram_data*) /* .data* sections */
. = ALIGN(32);
_edmaram_data = .; /* define a global symbol at data end */
} >RAM AT >MAIN
. = ALIGN(32);
.dmaram_bss (NOLOAD) :
{
_sdmaram_bss = .;
__dmaram_bss_start__ = _sdmaram_bss;
*(.dmaram_bss)
*(SORT_BY_ALIGNMENT(.dmaram_bss*))
. = ALIGN(32);
_edmaram_bss = .;
__dmaram_bss_end__ = _edmaram_bss;
} >RAM
. = ALIGN(32);
.DMA_RAM (NOLOAD) :
{
KEEP(*(.DMA_RAM))
PROVIDE(dmaram_end = .);
_edmaram = .;
_dmaram_end__ = _edmaram;
} >D2_RAM
} >RAM
.DMA_RW_D2 (NOLOAD) :
{
@ -112,7 +139,6 @@ SECTIONS
} >RAM
}
INCLUDE "stm32_h750_common.ld"
INCLUDE "stm32_h750_common_post.ld"
INCLUDE "stm32_ram_h750_exst_post.ld"

7
src/main/cli/settings.c
View File

@ -703,7 +703,7 @@ const clivalue_t valueTable[] = {
// PG_BAROMETER_CONFIG
#ifdef USE_BARO
{ "baro_bustype", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BUS_TYPE }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_bustype) },
{ "baro_bustype", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BUS_TYPE }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_busType) },
{ "baro_spi_device", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, 5 }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_spi_device) },
{ "baro_i2c_device", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, 5 }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_i2c_device) },
{ "baro_i2c_address", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2C_ADDR7_MAX }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_i2c_address) },
@ -1254,7 +1254,6 @@ const clivalue_t valueTable[] = {
{ "sdcard_mode", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_SDCARD_MODE }, PG_SDCARD_CONFIG, offsetof(sdcardConfig_t, mode) },
#endif
#ifdef USE_SDCARD_SPI
{ "sdcard_dma", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_SDCARD_CONFIG, offsetof(sdcardConfig_t, useDma) },
{ "sdcard_spi_bus", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, SPIDEV_COUNT }, PG_SDCARD_CONFIG, offsetof(sdcardConfig_t, device) },
#endif
#ifdef USE_SDCARD_SDIO
@ -1599,7 +1598,7 @@ const clivalue_t valueTable[] = {
#endif
// PG_GYRO_DEVICE_CONFIG
{ "gyro_1_bustype", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BUS_TYPE }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, bustype) },
{ "gyro_1_bustype", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BUS_TYPE }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, busType) },
{ "gyro_1_spibus", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, SPIDEV_COUNT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, spiBus) },
{ "gyro_1_i2cBus", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2CDEV_COUNT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, i2cBus) },
{ "gyro_1_i2c_address", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2C_ADDR7_MAX }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, i2cAddress) },
@ -1608,7 +1607,7 @@ const clivalue_t valueTable[] = {
{ "gyro_1_align_pitch", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, customAlignment.pitch) },
{ "gyro_1_align_yaw", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, customAlignment.yaw) },
#ifdef USE_MULTI_GYRO
{ "gyro_2_bustype", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BUS_TYPE }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, bustype) },
{ "gyro_2_bustype", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BUS_TYPE }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, busType) },
{ "gyro_2_spibus", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, SPIDEV_COUNT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, spiBus) },
{ "gyro_2_i2cBus", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2CDEV_COUNT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, i2cBus) },
{ "gyro_2_i2c_address", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2C_ADDR7_MAX }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, i2cAddress) },

6
src/main/drivers/accgyro/accgyro.h
View File

@ -33,8 +33,6 @@
#pragma GCC diagnostic push
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
#include <pthread.h>
#elif !defined(UNIT_TEST)
#pragma GCC diagnostic warning "-Wpadded"
#endif
#define GYRO_SCALE_2000DPS (2000.0f / (1 << 15)) // 16.384 dps/lsb scalefactor for 2000dps sensors
@ -88,7 +86,7 @@ typedef struct gyroDev_s {
sensorGyroReadFuncPtr readFn; // read 3 axis data function
sensorGyroReadDataFuncPtr temperatureFn; // read temperature if available
extiCallbackRec_t exti;
busDevice_t bus;
extDevice_t dev;
float scale; // scalefactor
float gyroZero[XYZ_AXIS_COUNT];
float gyroADC[XYZ_AXIS_COUNT]; // gyro data after calibration and alignment
@ -118,12 +116,12 @@ typedef struct accDev_s {
float acc_1G_rec;
sensorAccInitFuncPtr initFn; // initialize function
sensorAccReadFuncPtr readFn; // read 3 axis data function
busDevice_t bus;
uint16_t acc_1G;
int16_t ADCRaw[XYZ_AXIS_COUNT];
mpuDetectionResult_t mpuDetectionResult;
sensor_align_e accAlign;
bool dataReady;
gyroDev_t *gyro;
bool acc_high_fsr;
char revisionCode; // a revision code for the sensor, if known
uint8_t filler[2];

81
src/main/drivers/accgyro/accgyro_mpu.c
View File

@ -75,7 +75,7 @@ static void mpu6050FindRevision(gyroDev_t *gyro)
// determine product ID and revision
uint8_t readBuffer[6];
bool ack = busReadRegisterBuffer(&gyro->bus, MPU_RA_XA_OFFS_H, readBuffer, 6);
bool ack = busReadRegisterBuffer(&gyro->dev, MPU_RA_XA_OFFS_H, readBuffer, 6);
uint8_t revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01);
if (ack && revision) {
// Congrats, these parts are better
@ -90,7 +90,7 @@ static void mpu6050FindRevision(gyroDev_t *gyro)
}
} else {
uint8_t productId;
ack = busReadRegisterBuffer(&gyro->bus, MPU_RA_PRODUCT_ID, &productId, 1);
ack = busReadRegisterBuffer(&gyro->dev, MPU_RA_PRODUCT_ID, &productId, 1);
revision = productId & 0x0F;
if (!ack || revision == 0) {
failureMode(FAILURE_ACC_INCOMPATIBLE);
@ -149,7 +149,7 @@ bool mpuAccRead(accDev_t *acc)
{
uint8_t data[6];
const bool ack = busReadRegisterBuffer(&acc->bus, MPU_RA_ACCEL_XOUT_H, data, 6);
const bool ack = busReadRegisterBuffer(&acc->gyro->dev, MPU_RA_ACCEL_XOUT_H, data, 6);
if (!ack) {
return false;
}
@ -165,7 +165,7 @@ bool mpuGyroRead(gyroDev_t *gyro)
{
uint8_t data[6];
const bool ack = busReadRegisterBuffer(&gyro->bus, MPU_RA_GYRO_XOUT_H, data, 6);
const bool ack = busReadRegisterBuffer(&gyro->dev, MPU_RA_GYRO_XOUT_H, data, 6);
if (!ack) {
return false;
}
@ -178,12 +178,29 @@ bool mpuGyroRead(gyroDev_t *gyro)
}
#ifdef USE_SPI_GYRO
bool mpuAccReadSPI(accDev_t *acc)
{
STATIC_DMA_DATA_AUTO uint8_t dataToSend[7] = {MPU_RA_ACCEL_XOUT_H | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
STATIC_DMA_DATA_AUTO uint8_t data[7];
const bool ack = spiReadWriteBufRB(&acc->gyro->dev, dataToSend, data, 7);
if (!ack) {
return false;
}
acc->ADCRaw[X] = (int16_t)((data[1] << 8) | data[2]);
acc->ADCRaw[Y] = (int16_t)((data[3] << 8) | data[4]);
acc->ADCRaw[Z] = (int16_t)((data[5] << 8) | data[6]);
return true;
}
bool mpuGyroReadSPI(gyroDev_t *gyro)
{
static const uint8_t dataToSend[7] = {MPU_RA_GYRO_XOUT_H | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
uint8_t data[7];
STATIC_DMA_DATA_AUTO uint8_t dataToSend[7] = {MPU_RA_GYRO_XOUT_H | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
STATIC_DMA_DATA_AUTO uint8_t data[7];
const bool ack = spiBusTransfer(&gyro->bus, dataToSend, data, 7);
const bool ack = spiReadWriteBufRB(&gyro->dev, dataToSend, data, 7);
if (!ack) {
return false;
}
@ -195,7 +212,7 @@ bool mpuGyroReadSPI(gyroDev_t *gyro)
return true;
}
typedef uint8_t (*gyroSpiDetectFn_t)(const busDevice_t *bus);
typedef uint8_t (*gyroSpiDetectFn_t)(const extDevice_t *dev);
static gyroSpiDetectFn_t gyroSpiDetectFnTable[] = {
#ifdef USE_GYRO_SPI_MPU6000
@ -233,17 +250,15 @@ static gyroSpiDetectFn_t gyroSpiDetectFnTable[] = {
static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro, const gyroDeviceConfig_t *config)
{
SPI_TypeDef *instance = spiInstanceByDevice(SPI_CFG_TO_DEV(config->spiBus));
if (!instance || !config->csnTag) {
if (!config->csnTag || !spiSetBusInstance(&gyro->dev, config->spiBus, OWNER_GYRO_CS)) {
return false;
}
spiBusSetInstance(&gyro->bus, instance);
gyro->bus.busdev_u.spi.csnPin = IOGetByTag(config->csnTag);
IOInit(gyro->bus.busdev_u.spi.csnPin, OWNER_GYRO_CS, RESOURCE_INDEX(config->index));
IOConfigGPIO(gyro->bus.busdev_u.spi.csnPin, SPI_IO_CS_CFG);
IOHi(gyro->bus.busdev_u.spi.csnPin); // Ensure device is disabled, important when two devices are on the same bus.
gyro->dev.busType_u.spi.csnPin = IOGetByTag(config->csnTag);
IOInit(gyro->dev.busType_u.spi.csnPin, OWNER_GYRO_CS, RESOURCE_INDEX(config->index));
IOConfigGPIO(gyro->dev.busType_u.spi.csnPin, SPI_IO_CS_CFG);
IOHi(gyro->dev.busType_u.spi.csnPin); // Ensure device is disabled, important when two devices are on the same bus.
uint8_t sensor = MPU_NONE;
@ -252,11 +267,10 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro, const gyro
// May need a bitmap of hardware to detection function to do it right?
for (size_t index = 0 ; gyroSpiDetectFnTable[index] ; index++) {
sensor = (gyroSpiDetectFnTable[index])(&gyro->bus);
sensor = (gyroSpiDetectFnTable[index])(&gyro->dev);
if (sensor != MPU_NONE) {
gyro->mpuDetectionResult.sensor = sensor;
busDeviceRegister(&gyro->bus);
busDeviceRegister(&gyro->dev);
return true;
}
}
@ -280,32 +294,35 @@ void mpuPreInit(const struct gyroDeviceConfig_s *config)
bool mpuDetect(gyroDev_t *gyro, const gyroDeviceConfig_t *config)
{
static busDevice_t bus;
gyro->dev.bus = &bus;
// MPU datasheet specifies 30ms.
delay(35);
if (config->bustype == BUSTYPE_NONE) {
if (config->busType == BUS_TYPE_NONE) {
return false;
}
if (config->bustype == BUSTYPE_GYRO_AUTO) {
gyro->bus.bustype = BUSTYPE_I2C;
if (config->busType == BUS_TYPE_GYRO_AUTO) {
gyro->dev.bus->busType = BUS_TYPE_I2C;
} else {
gyro->bus.bustype = config->bustype;
gyro->dev.bus->busType = config->busType;
}
#ifdef USE_I2C_GYRO
if (gyro->bus.bustype == BUSTYPE_I2C) {
gyro->bus.busdev_u.i2c.device = I2C_CFG_TO_DEV(config->i2cBus);
gyro->bus.busdev_u.i2c.address = config->i2cAddress ? config->i2cAddress : MPU_ADDRESS;
if (gyro->dev.bus->busType == BUS_TYPE_I2C) {
gyro->dev.bus->busType_u.i2c.device = I2C_CFG_TO_DEV(config->i2cBus);
gyro->dev.busType_u.i2c.address = config->i2cAddress ? config->i2cAddress : MPU_ADDRESS;
uint8_t sig = 0;
bool ack = busReadRegisterBuffer(&gyro->bus, MPU_RA_WHO_AM_I, &sig, 1);
bool ack = busReadRegisterBuffer(&gyro->dev, MPU_RA_WHO_AM_I, &sig, 1);
if (ack) {
busDeviceRegister(&gyro->bus);
busDeviceRegister(&gyro->dev);
// If an MPU3050 is connected sig will contain 0.
uint8_t inquiryResult;
ack = busReadRegisterBuffer(&gyro->bus, MPU_RA_WHO_AM_I_LEGACY, &inquiryResult, 1);
ack = busReadRegisterBuffer(&gyro->dev, MPU_RA_WHO_AM_I_LEGACY, &inquiryResult, 1);
inquiryResult &= MPU_INQUIRY_MASK;
if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) {
gyro->mpuDetectionResult.sensor = MPU_3050;
@ -325,7 +342,7 @@ bool mpuDetect(gyroDev_t *gyro, const gyroDeviceConfig_t *config)
#endif
#ifdef USE_SPI_GYRO
gyro->bus.bustype = BUSTYPE_SPI;
gyro->dev.bus->busType = BUS_TYPE_SPI;
return detectSPISensorsAndUpdateDetectionResult(gyro, config);
#else
@ -370,10 +387,10 @@ uint8_t mpuGyroDLPF(gyroDev_t *gyro)
}
#ifdef USE_GYRO_REGISTER_DUMP
uint8_t mpuGyroReadRegister(const busDevice_t *bus, uint8_t reg)
uint8_t mpuGyroReadRegister(const extDevice_t *dev, uint8_t reg)
{
uint8_t data;
const bool ack = busReadRegisterBuffer(bus, reg, &data, 1);
const bool ack = busReadRegisterBuffer(dev, reg, &data, 1);
if (ack) {
return data;
} else {

3
src/main/drivers/accgyro/accgyro_mpu.h
View File

@ -224,7 +224,8 @@ bool mpuGyroReadSPI(struct gyroDev_s *gyro);
void mpuPreInit(const struct gyroDeviceConfig_s *config);
bool mpuDetect(struct gyroDev_s *gyro, const struct gyroDeviceConfig_s *config);
uint8_t mpuGyroDLPF(struct gyroDev_s *gyro);
uint8_t mpuGyroReadRegister(const busDevice_t *bus, uint8_t reg);
uint8_t mpuGyroReadRegister(const extDevice_t *dev, uint8_t reg);
struct accDev_s;
bool mpuAccRead(struct accDev_s *acc);
bool mpuAccReadSPI(struct accDev_s *acc);

16
src/main/drivers/accgyro/accgyro_mpu6050.c
View File

@ -81,23 +81,23 @@ static void mpu6050GyroInit(gyroDev_t *gyro)
{
mpuGyroInit(gyro);
busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
busWriteRegister(&gyro->dev, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
delay(100);
busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
busWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyro->mpuDividerDrops); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
busWriteRegister(&gyro->dev, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
busWriteRegister(&gyro->dev, MPU_RA_SMPLRT_DIV, gyro->mpuDividerDrops); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure
busWriteRegister(&gyro->bus, MPU_RA_CONFIG, mpuGyroDLPF(gyro)); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
busWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
busWriteRegister(&gyro->dev, MPU_RA_CONFIG, mpuGyroDLPF(gyro)); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
busWriteRegister(&gyro->dev, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
// ACC Init stuff.
// Accel scale 8g (4096 LSB/g)
busWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
busWriteRegister(&gyro->dev, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
busWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG,
busWriteRegister(&gyro->dev, MPU_RA_INT_PIN_CFG,
0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS
#ifdef USE_MPU_DATA_READY_SIGNAL
busWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
busWriteRegister(&gyro->dev, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
#endif
}

22
src/main/drivers/accgyro/accgyro_mpu6500.c
View File

@ -64,33 +64,33 @@ void mpu6500GyroInit(gyroDev_t *gyro)
accel_range = ICM_HIGH_RANGE_FSR_16G;
}
busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
busWriteRegister(&gyro->dev, MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
delay(100);
busWriteRegister(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, 0x07);
busWriteRegister(&gyro->dev, MPU_RA_SIGNAL_PATH_RESET, 0x07);
delay(100);
busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, 0);
busWriteRegister(&gyro->dev, MPU_RA_PWR_MGMT_1, 0);
delay(100);
busWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
busWriteRegister(&gyro->dev, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
delay(15);
busWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, gyro_range << 3);
busWriteRegister(&gyro->dev, MPU_RA_GYRO_CONFIG, gyro_range << 3);
delay(15);
busWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, accel_range << 3);
busWriteRegister(&gyro->dev, MPU_RA_ACCEL_CONFIG, accel_range << 3);
delay(15);
busWriteRegister(&gyro->bus, MPU_RA_CONFIG, mpuGyroDLPF(gyro));
busWriteRegister(&gyro->dev, MPU_RA_CONFIG, mpuGyroDLPF(gyro));
delay(15);
busWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyro->mpuDividerDrops); // Get Divider Drops
busWriteRegister(&gyro->dev, MPU_RA_SMPLRT_DIV, gyro->mpuDividerDrops); // Get Divider Drops
delay(100);
// Data ready interrupt configuration
#ifdef USE_MPU9250_MAG
busWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); // INT_ANYRD_2CLEAR, BYPASS_EN
busWriteRegister(&gyro->dev, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); // INT_ANYRD_2CLEAR, BYPASS_EN
#else
busWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); // INT_ANYRD_2CLEAR
busWriteRegister(&gyro->dev, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); // INT_ANYRD_2CLEAR
#endif
delay(15);
#ifdef USE_MPU_DATA_READY_SIGNAL
busWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, MPU6500_BIT_RAW_RDY_EN); // RAW_RDY_EN interrupt enable
busWriteRegister(&gyro->dev, MPU_RA_INT_ENABLE, MPU6500_BIT_RAW_RDY_EN); // RAW_RDY_EN interrupt enable
#endif
delay(15);
}

76
src/main/drivers/accgyro/accgyro_spi_bmi160.c
View File

@ -89,23 +89,23 @@ static volatile bool BMI160InitDone = false;
static volatile bool BMI160Detected = false;
//! Private functions
static int32_t BMI160_Config(const busDevice_t *bus);
static int32_t BMI160_do_foc(const busDevice_t *bus);
static int32_t BMI160_Config(const extDevice_t *dev);
static int32_t BMI160_do_foc(const extDevice_t *dev);
uint8_t bmi160Detect(const busDevice_t *bus)
uint8_t bmi160Detect(const extDevice_t *dev)
{
if (BMI160Detected) {
return BMI_160_SPI;
}
spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(BMI160_MAX_SPI_CLK_HZ));
spiSetClkDivisor(dev, spiCalculateDivider(BMI160_MAX_SPI_CLK_HZ));
/* Read this address to activate SPI (see p. 84) */
spiBusReadRegister(bus, 0x7F);
spiReadRegMsk(dev, 0x7F);
delay(100); // Give SPI some time to start up
/* Check the chip ID */
if (spiBusReadRegister(bus, BMI160_REG_CHIPID) != 0xd1) {
if (spiReadRegMsk(dev, BMI160_REG_CHIPID) != 0xd1) {
return MPU_NONE;
}
@ -118,14 +118,14 @@ uint8_t bmi160Detect(const busDevice_t *bus)
* @brief Initialize the BMI160 6-axis sensor.
* @return 0 for success, -1 for failure to allocate, -10 for failure to get irq
*/
static void BMI160_Init(const busDevice_t *bus)
static void BMI160_Init(const extDevice_t *dev)
{
if (BMI160InitDone || !BMI160Detected) {
return;
}
/* Configure the BMI160 Sensor */
if (BMI160_Config(bus) != 0) {
if (BMI160_Config(dev) != 0) {
return;
}
@ -133,7 +133,7 @@ static void BMI160_Init(const busDevice_t *bus)
/* Perform fast offset compensation if requested */
if (do_foc) {
BMI160_do_foc(bus);
BMI160_do_foc(dev);
}
BMI160InitDone = true;
@ -143,68 +143,68 @@ static void BMI160_Init(const busDevice_t *bus)
/**
* @brief Configure the sensor
*/
static int32_t BMI160_Config(const busDevice_t *bus)
static int32_t BMI160_Config(const extDevice_t *dev)
{
// Set normal power mode for gyro and accelerometer
spiBusWriteRegister(bus, BMI160_REG_CMD, BMI160_PMU_CMD_PMU_GYR_NORMAL);
spiWriteReg(dev, BMI160_REG_CMD, BMI160_PMU_CMD_PMU_GYR_NORMAL);
delay(100); // can take up to 80ms
spiBusWriteRegister(bus, BMI160_REG_CMD, BMI160_PMU_CMD_PMU_ACC_NORMAL);
spiWriteReg(dev, BMI160_REG_CMD, BMI160_PMU_CMD_PMU_ACC_NORMAL);
delay(5); // can take up to 3.8ms
// Verify that normal power mode was entered
uint8_t pmu_status = spiBusReadRegister(bus, BMI160_REG_PMU_STAT);
uint8_t pmu_status = spiReadRegMsk(dev, BMI160_REG_PMU_STAT);
if ((pmu_status & 0x3C) != 0x14) {
return -3;
}
// Set odr and ranges
// Set acc_us = 0 acc_bwp = 0b010 so only the first filter stage is used
spiBusWriteRegister(bus, BMI160_REG_ACC_CONF, 0x20 | BMI160_ODR_800_Hz);
spiWriteReg(dev, BMI160_REG_ACC_CONF, 0x20 | BMI160_ODR_800_Hz);
delay(1);
// Set gyr_bwp = 0b010 so only the first filter stage is used
spiBusWriteRegister(bus, BMI160_REG_GYR_CONF, 0x20 | BMI160_ODR_3200_Hz);
spiWriteReg(dev, BMI160_REG_GYR_CONF, 0x20 | BMI160_ODR_3200_Hz);
delay(1);
spiBusWriteRegister(bus, BMI160_REG_ACC_RANGE, BMI160_RANGE_8G);
spiWriteReg(dev, BMI160_REG_ACC_RANGE, BMI160_RANGE_8G);
delay(1);
spiBusWriteRegister(bus, BMI160_REG_GYR_RANGE, BMI160_RANGE_2000DPS);
spiWriteReg(dev, BMI160_REG_GYR_RANGE, BMI160_RANGE_2000DPS);
delay(1);
// Enable offset compensation
uint8_t val = spiBusReadRegister(bus, BMI160_REG_OFFSET_0);
spiBusWriteRegister(bus, BMI160_REG_OFFSET_0, val | 0xC0);
uint8_t val = spiReadRegMsk(dev, BMI160_REG_OFFSET_0);
spiWriteReg(dev, BMI160_REG_OFFSET_0, val | 0xC0);
// Enable data ready interrupt
spiBusWriteRegister(bus, BMI160_REG_INT_EN1, BMI160_INT_EN1_DRDY);
spiWriteReg(dev, BMI160_REG_INT_EN1, BMI160_INT_EN1_DRDY);
delay(1);
// Enable INT1 pin
spiBusWriteRegister(bus, BMI160_REG_INT_OUT_CTRL, BMI160_INT_OUT_CTRL_INT1_CONFIG);
spiWriteReg(dev, BMI160_REG_INT_OUT_CTRL, BMI160_INT_OUT_CTRL_INT1_CONFIG);
delay(1);
// Map data ready interrupt to INT1 pin
spiBusWriteRegister(bus, BMI160_REG_INT_MAP1, BMI160_REG_INT_MAP1_INT1_DRDY);
spiWriteReg(dev, BMI160_REG_INT_MAP1, BMI160_REG_INT_MAP1_INT1_DRDY);
delay(1);
return 0;
}
static int32_t BMI160_do_foc(const busDevice_t *bus)
static int32_t BMI160_do_foc(const extDevice_t *dev)
{
// assume sensor is mounted on top
uint8_t val = 0x7D;;
spiBusWriteRegister(bus, BMI160_REG_FOC_CONF, val);
spiWriteReg(dev, BMI160_REG_FOC_CONF, val);
// Start FOC
spiBusWriteRegister(bus, BMI160_REG_CMD, BMI160_CMD_START_FOC);
spiWriteReg(dev, BMI160_REG_CMD, BMI160_CMD_START_FOC);
// Wait for FOC to complete
for (int i=0; i<50; i++) {
val = spiBusReadRegister(bus, BMI160_REG_STATUS);
val = spiReadRegMsk(dev, BMI160_REG_STATUS);
if (val & BMI160_REG_STATUS_FOC_RDY) {
break;
}
@ -215,14 +215,14 @@ static int32_t BMI160_do_foc(const busDevice_t *bus)
}
// Program NVM
val = spiBusReadRegister(bus, BMI160_REG_CONF);
spiBusWriteRegister(bus, BMI160_REG_CONF, val | BMI160_REG_CONF_NVM_PROG_EN);
val = spiReadRegMsk(dev, BMI160_REG_CONF);
spiWriteReg(dev, BMI160_REG_CONF, val | BMI160_REG_CONF_NVM_PROG_EN);
spiBusWriteRegister(bus, BMI160_REG_CMD, BMI160_CMD_PROG_NVM);
spiWriteReg(dev, BMI160_REG_CMD, BMI160_CMD_PROG_NVM);
// Wait for NVM programming to complete
for (int i=0; i<50; i++) {
val = spiBusReadRegister(bus, BMI160_REG_STATUS);
val = spiReadRegMsk(dev, BMI160_REG_STATUS);
if (val & BMI160_REG_STATUS_NVM_RDY) {
break;
}
@ -275,9 +275,7 @@ bool bmi160AccRead(accDev_t *acc)
uint8_t bmi160_rx_buf[BUFFER_SIZE];
static const uint8_t bmi160_tx_buf[BUFFER_SIZE] = {BMI160_REG_ACC_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0};
IOLo(acc->bus.busdev_u.spi.csnPin);
spiTransfer(acc->bus.busdev_u.spi.instance, bmi160_tx_buf, bmi160_rx_buf, BUFFER_SIZE); // receive response
IOHi(acc->bus.busdev_u.spi.csnPin);
spiReadWriteBufRB(&acc->gyro->dev, bmi160_tx_buf, bmi160_rx_buf, BUFFER_SIZE); // receive response
acc->ADCRaw[X] = (int16_t)((bmi160_rx_buf[IDX_ACCEL_XOUT_H] << 8) | bmi160_rx_buf[IDX_ACCEL_XOUT_L]);
acc->ADCRaw[Y] = (int16_t)((bmi160_rx_buf[IDX_ACCEL_YOUT_H] << 8) | bmi160_rx_buf[IDX_ACCEL_YOUT_L]);
@ -303,9 +301,7 @@ bool bmi160GyroRead(gyroDev_t *gyro)
uint8_t bmi160_rx_buf[BUFFER_SIZE];
static const uint8_t bmi160_tx_buf[BUFFER_SIZE] = {BMI160_REG_GYR_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0};
IOLo(gyro->bus.busdev_u.spi.csnPin);
spiTransfer(gyro->bus.busdev_u.spi.instance, bmi160_tx_buf, bmi160_rx_buf, BUFFER_SIZE); // receive response
IOHi(gyro->bus.busdev_u.spi.csnPin);
spiReadWriteBufRB(&acc->gyro->dev, bmi160_tx_buf, bmi160_rx_buf, BUFFER_SIZE); // receive response
gyro->gyroADCRaw[X] = (int16_t)((bmi160_rx_buf[IDX_GYRO_XOUT_H] << 8) | bmi160_rx_buf[IDX_GYRO_XOUT_L]);
gyro->gyroADCRaw[Y] = (int16_t)((bmi160_rx_buf[IDX_GYRO_YOUT_H] << 8) | bmi160_rx_buf[IDX_GYRO_YOUT_L]);
@ -317,7 +313,7 @@ bool bmi160GyroRead(gyroDev_t *gyro)
void bmi160SpiGyroInit(gyroDev_t *gyro)
{
BMI160_Init(&gyro->bus);
BMI160_Init(&gyro->dev);
#if defined(USE_MPU_DATA_READY_SIGNAL)
bmi160IntExtiInit(gyro);
#endif
@ -325,7 +321,7 @@ void bmi160SpiGyroInit(gyroDev_t *gyro)
void bmi160SpiAccInit(accDev_t *acc)
{
BMI160_Init(&acc->bus);
BMI160_Init(&acc->dev);
acc->acc_1G = 512 * 8;
}
@ -333,7 +329,7 @@ void bmi160SpiAccInit(accDev_t *acc)
bool bmi160SpiAccDetect(accDev_t *acc)
{
if (bmi160Detect(&acc->bus) == MPU_NONE) {
if (bmi160Detect(&acc->dev) == MPU_NONE) {
return false;
}
@ -346,7 +342,7 @@ bool bmi160SpiAccDetect(accDev_t *acc)
bool bmi160SpiGyroDetect(gyroDev_t *gyro)
{
if (bmi160Detect(&gyro->bus) == MPU_NONE) {
if (bmi160Detect(&gyro->dev) == MPU_NONE) {
return false;
}

91
src/main/drivers/accgyro/accgyro_spi_bmi270.c
View File

@ -125,20 +125,20 @@ typedef enum {
// BMI270 register reads are 16bits with the first byte a "dummy" value 0
// that must be ignored. The result is in the second byte.
static uint8_t bmi270RegisterRead(const busDevice_t *bus, bmi270Register_e registerId)
static uint8_t bmi270RegisterRead(const extDevice_t *dev, bmi270Register_e registerId)
{
uint8_t data[2] = { 0, 0 };
if (spiBusReadRegisterBuffer(bus, registerId, data, 2)) {
if (spiReadRegMskBufRB(dev, registerId, data, 2)) {
return data[1];
} else {
return 0;
}
}
static void bmi270RegisterWrite(const busDevice_t *bus, bmi270Register_e registerId, uint8_t value, unsigned delayMs)
static void bmi270RegisterWrite(const extDevice_t *dev, bmi270Register_e registerId, uint8_t value, unsigned delayMs)
{
spiBusWriteRegister(bus, registerId, value);
spiWriteReg(dev, registerId, value);
if (delayMs) {
delay(delayMs);
}
@ -146,44 +146,41 @@ static void bmi270RegisterWrite(const busDevice_t *bus, bmi270Register_e registe
// Toggle the CS to switch the device into SPI mode.
// Device switches initializes as I2C and switches to SPI on a low to high CS transition
static void bmi270EnableSPI(const busDevice_t *bus)
static void bmi270EnableSPI(const extDevice_t *dev)
{
IOLo(bus->busdev_u.spi.csnPin);
IOLo(dev->busType_u.spi.csnPin);
delay(1);
IOHi(bus->busdev_u.spi.csnPin);
IOHi(dev->busType_u.spi.csnPin);
delay(10);
}
uint8_t bmi270Detect(const busDevice_t *bus)
uint8_t bmi270Detect(const extDevice_t *dev)
{
spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(BMI270_MAX_SPI_CLK_HZ));
bmi270EnableSPI(bus);
spiSetClkDivisor(dev, spiCalculateDivider(BMI270_MAX_SPI_CLK_HZ));
bmi270EnableSPI(dev);
if (bmi270RegisterRead(bus, BMI270_REG_CHIP_ID) == BMI270_CHIP_ID) {
if (bmi270RegisterRead(dev, BMI270_REG_CHIP_ID) == BMI270_CHIP_ID) {
return BMI_270_SPI;
}
return MPU_NONE;
}
static void bmi270UploadConfig(const busDevice_t *bus)
static void bmi270UploadConfig(const extDevice_t *dev)
{
bmi270RegisterWrite(bus, BMI270_REG_PWR_CONF, 0, 1);
bmi270RegisterWrite(bus, BMI270_REG_INIT_CTRL, 0, 1);
bmi270RegisterWrite(dev, BMI270_REG_PWR_CONF, 0, 1);
bmi270RegisterWrite(dev, BMI270_REG_INIT_CTRL, 0, 1);
// Transfer the config file
IOLo(bus->busdev_u.spi.csnPin);
spiTransferByte(bus->busdev_u.spi.instance, BMI270_REG_INIT_DATA);
spiTransfer(bus->busdev_u.spi.instance, bmi270_maximum_fifo_config_file, NULL, sizeof(bmi270_maximum_fifo_config_file));
IOHi(bus->busdev_u.spi.csnPin);
spiWriteRegBuf(dev, BMI270_REG_INIT_DATA, (uint8_t *)bmi270_maximum_fifo_config_file, sizeof(bmi270_maximum_fifo_config_file));
delay(10);
bmi270RegisterWrite(bus, BMI270_REG_INIT_CTRL, 1, 1);
bmi270RegisterWrite(dev, BMI270_REG_INIT_CTRL, 1, 1);
}
static void bmi270Config(const gyroDev_t *gyro)
static void bmi270Config(gyroDev_t *gyro)
{
const busDevice_t *bus = &gyro->bus;
extDevice_t *dev = &gyro->dev;
// If running in hardware_lpf experimental mode then switch to FIFO-based,
// 6.4KHz sampling, unfiltered data vs. the default 3.2KHz with hardware filtering
@ -195,55 +192,55 @@ static void bmi270Config(const gyroDev_t *gyro)
// Perform a soft reset to set all configuration to default
// Delay 100ms before continuing configuration
bmi270RegisterWrite(bus, BMI270_REG_CMD, BMI270_VAL_CMD_SOFTRESET, 100);
bmi270RegisterWrite(dev, BMI270_REG_CMD, BMI270_VAL_CMD_SOFTRESET, 100);
// Toggle the chip into SPI mode
bmi270EnableSPI(bus);
bmi270EnableSPI(dev);
bmi270UploadConfig(bus);
bmi270UploadConfig(dev);
// Configure the FIFO
if (fifoMode) {
bmi270RegisterWrite(bus, BMI270_REG_FIFO_CONFIG_0, BMI270_VAL_FIFO_CONFIG_0, 1);
bmi270RegisterWrite(bus, BMI270_REG_FIFO_CONFIG_1, BMI270_VAL_FIFO_CONFIG_1, 1);
bmi270RegisterWrite(bus, BMI270_REG_FIFO_DOWNS, BMI270_VAL_FIFO_DOWNS, 1);
bmi270RegisterWrite(bus, BMI270_REG_FIFO_WTM_0, BMI270_VAL_FIFO_WTM_0, 1);
bmi270RegisterWrite(bus, BMI270_REG_FIFO_WTM_1, BMI270_VAL_FIFO_WTM_1, 1);
bmi270RegisterWrite(dev, BMI270_REG_FIFO_CONFIG_0, BMI270_VAL_FIFO_CONFIG_0, 1);
bmi270RegisterWrite(dev, BMI270_REG_FIFO_CONFIG_1, BMI270_VAL_FIFO_CONFIG_1, 1);
bmi270RegisterWrite(dev, BMI270_REG_FIFO_DOWNS, BMI270_VAL_FIFO_DOWNS, 1);
bmi270RegisterWrite(dev, BMI270_REG_FIFO_WTM_0, BMI270_VAL_FIFO_WTM_0, 1);
bmi270RegisterWrite(dev, BMI270_REG_FIFO_WTM_1, BMI270_VAL_FIFO_WTM_1, 1);
}
// Configure the accelerometer
bmi270RegisterWrite(bus, BMI270_REG_ACC_CONF, (BMI270_VAL_ACC_CONF_HP << 7) | (BMI270_VAL_ACC_CONF_BWP << 4) | BMI270_VAL_ACC_CONF_ODR800, 1);
bmi270RegisterWrite(dev, BMI270_REG_ACC_CONF, (BMI270_VAL_ACC_CONF_HP << 7) | (BMI270_VAL_ACC_CONF_BWP << 4) | BMI270_VAL_ACC_CONF_ODR800, 1);
// Configure the accelerometer full-scale range
bmi270RegisterWrite(bus, BMI270_REG_ACC_RANGE, BMI270_VAL_ACC_RANGE_16G, 1);
bmi270RegisterWrite(dev, BMI270_REG_ACC_RANGE, BMI270_VAL_ACC_RANGE_16G, 1);
// Configure the gyro
bmi270RegisterWrite(bus, BMI270_REG_GYRO_CONF, (BMI270_VAL_GYRO_CONF_FILTER_PERF << 7) | (BMI270_VAL_GYRO_CONF_NOISE_PERF << 6) | (BMI270_VAL_GYRO_CONF_BWP << 4) | BMI270_VAL_GYRO_CONF_ODR3200, 1);
bmi270RegisterWrite(dev, BMI270_REG_GYRO_CONF, (BMI270_VAL_GYRO_CONF_FILTER_PERF << 7) | (BMI270_VAL_GYRO_CONF_NOISE_PERF << 6) | (BMI270_VAL_GYRO_CONF_BWP << 4) | BMI270_VAL_GYRO_CONF_ODR3200, 1);
// Configure the gyro full-range scale
bmi270RegisterWrite(bus, BMI270_REG_GYRO_RANGE, BMI270_VAL_GYRO_RANGE_2000DPS, 1);
bmi270RegisterWrite(dev, BMI270_REG_GYRO_RANGE, BMI270_VAL_GYRO_RANGE_2000DPS, 1);
// Configure the gyro data ready interrupt
if (fifoMode) {
// Interrupt driven by FIFO watermark level
bmi270RegisterWrite(bus, BMI270_REG_INT_MAP_DATA, BMI270_VAL_INT_MAP_FIFO_WM_INT1, 1);
bmi270RegisterWrite(dev, BMI270_REG_INT_MAP_DATA, BMI270_VAL_INT_MAP_FIFO_WM_INT1, 1);
} else {
// Interrupt driven by data ready
bmi270RegisterWrite(bus, BMI270_REG_INT_MAP_DATA, BMI270_VAL_INT_MAP_DATA_DRDY_INT1, 1);
bmi270RegisterWrite(dev, BMI270_REG_INT_MAP_DATA, BMI270_VAL_INT_MAP_DATA_DRDY_INT1, 1);
}
// Configure the behavior of the INT1 pin
bmi270RegisterWrite(bus, BMI270_REG_INT1_IO_CTRL, BMI270_VAL_INT1_IO_CTRL_PINMODE, 1);
bmi270RegisterWrite(dev, BMI270_REG_INT1_IO_CTRL, BMI270_VAL_INT1_IO_CTRL_PINMODE, 1);
// Configure the device for performance mode
bmi270RegisterWrite(bus, BMI270_REG_PWR_CONF, BMI270_VAL_PWR_CONF, 1);
bmi270RegisterWrite(dev, BMI270_REG_PWR_CONF, BMI270_VAL_PWR_CONF, 1);
// Enable the gyro, accelerometer and temperature sensor - disable aux interface
bmi270RegisterWrite(bus, BMI270_REG_PWR_CTRL, BMI270_VAL_PWR_CTRL, 1);
bmi270RegisterWrite(dev, BMI270_REG_PWR_CTRL, BMI270_VAL_PWR_CTRL, 1);
// Flush the FIFO
if (fifoMode) {
bmi270RegisterWrite(bus, BMI270_REG_CMD, BMI270_VAL_CMD_FIFOFLUSH, 1);
bmi270RegisterWrite(dev, BMI270_REG_CMD, BMI270_VAL_CMD_FIFOFLUSH, 1);
}
}
@ -288,9 +285,7 @@ static bool bmi270AccRead(accDev_t *acc)
uint8_t bmi270_rx_buf[BUFFER_SIZE];
static const uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_ACC_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0};
IOLo(acc->bus.busdev_u.spi.csnPin);
spiTransfer(acc->bus.busdev_u.spi.instance, bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response
IOHi(acc->bus.busdev_u.spi.csnPin);
spiReadWriteBuf(&acc->gyro->dev, (uint8_t *)bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response
acc->ADCRaw[X] = (int16_t)((bmi270_rx_buf[IDX_ACCEL_XOUT_H] << 8) | bmi270_rx_buf[IDX_ACCEL_XOUT_L]);
acc->ADCRaw[Y] = (int16_t)((bmi270_rx_buf[IDX_ACCEL_YOUT_H] << 8) | bmi270_rx_buf[IDX_ACCEL_YOUT_L]);
@ -316,9 +311,7 @@ static bool bmi270GyroReadRegister(gyroDev_t *gyro)
uint8_t bmi270_rx_buf[BUFFER_SIZE];
static const uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_GYR_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0};
IOLo(gyro->bus.busdev_u.spi.csnPin);
spiTransfer(gyro->bus.busdev_u.spi.instance, bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response
IOHi(gyro->bus.busdev_u.spi.csnPin);
spiReadWriteBuf(&gyro->dev, (uint8_t *)bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response
gyro->gyroADCRaw[X] = (int16_t)((bmi270_rx_buf[IDX_GYRO_XOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_XOUT_L]);
gyro->gyroADCRaw[Y] = (int16_t)((bmi270_rx_buf[IDX_GYRO_YOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_YOUT_L]);
@ -351,9 +344,7 @@ static bool bmi270GyroReadFifo(gyroDev_t *gyro)
// Burst read the FIFO length followed by the next 6 bytes containing the gyro axis data for
// the first sample in the queue. It's possible for the FIFO to be empty so we need to check the
// length before using the sample.
IOLo(gyro->bus.busdev_u.spi.csnPin);
spiTransfer(gyro->bus.busdev_u.spi.instance, bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response
IOHi(gyro->bus.busdev_u.spi.csnPin);
spiReadWriteBuf(&gyro->dev, (uint8_t *)bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response
int fifoLength = (uint16_t)((bmi270_rx_buf[IDX_FIFO_LENGTH_H] << 8) | bmi270_rx_buf[IDX_FIFO_LENGTH_L]);
@ -385,7 +376,7 @@ static bool bmi270GyroReadFifo(gyroDev_t *gyro)
// would end up in a lock state of always re-reading the same partial or invalid sample.
if (fifoLength > 0) {
// Partial or additional frames left - flush the FIFO
bmi270RegisterWrite(&gyro->bus, BMI270_REG_CMD, BMI270_VAL_CMD_FIFOFLUSH, 0);
bmi270RegisterWrite(&gyro->dev, BMI270_REG_CMD, BMI270_VAL_CMD_FIFOFLUSH, 0);
}
return dataRead;
@ -456,6 +447,6 @@ bool bmi270SpiGyroDetect(gyroDev_t *gyro)
// watermark reason as an idication of gyro data ready.
uint8_t bmi270InterruptStatus(gyroDev_t *gyro)
{
return bmi270RegisterRead(&gyro->bus, BMI270_REG_INT_STATUS_1);
return bmi270RegisterRead(&gyro->dev, BMI270_REG_INT_STATUS_1);
}
#endif // USE_ACCGYRO_BMI270

2
src/main/drivers/accgyro/accgyro_spi_bmi270.h
View File

@ -22,7 +22,7 @@
#include "drivers/bus.h"
uint8_t bmi270Detect(const busDevice_t *bus);
uint8_t bmi270Detect(const extDevice_t *dev);
bool bmi270SpiAccDetect(accDev_t *acc);
bool bmi270SpiGyroDetect(gyroDev_t *gyro);
uint8_t bmi270InterruptStatus(gyroDev_t *gyro);

44
src/main/drivers/accgyro/accgyro_spi_icm20649.c
View File

@ -50,7 +50,9 @@ static void icm20649SpiInit(const busDevice_t *bus)
return;
}
spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(ICM20649_MAX_SPI_CLK_HZ));
// all registers can be read/written at full speed (7MHz +-10%)
// TODO verify that this works at 9MHz and 10MHz on non F7
spiSetClkDivisor(dev, spiCalculateDivider(ICM20649_MAX_SPI_CLK_HZ));
hardwareInitialised = true;
}
@ -59,18 +61,18 @@ uint8_t icm20649SpiDetect(const busDevice_t *bus)
{
icm20649SpiInit(bus);
spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(ICM20649_MAX_SPI_CLK_HZ));
spiSetClkDivisor(dev, spiCalculateDivider(ICM20649_MAX_SPI_CLK_HZ));
spiBusWriteRegister(bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // select bank 0 just to be safe
spiWriteReg(bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // select bank 0 just to be safe
delay(15);
spiBusWriteRegister(bus, ICM20649_RA_PWR_MGMT_1, ICM20649_BIT_RESET);
spiWriteReg(bus, ICM20649_RA_PWR_MGMT_1, ICM20649_BIT_RESET);
uint8_t icmDetected = MPU_NONE;
uint8_t attemptsRemaining = 20;
do {
delay(150);
const uint8_t whoAmI = spiBusReadRegister(bus, ICM20649_RA_WHO_AM_I);
const uint8_t whoAmI = spiReadRegMsk(bus, ICM20649_RA_WHO_AM_I);
if (whoAmI == ICM20649_WHO_AM_I_CONST) {
icmDetected = ICM_20649_SPI;
} else {
@ -94,14 +96,14 @@ void icm20649AccInit(accDev_t *acc)
// 1,024 LSB/g 30g
acc->acc_1G = acc->acc_high_fsr ? 1024 : 2048;
spiSetDivisor(acc->bus.busdev_u.spi.instance, spiCalculateDivider(ICM20649_MAX_SPI_CLK_HZ));
spiSetClkDivisor(dev, spiCalculateDivider(ICM20649_MAX_SPI_CLK_HZ));
spiBusWriteRegister(&acc->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2
spiWriteReg(&acc->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2
delay(15);
const uint8_t acc_fsr = acc->acc_high_fsr ? ICM20649_FSR_30G : ICM20649_FSR_16G;
spiBusWriteRegister(&acc->bus, ICM20649_RA_ACCEL_CONFIG, acc_fsr << 1);
spiWriteReg(&acc->bus, ICM20649_RA_ACCEL_CONFIG, acc_fsr << 1);
delay(15);
spiBusWriteRegister(&acc->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // back to bank 0
spiWriteReg(&acc->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // back to bank 0
delay(15);
}
@ -122,15 +124,15 @@ void icm20649GyroInit(gyroDev_t *gyro)
{
mpuGyroInit(gyro);
spiSetDivisor(gyro->bus.busdev_u.spi.instance, spiCalculateDivider(ICM20649_MAX_SPI_CLK_HZ)); // ensure proper speed
spiSetClkDivisor(dev, spiCalculateDivider(ICM20649_MAX_SPI_CLK_HZ)); // ensure proper speed
spiBusWriteRegister(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // select bank 0 just to be safe
spiWriteReg(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4); // select bank 0 just to be safe
delay(15);
spiBusWriteRegister(&gyro->bus, ICM20649_RA_PWR_MGMT_1, ICM20649_BIT_RESET);
spiWriteReg(&gyro->bus, ICM20649_RA_PWR_MGMT_1, ICM20649_BIT_RESET);
delay(100);
spiBusWriteRegister(&gyro->bus, ICM20649_RA_PWR_MGMT_1, INV_CLK_PLL);
spiWriteReg(&gyro->bus, ICM20649_RA_PWR_MGMT_1, INV_CLK_PLL);
delay(15);
spiBusWriteRegister(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2
spiWriteReg(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 2 << 4); // config in bank 2
delay(15);
const uint8_t gyro_fsr = gyro->gyro_high_fsr ? ICM20649_FSR_4000DPS : ICM20649_FSR_2000DPS;
@ -141,20 +143,20 @@ void icm20649GyroInit(gyroDev_t *gyro)
// the ICM20649 only has a single 9KHz DLPF cutoff.
uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_1100_Hz ? 0 : 1; // deactivate GYRO_FCHOICE for sample rates over 1kHz (opposite of other invensense chips)
raGyroConfigData |= gyro_fsr << 1;
spiBusWriteRegister(&gyro->bus, ICM20649_RA_GYRO_CONFIG_1, raGyroConfigData);
spiWriteReg(&gyro->bus, ICM20649_RA_GYRO_CONFIG_1, raGyroConfigData);
delay(15);
spiBusWriteRegister(&gyro->bus, ICM20649_RA_GYRO_SMPLRT_DIV, gyro->mpuDividerDrops); // Get Divider Drops
spiWriteReg(&gyro->bus, ICM20649_RA_GYRO_SMPLRT_DIV, gyro->mpuDividerDrops); // Get Divider Drops
delay(100);
// Data ready interrupt configuration
// back to bank 0
spiBusWriteRegister(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4);
spiWriteReg(&gyro->bus, ICM20649_RA_REG_BANK_SEL, 0 << 4);
delay(15);
spiBusWriteRegister(&gyro->bus, ICM20649_RA_INT_PIN_CFG, 0x11); // INT_ANYRD_2CLEAR, BYPASS_EN
spiWriteReg(&gyro->bus, ICM20649_RA_INT_PIN_CFG, 0x11); // INT_ANYRD_2CLEAR, BYPASS_EN
delay(15);
#ifdef USE_MPU_DATA_READY_SIGNAL
spiBusWriteRegister(&gyro->bus, ICM20649_RA_INT_ENABLE_1, 0x01);
spiWriteReg(&gyro->bus, ICM20649_RA_INT_ENABLE_1, 0x01);
#endif
}
@ -178,7 +180,7 @@ bool icm20649GyroReadSPI(gyroDev_t *gyro)
static const uint8_t dataToSend[7] = {ICM20649_RA_GYRO_XOUT_H | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
uint8_t data[7];
const bool ack = spiBusTransfer(&gyro->bus, dataToSend, data, 7);
const bool ack = spiReadWriteBufRB(&gyro->bus, dataToSend, data, 7);
if (!ack) {
return false;
}
@ -194,7 +196,7 @@ bool icm20649AccRead(accDev_t *acc)
{
uint8_t data[6];
const bool ack = spiBusReadRegisterBuffer(&acc->bus, ICM20649_RA_ACCEL_XOUT_H, data, 6);
const bool ack = spiReadRegMskBufRB(&acc->bus, ICM20649_RA_ACCEL_XOUT_H, data, 6);
if (!ack) {
return false;
}

41
src/main/drivers/accgyro/accgyro_spi_icm20689.c
View File

@ -37,13 +37,14 @@
#include "drivers/time.h"
// 10 MHz max SPI frequency
#define ICM20689_MAX_SPI_CLK_HZ 10000000
// 10 MHz max SPI frequency for intialisation
#define ICM20689_MAX_SPI_INIT_CLK_HZ 1000000
static void icm20689SpiInit(const busDevice_t *bus)
static void icm20689SpiInit(const extDevice_t *dev)
{
static bool hardwareInitialised = false;
@ -52,28 +53,28 @@ static void icm20689SpiInit(const busDevice_t *bus)
}
spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(ICM20689_MAX_SPI_CLK_HZ));
spiSetClkDivisor(dev, spiCalculateDivider(ICM20689_MAX_SPI_CLK_HZ));
hardwareInitialised = true;
}
uint8_t icm20689SpiDetect(const busDevice_t *bus)
uint8_t icm20689SpiDetect(const extDevice_t *dev)
{
icm20689SpiInit(bus);
icm20689SpiInit(dev);
spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(ICM20689_MAX_SPI_INIT_CLK_HZ));
spiSetClkDivisor(dev, spiCalculateDivider(ICM20689_MAX_SPI_INIT_CLK_HZ));
// reset the device configuration
spiBusWriteRegister(bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
spiWriteReg(dev, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
delay(100);
// reset the device signal paths
spiBusWriteRegister(bus, MPU_RA_SIGNAL_PATH_RESET, 0x03);
spiWriteReg(dev, MPU_RA_SIGNAL_PATH_RESET, 0x03);
delay(100);
uint8_t icmDetected;
const uint8_t whoAmI = spiBusReadRegister(bus, MPU_RA_WHO_AM_I);
const uint8_t whoAmI = spiReadRegMsk(dev, MPU_RA_WHO_AM_I);
switch (whoAmI) {
case ICM20601_WHO_AM_I_CONST:
@ -93,7 +94,7 @@ uint8_t icm20689SpiDetect(const busDevice_t *bus)
break;
}
spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(ICM20689_MAX_SPI_CLK_HZ));
spiSetClkDivisor(dev, spiCalculateDivider(ICM20689_MAX_SPI_CLK_HZ));
return icmDetected;
}
@ -114,7 +115,7 @@ bool icm20689SpiAccDetect(accDev_t *acc)
}
acc->initFn = icm20689AccInit;
acc->readFn = mpuAccRead;
acc->readFn = mpuAccReadSPI;
return true;
}
@ -123,32 +124,32 @@ void icm20689GyroInit(gyroDev_t *gyro)
{
mpuGyroInit(gyro);
spiSetDivisor(gyro->bus.busdev_u.spi.instance, spiCalculateDivider(ICM20689_MAX_SPI_INIT_CLK_HZ));
spiSetClkDivisor(&gyro->dev, spiCalculateDivider(ICM20689_MAX_SPI_INIT_CLK_HZ));
// Device was already reset during detection so proceed with configuration
spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
spiWriteReg(&gyro->dev, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
delay(15);
spiBusWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
spiWriteReg(&gyro->dev, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
delay(15);
spiBusWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
spiWriteReg(&gyro->dev, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
delay(15);
spiBusWriteRegister(&gyro->bus, MPU_RA_CONFIG, mpuGyroDLPF(gyro));
spiWriteReg(&gyro->dev, MPU_RA_CONFIG, mpuGyroDLPF(gyro));
delay(15);
spiBusWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyro->mpuDividerDrops); // Get Divider Drops
spiWriteReg(&gyro->dev, MPU_RA_SMPLRT_DIV, gyro->mpuDividerDrops); // Get Divider Drops
delay(100);
// Data ready interrupt configuration
// spiBusWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN
spiBusWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR, BYPASS_EN
// spiWriteReg(&gyro->dev, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN
spiWriteReg(&gyro->dev, MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR, BYPASS_EN
delay(15);
#ifdef USE_MPU_DATA_READY_SIGNAL
spiBusWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable
spiWriteReg(&gyro->dev, MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable
#endif
spiSetDivisor(gyro->bus.busdev_u.spi.instance, spiCalculateDivider(ICM20689_MAX_SPI_CLK_HZ));
spiSetClkDivisor(&gyro->dev, spiCalculateDivider(ICM20689_MAX_SPI_CLK_HZ));
}
bool icm20689SpiGyroDetect(gyroDev_t *gyro)

2
src/main/drivers/accgyro/accgyro_spi_icm20689.h
View File

@ -30,7 +30,7 @@ bool icm20689GyroDetect(gyroDev_t *gyro);
void icm20689AccInit(accDev_t *acc);
void icm20689GyroInit(gyroDev_t *gyro);
uint8_t icm20689SpiDetect(const busDevice_t *bus);
uint8_t icm20689SpiDetect(const extDevice_t *dev);
bool icm20689SpiAccDetect(accDev_t *acc);
bool icm20689SpiGyroDetect(gyroDev_t *gyro);

46
src/main/drivers/accgyro/accgyro_spi_icm42605.c
View File

@ -95,7 +95,7 @@
#define ICM42605_UI_DRDY_INT1_EN_DISABLED (0 << 3)
#define ICM42605_UI_DRDY_INT1_EN_ENABLED (1 << 3)
static void icm42605SpiInit(const busDevice_t *bus)
static void icm42605SpiInit(const extDevice_t *dev)
{
static bool hardwareInitialised = false;
@ -104,24 +104,24 @@ static void icm42605SpiInit(const busDevice_t *bus)
}
spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(ICM42605_MAX_SPI_CLK_HZ));
spiSetClkDivisor(dev, spiCalculateDivider(ICM42605_MAX_SPI_CLK_HZ));
hardwareInitialised = true;
}
uint8_t icm42605SpiDetect(const busDevice_t *bus)
uint8_t icm42605SpiDetect(const extDevice_t *dev)
{
icm42605SpiInit(bus);
icm42605SpiInit(dev);
spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(ICM42605_MAX_SPI_INIT_CLK_HZ));
spiSetClkDivisor(dev, spiCalculateDivider(ICM42605_MAX_SPI_INIT_CLK_HZ));
spiBusWriteRegister(bus, ICM42605_RA_PWR_MGMT0, 0x00);
spiWriteReg(dev, ICM42605_RA_PWR_MGMT0, 0x00);
uint8_t icmDetected = MPU_NONE;
uint8_t attemptsRemaining = 20;
do {
delay(150);
const uint8_t whoAmI = spiBusReadRegister(bus, MPU_RA_WHO_AM_I);
const uint8_t whoAmI = spiReadRegMsk(dev, MPU_RA_WHO_AM_I);
switch (whoAmI) {
case ICM42605_WHO_AM_I_CONST:
icmDetected = ICM_42605_SPI;
@ -138,7 +138,7 @@ uint8_t icm42605SpiDetect(const busDevice_t *bus)
}
} while (attemptsRemaining--);
spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(ICM42605_MAX_SPI_CLK_HZ));
spiSetClkDivisor(dev, spiCalculateDivider(ICM42605_MAX_SPI_CLK_HZ));
return icmDetected;
}
@ -152,7 +152,7 @@ bool icm42605AccRead(accDev_t *acc)
{
uint8_t data[6];
const bool ack = busReadRegisterBuffer(&acc->bus, ICM42605_RA_ACCEL_DATA_X1, data, 6);
const bool ack = busReadRegisterBuffer(&acc->gyro->dev, ICM42605_RA_ACCEL_DATA_X1, data, 6);
if (!ack) {
return false;
}
@ -194,9 +194,9 @@ void icm42605GyroInit(gyroDev_t *gyro)
{
mpuGyroInit(gyro);
spiSetDivisor(gyro->bus.busdev_u.spi.instance, spiCalculateDivider(ICM42605_MAX_SPI_INIT_CLK_HZ));
spiSetClkDivisor(&gyro->dev, spiCalculateDivider(ICM42605_MAX_SPI_INIT_CLK_HZ));
spiBusWriteRegister(&gyro->bus, ICM42605_RA_PWR_MGMT0, ICM42605_PWR_MGMT0_TEMP_DISABLE_OFF | ICM42605_PWR_MGMT0_ACCEL_MODE_LN | ICM42605_PWR_MGMT0_GYRO_MODE_LN);
spiWriteReg(&gyro->dev, ICM42605_RA_PWR_MGMT0, ICM42605_PWR_MGMT0_TEMP_DISABLE_OFF | ICM42605_PWR_MGMT0_ACCEL_MODE_LN | ICM42605_PWR_MGMT0_GYRO_MODE_LN);
delay(15);
uint8_t outputDataRate = 0;
@ -220,39 +220,39 @@ void icm42605GyroInit(gyroDev_t *gyro)
}
STATIC_ASSERT(INV_FSR_2000DPS == 3, "INV_FSR_2000DPS must be 3 to generate correct value");
spiBusWriteRegister(&gyro->bus, ICM42605_RA_GYRO_CONFIG0, (3 - INV_FSR_2000DPS) << 5 | (outputDataRate & 0x0F));
spiWriteReg(&gyro->dev, ICM42605_RA_GYRO_CONFIG0, (3 - INV_FSR_2000DPS) << 5 | (outputDataRate & 0x0F));
delay(15);
STATIC_ASSERT(INV_FSR_16G == 3, "INV_FSR_16G must be 3 to generate correct value");
spiBusWriteRegister(&gyro->bus, ICM42605_RA_ACCEL_CONFIG0, (3 - INV_FSR_16G) << 5 | (outputDataRate & 0x0F));
spiWriteReg(&gyro->dev, ICM42605_RA_ACCEL_CONFIG0, (3 - INV_FSR_16G) << 5 | (outputDataRate & 0x0F));
delay(15);
spiBusWriteRegister(&gyro->bus, ICM42605_RA_GYRO_ACCEL_CONFIG0, ICM42605_ACCEL_UI_FILT_BW_LOW_LATENCY | ICM42605_GYRO_UI_FILT_BW_LOW_LATENCY);
spiWriteReg(&gyro->dev, ICM42605_RA_GYRO_ACCEL_CONFIG0, ICM42605_ACCEL_UI_FILT_BW_LOW_LATENCY | ICM42605_GYRO_UI_FILT_BW_LOW_LATENCY);
spiBusWriteRegister(&gyro->bus, ICM42605_RA_INT_CONFIG, ICM42605_INT1_MODE_PULSED | ICM42605_INT1_DRIVE_CIRCUIT_PP | ICM42605_INT1_POLARITY_ACTIVE_HIGH);
spiBusWriteRegister(&gyro->bus, ICM42605_RA_INT_CONFIG0, ICM42605_UI_DRDY_INT_CLEAR_ON_SBR);
spiWriteReg(&gyro->dev, ICM42605_RA_INT_CONFIG, ICM42605_INT1_MODE_PULSED | ICM42605_INT1_DRIVE_CIRCUIT_PP | ICM42605_INT1_POLARITY_ACTIVE_HIGH);
spiWriteReg(&gyro->dev, ICM42605_RA_INT_CONFIG0, ICM42605_UI_DRDY_INT_CLEAR_ON_SBR);
#ifdef USE_MPU_DATA_READY_SIGNAL
spiBusWriteRegister(&gyro->bus, ICM42605_RA_INT_SOURCE0, ICM42605_UI_DRDY_INT1_EN_ENABLED);
spiWriteReg(&gyro->dev, ICM42605_RA_INT_SOURCE0, ICM42605_UI_DRDY_INT1_EN_ENABLED);
uint8_t intConfig1Value = spiBusReadRegister(&gyro->bus, ICM42605_RA_INT_CONFIG1);
uint8_t intConfig1Value = spiReadRegMsk(&gyro->dev, ICM42605_RA_INT_CONFIG1);
// Datasheet says: "User should change setting to 0 from default setting of 1, for proper INT1 and INT2 pin operation"
intConfig1Value &= ~(1 << ICM42605_INT_ASYNC_RESET_BIT);
intConfig1Value |= (ICM42605_INT_TPULSE_DURATION_8 | ICM42605_INT_TDEASSERT_DISABLED);
spiBusWriteRegister(&gyro->bus, ICM42605_RA_INT_CONFIG1, intConfig1Value);
spiWriteReg(&gyro->dev, ICM42605_RA_INT_CONFIG1, intConfig1Value);
#endif
//
spiSetDivisor(gyro->bus.busdev_u.spi.instance, spiCalculateDivider(ICM42605_MAX_SPI_CLK_HZ));
spiSetClkDivisor(&gyro->dev, spiCalculateDivider(ICM42605_MAX_SPI_CLK_HZ));
}
bool icm42605GyroReadSPI(gyroDev_t *gyro)
{
static const uint8_t dataToSend[7] = {ICM42605_RA_GYRO_DATA_X1 | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
uint8_t data[7];
STATIC_DMA_DATA_AUTO uint8_t dataToSend[7] = {ICM42605_RA_GYRO_DATA_X1 | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
STATIC_DMA_DATA_AUTO uint8_t data[7];
const bool ack = spiBusTransfer(&gyro->bus, dataToSend, data, 7);
const bool ack = spiReadWriteBufRB(&gyro->dev, dataToSend, data, 7);
if (!ack) {
return false;
}

2
src/main/drivers/accgyro/accgyro_spi_icm42605.h
View File

@ -28,7 +28,7 @@ bool icm42605GyroDetect(gyroDev_t *gyro);
void icm42605AccInit(accDev_t *acc);
void icm42605GyroInit(gyroDev_t *gyro);
uint8_t icm42605SpiDetect(const busDevice_t *bus);
uint8_t icm42605SpiDetect(const extDevice_t *dev);
bool icm42605SpiAccDetect(accDev_t *acc);
bool icm42605SpiGyroDetect(gyroDev_t *gyro);

15
src/main/drivers/accgyro/accgyro_spi_l3gd20.c
View File

@ -101,18 +101,18 @@ static void l3gd20IntExtiInit(gyroDev_t *gyro)
void l3gd20GyroInit(gyroDev_t *gyro)
{
spiSetDivisor(gyro->bus.busdev_u.spi.instance, spiCalculateDivider(L3GD20_MAX_SPI_CLK_HZ));
spiSetClkDivisor(dev, spiCalculateDivider(L3GD20_MAX_SPI_CLK_HZ));
spiBusWriteRegister(&gyro->bus, CTRL_REG5_ADDR, BOOT);
spiWriteReg(&gyro->bus, CTRL_REG5_ADDR, BOOT);
delayMicroseconds(100);
spiBusWriteRegister(&gyro->bus, CTRL_REG1_ADDR, MODE_ACTIVE | OUTPUT_DATARATE_3 | AXES_ENABLE | BANDWIDTH_3);
//spiBusWriteRegister(&gyro->bus, CTRL_REG1_ADDR. MODE_ACTIVE | OUTPUT_DATARATE_3 | AXES_ENABLE | BANDWIDTH_4);
spiWriteReg(&gyro->bus, CTRL_REG1_ADDR, MODE_ACTIVE | OUTPUT_DATARATE_3 | AXES_ENABLE | BANDWIDTH_3);
//spiWriteReg(&gyro->bus, CTRL_REG1_ADDR. MODE_ACTIVE | OUTPUT_DATARATE_3 | AXES_ENABLE | BANDWIDTH_4);
delayMicroseconds(1);
spiBusWriteRegister(&gyro->bus, CTRL_REG4_ADDR, BLOCK_DATA_UPDATE_CONTINUOUS | BLE_MSB | FULLSCALE_2000);
spiWriteReg(&gyro->bus, CTRL_REG4_ADDR, BLOCK_DATA_UPDATE_CONTINUOUS | BLE_MSB | FULLSCALE_2000);
delay(100);
@ -126,7 +126,10 @@ static bool l3gd20GyroRead(gyroDev_t *gyro)
{
uint8_t buf[6];
spiBusReadRegisterBuffer(&gyro->bus, OUT_X_L_ADDR | READ_CMD | MULTIPLEBYTE_CMD,buf, sizeof(buf));
const bool ack = spiReadRegMskBufRB(&gyro->bus, OUT_X_L_ADDR | READ_CMD | MULTIPLEBYTE_CMD,buf, sizeof(buf));
if (!ack) {
return false;
}
gyro->gyroADCRaw[0] = (int16_t)((buf[0] << 8) | buf[1]);
gyro->gyroADCRaw[1] = (int16_t)((buf[2] << 8) | buf[3]);

8
src/main/drivers/accgyro/accgyro_spi_lsm6dso.c
View File

@ -54,8 +54,8 @@ bool lsm6dsoAccRead(accDev_t *acc)
uint8_t lsm6dso_rx_buf[BUFFER_SIZE];
const busDevice_t *busdev = &acc->bus;
busReadRegisterBuffer(busdev, LSM6DSO_REG_OUTX_L_A, lsm6dso_rx_buf, BUFFER_SIZE);
extDevice_t *dev = &acc->gyro->dev;
busReadRegisterBuffer(dev, LSM6DSO_REG_OUTX_L_A, lsm6dso_rx_buf, BUFFER_SIZE);
acc->ADCRaw[X] = (int16_t)((lsm6dso_rx_buf[IDX_ACCEL_XOUT_H] << 8) | lsm6dso_rx_buf[IDX_ACCEL_XOUT_L]);
acc->ADCRaw[Y] = (int16_t)((lsm6dso_rx_buf[IDX_ACCEL_YOUT_H] << 8) | lsm6dso_rx_buf[IDX_ACCEL_YOUT_L]);
@ -78,8 +78,8 @@ bool lsm6dsoGyroRead(gyroDev_t *gyro)
uint8_t lsm6dso_rx_buf[BUFFER_SIZE];
const busDevice_t *busdev = &gyro->bus;
busReadRegisterBuffer(busdev, LSM6DSO_REG_OUTX_L_G, lsm6dso_rx_buf, BUFFER_SIZE);
extDevice_t *dev = &gyro->dev;
busReadRegisterBuffer(dev, LSM6DSO_REG_OUTX_L_G, lsm6dso_rx_buf, BUFFER_SIZE);
gyro->gyroADCRaw[X] = (int16_t)((lsm6dso_rx_buf[IDX_GYRO_XOUT_H] << 8) | lsm6dso_rx_buf[IDX_GYRO_XOUT_L]);
gyro->gyroADCRaw[Y] = (int16_t)((lsm6dso_rx_buf[IDX_GYRO_YOUT_H] << 8) | lsm6dso_rx_buf[IDX_GYRO_YOUT_L]);

2
src/main/drivers/accgyro/accgyro_spi_lsm6dso.h
View File

@ -56,7 +56,7 @@ typedef enum {
} lsm6dsoRegister_e;
// Contained in accgyro_spi_lsm6dso_init.c which is size-optimized
uint8_t lsm6dsoDetect(const busDevice_t *bus);
uint8_t lsm6dsoDetect(const extDevice_t *dev);
bool lsm6dsoSpiAccDetect(accDev_t *acc);
bool lsm6dsoSpiGyroDetect(gyroDev_t *gyro);

38
src/main/drivers/accgyro/accgyro_spi_lsm6dso_init.c
View File

@ -78,12 +78,12 @@ typedef enum {
LSM6DSO_MASK_CTRL9_XL = 0x02, // 0b00000010
} lsm6dsoConfigMasks_e;
uint8_t lsm6dsoDetect(const busDevice_t *bus)
uint8_t lsm6dsoDetect(const extDevice_t *dev)
{
uint8_t chipID = 0;
spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(LSM6DSO_MAX_SPI_CLK_HZ));
spiSetClkDivisor(dev, spiCalculateDivider(LSM6DSO_MAX_SPI_CLK_HZ));
if (busReadRegisterBuffer(bus, LSM6DSO_REG_WHO_AM_I, &chipID, 1)) {
if (busReadRegisterBuffer(dev, LSM6DSO_REG_WHO_AM_I, &chipID, 1)) {
if (chipID == LSM6DSO_CHIP_ID) {
return LSM6DSO_SPI;
}
@ -92,60 +92,60 @@ uint8_t lsm6dsoDetect(const busDevice_t *bus)
return MPU_NONE;
}
static void lsm6dsoWriteRegister(const busDevice_t *bus, lsm6dsoRegister_e registerID, uint8_t value, unsigned delayMs)
static void lsm6dsoWriteRegister(const extDevice_t *dev, lsm6dsoRegister_e registerID, uint8_t value, unsigned delayMs)
{
busWriteRegister(bus, registerID, value);
busWriteRegister(dev, registerID, value);
if (delayMs) {
delay(delayMs);
}
}
static void lsm6dsoWriteRegisterBits(const busDevice_t *bus, lsm6dsoRegister_e registerID, lsm6dsoConfigMasks_e mask, uint8_t value, unsigned delayMs)
static void lsm6dsoWriteRegisterBits(const extDevice_t *dev, lsm6dsoRegister_e registerID, lsm6dsoConfigMasks_e mask, uint8_t value, unsigned delayMs)
{
uint8_t newValue;
if (busReadRegisterBuffer(bus, registerID, &newValue, 1)) {
if (busReadRegisterBuffer(dev, registerID, &newValue, 1)) {
delayMicroseconds(2);
newValue = (newValue & ~mask) | value;
lsm6dsoWriteRegister(bus, registerID, newValue, delayMs);
lsm6dsoWriteRegister(dev, registerID, newValue, delayMs);
}
}
static void lsm6dsoConfig(const gyroDev_t *gyro)
static void lsm6dsoConfig(gyroDev_t *gyro)
{
const busDevice_t *bus = &gyro->bus;
extDevice_t *dev = &gyro->dev;
// Reset the device (wait 100ms before continuing config)
lsm6dsoWriteRegisterBits(bus, LSM6DSO_REG_CTRL3_C, LSM6DSO_MASK_CTRL3_C_RESET, BIT(0), 100);
lsm6dsoWriteRegisterBits(dev, LSM6DSO_REG_CTRL3_C, LSM6DSO_MASK_CTRL3_C_RESET, BIT(0), 100);
// Configure interrupt pin 1 for gyro data ready only
lsm6dsoWriteRegister(bus, LSM6DSO_REG_INT1_CTRL, LSM6DSO_VAL_INT1_CTRL, 1);
lsm6dsoWriteRegister(dev, LSM6DSO_REG_INT1_CTRL, LSM6DSO_VAL_INT1_CTRL, 1);
// Disable interrupt pin 2
lsm6dsoWriteRegister(bus, LSM6DSO_REG_INT2_CTRL, LSM6DSO_VAL_INT2_CTRL, 1);
lsm6dsoWriteRegister(dev, LSM6DSO_REG_INT2_CTRL, LSM6DSO_VAL_INT2_CTRL, 1);
// Configure the accelerometer
// 833hz ODR, 16G scale, use LPF1 output
lsm6dsoWriteRegister(bus, LSM6DSO_REG_CTRL1_XL, (LSM6DSO_VAL_CTRL1_XL_ODR833 << 4) | (LSM6DSO_VAL_CTRL1_XL_16G << 2) | (LSM6DSO_VAL_CTRL1_XL_LPF1 << 1), 1);
lsm6dsoWriteRegister(dev, LSM6DSO_REG_CTRL1_XL, (LSM6DSO_VAL_CTRL1_XL_ODR833 << 4) | (LSM6DSO_VAL_CTRL1_XL_16G << 2) | (LSM6DSO_VAL_CTRL1_XL_LPF1 << 1), 1);
// Configure the gyro
// 6664hz ODR, 2000dps scale
lsm6dsoWriteRegister(bus, LSM6DSO_REG_CTRL2_G, (LSM6DSO_VAL_CTRL2_G_ODR6664 << 4) | (LSM6DSO_VAL_CTRL2_G_2000DPS << 2), 1);
lsm6dsoWriteRegister(dev, LSM6DSO_REG_CTRL2_G, (LSM6DSO_VAL_CTRL2_G_ODR6664 << 4) | (LSM6DSO_VAL_CTRL2_G_2000DPS << 2), 1);
// Configure control register 3
// latch LSB/MSB during reads; set interrupt pins active high; set interrupt pins push/pull; set 4-wire SPI; enable auto-increment burst reads
lsm6dsoWriteRegisterBits(bus, LSM6DSO_REG_CTRL3_C, LSM6DSO_MASK_CTRL3_C, (LSM6DSO_VAL_CTRL3_C_BDU | LSM6DSO_VAL_CTRL3_C_H_LACTIVE | LSM6DSO_VAL_CTRL3_C_PP_OD | LSM6DSO_VAL_CTRL3_C_SIM | LSM6DSO_VAL_CTRL3_C_IF_INC), 1);
lsm6dsoWriteRegisterBits(dev, LSM6DSO_REG_CTRL3_C, LSM6DSO_MASK_CTRL3_C, (LSM6DSO_VAL_CTRL3_C_BDU | LSM6DSO_VAL_CTRL3_C_H_LACTIVE | LSM6DSO_VAL_CTRL3_C_PP_OD | LSM6DSO_VAL_CTRL3_C_SIM | LSM6DSO_VAL_CTRL3_C_IF_INC), 1);
// Configure control register 4
// enable accelerometer high performane mode; set gyro LPF1 cutoff to 335.5hz
lsm6dsoWriteRegisterBits(bus, LSM6DSO_REG_CTRL4_C, LSM6DSO_MASK_CTRL4_C, (LSM6DSO_VAL_CTRL4_C_I2C_DISABLE | LSM6DSO_VAL_CTRL4_C_LPF1_SEL_G), 1);
lsm6dsoWriteRegisterBits(dev, LSM6DSO_REG_CTRL4_C, LSM6DSO_MASK_CTRL4_C, (LSM6DSO_VAL_CTRL4_C_I2C_DISABLE | LSM6DSO_VAL_CTRL4_C_LPF1_SEL_G), 1);
// Configure control register 6
// disable I2C interface; enable gyro LPF1
lsm6dsoWriteRegisterBits(bus, LSM6DSO_REG_CTRL6_C, LSM6DSO_MASK_CTRL6_C, (LSM6DSO_VAL_CTRL6_C_XL_HM_MODE | LSM6DSO_VAL_CTRL6_C_FTYPE_335HZ), 1);
lsm6dsoWriteRegisterBits(dev, LSM6DSO_REG_CTRL6_C, LSM6DSO_MASK_CTRL6_C, (LSM6DSO_VAL_CTRL6_C_XL_HM_MODE | LSM6DSO_VAL_CTRL6_C_FTYPE_335HZ), 1);
// Configure control register 9
// disable I3C interface
lsm6dsoWriteRegisterBits(bus, LSM6DSO_REG_CTRL9_XL, LSM6DSO_MASK_CTRL9_XL, LSM6DSO_VAL_CTRL9_XL_I3C_DISABLE, 1);
lsm6dsoWriteRegisterBits(dev, LSM6DSO_REG_CTRL9_XL, LSM6DSO_MASK_CTRL9_XL, LSM6DSO_VAL_CTRL9_XL_I3C_DISABLE, 1);
}
#if defined(USE_GYRO_EXTI) && defined(USE_MPU_DATA_READY_SIGNAL)

43
src/main/drivers/accgyro/accgyro_spi_mpu6000.c
View File

@ -107,13 +107,13 @@ void mpu6000SpiGyroInit(gyroDev_t *gyro)
mpu6000AccAndGyroInit(gyro);
spiSetDivisor(gyro->bus.busdev_u.spi.instance, spiCalculateDivider(MPU6000_MAX_SPI_INIT_CLK_HZ));
spiSetClkDivisor(&gyro->dev, spiCalculateDivider(MPU6000_MAX_SPI_INIT_CLK_HZ));
// Accel and Gyro DLPF Setting
spiBusWriteRegister(&gyro->bus, MPU6000_CONFIG, mpuGyroDLPF(gyro));
spiWriteReg(&gyro->dev, MPU6000_CONFIG, mpuGyroDLPF(gyro));
delayMicroseconds(1);
spiSetDivisor(gyro->bus.busdev_u.spi.instance, spiCalculateDivider(MPU6000_MAX_SPI_CLK_HZ));
spiSetClkDivisor(&gyro->dev, spiCalculateDivider(MPU6000_MAX_SPI_CLK_HZ));
mpuGyroRead(gyro);
@ -127,25 +127,26 @@ void mpu6000SpiAccInit(accDev_t *acc)
acc->acc_1G = 512 * 4;
}
uint8_t mpu6000SpiDetect(const busDevice_t *bus)
uint8_t mpu6000SpiDetect(const extDevice_t *dev)
{
spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(MPU6000_MAX_SPI_INIT_CLK_HZ));
spiSetClkDivisor(dev, spiCalculateDivider(MPU6000_MAX_SPI_INIT_CLK_HZ));
// reset the device configuration
spiBusWriteRegister(bus, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
spiWriteReg(dev, MPU_RA_PWR_MGMT_1, BIT_H_RESET);
delay(100); // datasheet specifies a 100ms delay after reset
// reset the device signal paths
spiBusWriteRegister(bus, MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP);
spiWriteReg(dev, MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP);
delay(100); // datasheet specifies a 100ms delay after signal path reset
const uint8_t whoAmI = spiBusReadRegister(bus, MPU_RA_WHO_AM_I);
const uint8_t whoAmI = spiReadRegMsk(dev, MPU_RA_WHO_AM_I);
delayMicroseconds(1); // Ensure CS high time is met which is violated on H7 without this delay
uint8_t detectedSensor = MPU_NONE;
if (whoAmI == MPU6000_WHO_AM_I_CONST) {
const uint8_t productID = spiBusReadRegister(bus, MPU_RA_PRODUCT_ID);
const uint8_t productID = spiReadRegMsk(dev, MPU_RA_PRODUCT_ID);
/* look for a product ID we recognise */
@ -167,49 +168,49 @@ uint8_t mpu6000SpiDetect(const busDevice_t *bus)
}
}
spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(MPU6000_MAX_SPI_CLK_HZ));
spiSetClkDivisor(dev, spiCalculateDivider(MPU6000_MAX_SPI_CLK_HZ));
return detectedSensor;
}
static void mpu6000AccAndGyroInit(gyroDev_t *gyro)
{
spiSetDivisor(gyro->bus.busdev_u.spi.instance, spiCalculateDivider(MPU6000_MAX_SPI_INIT_CLK_HZ));
spiSetClkDivisor(&gyro->dev, spiCalculateDivider(MPU6000_MAX_SPI_INIT_CLK_HZ));
// Device was already reset during detection so proceed with configuration
// Clock Source PPL with Z axis gyro reference
spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
spiWriteReg(&gyro->dev, MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
delayMicroseconds(15);
// Disable Primary I2C Interface
spiBusWriteRegister(&gyro->bus, MPU_RA_USER_CTRL, BIT_I2C_IF_DIS);
spiWriteReg(&gyro->dev, MPU_RA_USER_CTRL, BIT_I2C_IF_DIS);
delayMicroseconds(15);
spiBusWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_2, 0x00);
spiWriteReg(&gyro->dev, MPU_RA_PWR_MGMT_2, 0x00);
delayMicroseconds(15);
// Accel Sample Rate 1kHz
// Gyroscope Output Rate = 1kHz when the DLPF is enabled
spiBusWriteRegister(&gyro->bus, MPU_RA_SMPLRT_DIV, gyro->mpuDividerDrops);
spiWriteReg(&gyro->dev, MPU_RA_SMPLRT_DIV, gyro->mpuDividerDrops);
delayMicroseconds(15);
// Gyro +/- 2000 DPS Full Scale
spiBusWriteRegister(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
spiWriteReg(&gyro->dev, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
delayMicroseconds(15);
// Accel +/- 16 G Full Scale
spiBusWriteRegister(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
spiWriteReg(&gyro->dev, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
delayMicroseconds(15);
spiBusWriteRegister(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR
spiWriteReg(&gyro->dev, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR
delayMicroseconds(15);
#ifdef USE_MPU_DATA_READY_SIGNAL
spiBusWriteRegister(&gyro->bus, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
spiWriteReg(&gyro->dev, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
delayMicroseconds(15);
#endif
spiSetDivisor(gyro->bus.busdev_u.spi.instance, spiCalculateDivider(MPU6000_MAX_SPI_CLK_HZ));
spiSetClkDivisor(&gyro->dev, spiCalculateDivider(MPU6000_MAX_SPI_CLK_HZ));
delayMicroseconds(1);
}
@ -220,7 +221,7 @@ bool mpu6000SpiAccDetect(accDev_t *acc)
}
acc->initFn = mpu6000SpiAccInit;
acc->readFn = mpuAccRead;
acc->readFn = mpuAccReadSPI;
return true;
}

2
src/main/drivers/accgyro/accgyro_spi_mpu6000.h
View File

@ -34,7 +34,7 @@
// RF = Register Flag
#define MPU_RF_DATA_RDY_EN (1 << 0)
uint8_t mpu6000SpiDetect(const busDevice_t *bus);
uint8_t mpu6000SpiDetect(const extDevice_t *dev);
bool mpu6000SpiAccDetect(accDev_t *acc);
bool mpu6000SpiGyroDetect(gyroDev_t *gyro);

18
src/main/drivers/accgyro/accgyro_spi_mpu6500.c
View File

@ -44,17 +44,17 @@
#define BIT_SLEEP 0x40
static void mpu6500SpiInit(const busDevice_t *bus)
static void mpu6500SpiInit(const extDevice_t *dev)
{
spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(MPU6500_MAX_SPI_CLK_HZ));
spiSetClkDivisor(dev, spiCalculateDivider(MPU6500_MAX_SPI_CLK_HZ));
}
uint8_t mpu6500SpiDetect(const busDevice_t *bus)
uint8_t mpu6500SpiDetect(const extDevice_t *dev)
{
mpu6500SpiInit(bus);
mpu6500SpiInit(dev);
const uint8_t whoAmI = spiBusReadRegister(bus, MPU_RA_WHO_AM_I);
const uint8_t whoAmI = spiReadRegMsk(dev, MPU_RA_WHO_AM_I);
uint8_t mpuDetected = MPU_NONE;
switch (whoAmI) {
@ -90,16 +90,16 @@ void mpu6500SpiAccInit(accDev_t *acc)
void mpu6500SpiGyroInit(gyroDev_t *gyro)
{
spiSetDivisor(gyro->bus.busdev_u.spi.instance, spiCalculateDivider(MPU6500_MAX_SPI_INIT_CLK_HZ));
spiSetClkDivisor(&gyro->dev, spiCalculateDivider(MPU6500_MAX_SPI_INIT_CLK_HZ));
delayMicroseconds(1);
mpu6500GyroInit(gyro);
// Disable Primary I2C Interface
spiBusWriteRegister(&gyro->bus, MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS);
spiWriteReg(&gyro->dev, MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS);
delay(100);
spiSetDivisor(gyro->bus.busdev_u.spi.instance, spiCalculateDivider(MPU6500_MAX_SPI_CLK_HZ));
spiSetClkDivisor(&gyro->dev, spiCalculateDivider(MPU6500_MAX_SPI_CLK_HZ));
delayMicroseconds(1);
}
@ -118,7 +118,7 @@ bool mpu6500SpiAccDetect(accDev_t *acc)
}
acc->initFn = mpu6500SpiAccInit;
acc->readFn = mpuAccRead;
acc->readFn = mpuAccReadSPI;
return true;
}

2
src/main/drivers/accgyro/accgyro_spi_mpu6500.h
View File

@ -22,7 +22,7 @@
#include "drivers/bus.h"
uint8_t mpu6500SpiDetect(const busDevice_t *bus);
uint8_t mpu6500SpiDetect(const extDevice_t *dev);
bool mpu6500SpiAccDetect(accDev_t *acc);
bool mpu6500SpiGyroDetect(gyroDev_t *gyro);

61
src/main/drivers/accgyro/accgyro_spi_mpu9250.c
View File

@ -55,25 +55,19 @@
static void mpu9250AccAndGyroInit(gyroDev_t *gyro);
bool mpu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
bool mpu9250SpiWriteRegister(const extDevice_t *dev, uint8_t reg, uint8_t data)
{
IOLo(bus->busdev_u.spi.csnPin);
delayMicroseconds(1);
spiTransferByte(bus->busdev_u.spi.instance, reg);
spiTransferByte(bus->busdev_u.spi.instance, data);
IOHi(bus->busdev_u.spi.csnPin);
spiWriteRegBuf(dev, reg, &data, sizeof (data));
delayMicroseconds(1);
return true;
}
static bool mpu9250SpiSlowReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t *data, uint8_t length)
static bool mpu9250SpiSlowReadRegisterBuffer(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length)
{
IOLo(bus->busdev_u.spi.csnPin);
delayMicroseconds(1);
spiTransferByte(bus->busdev_u.spi.instance, reg | 0x80); // read transaction
spiTransfer(bus->busdev_u.spi.instance, NULL, data, length);
IOHi(bus->busdev_u.spi.csnPin);
spiReadRegBuf(dev, reg | 0x80, data, length);
delayMicroseconds(1);
return true;
@ -85,14 +79,11 @@ void mpu9250SpiGyroInit(gyroDev_t *gyro)
mpu9250AccAndGyroInit(gyro);
spiResetErrorCounter(gyro->bus.busdev_u.spi.instance);
spiSetDivisor(gyro->bus.busdev_u.spi.instance, spiCalculateDivider(MPU9250_MAX_SPI_CLK_HZ)); //high speed now that we don't need to write to the slow registers
spiSetClkDivisor(&gyro->dev, spiCalculateDivider(MPU9250_MAX_SPI_CLK_HZ)); //high speed now that we don't need to write to the slow registers
mpuGyroRead(gyro);
if ((((int8_t)gyro->gyroADCRaw[1]) == -1 && ((int8_t)gyro->gyroADCRaw[0]) == -1) || spiGetErrorCounter(gyro->bus.busdev_u.spi.instance) != 0) {
spiResetErrorCounter(gyro->bus.busdev_u.spi.instance);
if ((((int8_t)gyro->gyroADCRaw[1]) == -1 && ((int8_t)gyro->gyroADCRaw[0]) == -1)) {
failureMode(FAILURE_GYRO_INIT_FAILED);
}
}
@ -102,20 +93,20 @@ void mpu9250SpiAccInit(accDev_t *acc)
acc->acc_1G = 512 * 4;
}
bool mpu9250SpiWriteRegisterVerify(const busDevice_t *bus, uint8_t reg, uint8_t data)
bool mpu9250SpiWriteRegisterVerify(const extDevice_t *dev, uint8_t reg, uint8_t data)
{
mpu9250SpiWriteRegister(bus, reg, data);
mpu9250SpiWriteRegister(dev, reg, data);
delayMicroseconds(100);
uint8_t attemptsRemaining = 20;
do {
uint8_t in;
mpu9250SpiSlowReadRegisterBuffer(bus, reg, &in, 1);
mpu9250SpiSlowReadRegisterBuffer(dev, reg, &in, 1);
if (in == data) {
return true;
} else {
debug[3]++;
mpu9250SpiWriteRegister(bus, reg, data);
mpu9250SpiWriteRegister(dev, reg, data);
delayMicroseconds(100);
}
} while (attemptsRemaining--);
@ -124,39 +115,39 @@ bool mpu9250SpiWriteRegisterVerify(const busDevice_t *bus, uint8_t reg, uint8_t
static void mpu9250AccAndGyroInit(gyroDev_t *gyro) {
spiSetDivisor(gyro->bus.busdev_u.spi.instance, spiCalculateDivider(MPU9250_MAX_SPI_INIT_CLK_HZ)); //low speed for writing to slow registers
spiSetClkDivisor(&gyro->dev, spiCalculateDivider(MPU9250_MAX_SPI_INIT_CLK_HZ)); //low speed for writing to slow registers
mpu9250SpiWriteRegister(&gyro->bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
mpu9250SpiWriteRegister(&gyro->dev, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
delay(50);
mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
mpu9250SpiWriteRegisterVerify(&gyro->dev, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
mpu9250SpiWriteRegisterVerify(&gyro->dev, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_CONFIG, mpuGyroDLPF(gyro));
mpu9250SpiWriteRegisterVerify(&gyro->dev, MPU_RA_CONFIG, mpuGyroDLPF(gyro));
mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_SMPLRT_DIV, gyro->mpuDividerDrops);
mpu9250SpiWriteRegisterVerify(&gyro->dev, MPU_RA_SMPLRT_DIV, gyro->mpuDividerDrops);
mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
mpu9250SpiWriteRegisterVerify(&gyro->bus, 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
mpu9250SpiWriteRegisterVerify(&gyro->dev, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
mpu9250SpiWriteRegisterVerify(&gyro->dev, 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)
mpu9250SpiWriteRegisterVerify(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly.
mpu9250SpiWriteRegisterVerify(&gyro->dev, MPU_RA_INT_ENABLE, 0x01); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly.
#endif
spiSetDivisor(gyro->bus.busdev_u.spi.instance, spiCalculateDivider(MPU9250_MAX_SPI_CLK_HZ));
spiSetClkDivisor(&gyro->dev, spiCalculateDivider(MPU9250_MAX_SPI_CLK_HZ));
}
uint8_t mpu9250SpiDetect(const busDevice_t *bus)
uint8_t mpu9250SpiDetect(const extDevice_t *dev)
{
spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(MPU9250_MAX_SPI_INIT_CLK_HZ)); //low speed
mpu9250SpiWriteRegister(bus, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
spiSetClkDivisor(dev, spiCalculateDivider(MPU9250_MAX_SPI_INIT_CLK_HZ)); //low speed
mpu9250SpiWriteRegister(dev, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
uint8_t attemptsRemaining = 20;
do {
delay(150);
const uint8_t in = spiBusReadRegister(bus, MPU_RA_WHO_AM_I);
const uint8_t in = spiReadRegMsk(dev, MPU_RA_WHO_AM_I);
if (in == MPU9250_WHO_AM_I_CONST || in == MPU9255_WHO_AM_I_CONST) {
break;
}
@ -165,7 +156,7 @@ uint8_t mpu9250SpiDetect(const busDevice_t *bus)
}
} while (attemptsRemaining--);
spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(MPU9250_MAX_SPI_CLK_HZ));
spiSetClkDivisor(dev, spiCalculateDivider(MPU9250_MAX_SPI_CLK_HZ));
return MPU_9250_SPI;
}
@ -177,7 +168,7 @@ bool mpu9250SpiAccDetect(accDev_t *acc)
}
acc->initFn = mpu9250SpiAccInit;
acc->readFn = mpuAccRead;
acc->readFn = mpuAccReadSPI;
return true;
}

8
src/main/drivers/accgyro/accgyro_spi_mpu9250.h
View File

@ -44,11 +44,11 @@
void mpu9250SpiResetGyro(void);
uint8_t mpu9250SpiDetect(const busDevice_t *bus);
uint8_t mpu9250SpiDetect(const extDevice_t *dev);
bool mpu9250SpiAccDetect(accDev_t *acc);
bool mpu9250SpiGyroDetect(gyroDev_t *gyro);
bool mpu9250SpiWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
bool mpu9250SpiWriteRegisterVerify(const busDevice_t *bus, uint8_t reg, uint8_t data);
bool mpu9250SpiReadRegBuf(const busDevice_t *bus, uint8_t reg, uint8_t length, uint8_t *data);
bool mpu9250SpiWriteRegister(const extDevice_t *dev, uint8_t reg, uint8_t data);
bool mpu9250SpiWriteRegisterVerify(const extDevice_t *dev, uint8_t reg, uint8_t data);
bool mpu9250SpiReadRegBuf(const extDevice_t *dev, uint8_t reg, uint8_t length, uint8_t *data);

2
src/main/drivers/barometer/barometer.h
View File

@ -31,7 +31,7 @@ typedef void (*baroCalculateFuncPtr)(int32_t *pressure, int32_t *temperature); /
// the 'u' in these variable names means 'uncompensated', 't' is temperature, 'p' pressure.
typedef struct baroDev_s {
busDevice_t busdev;
extDevice_t dev;
#ifdef USE_EXTI
extiCallbackRec_t exti;
#endif

38
src/main/drivers/barometer/barometer_bmp085.c
View File

@ -124,7 +124,7 @@ static bool bmp085InitDone = false;
STATIC_UNIT_TESTED uint16_t bmp085_ut; // static result of temperature measurement
STATIC_UNIT_TESTED uint32_t bmp085_up; // static result of pressure measurement
static void bmp085ReadCalibrarionParameters(busDevice_t *busdev);
static void bmp085ReadCalibrarionParameters(const extDevice_t *dev);
static void bmp085StartUT(baroDev_t *baro);
static bool bmp085ReadUT(baroDev_t *baro);
static bool bmp085GetUT(baroDev_t *baro);
@ -175,26 +175,26 @@ bool bmp085Detect(const bmp085Config_t *config, baroDev_t *baro)
delay(20); // datasheet says 10ms, we'll be careful and do 20.
busDevice_t *busdev = &baro->busdev;
extDevice_t *dev = &baro->dev;
if ((busdev->bustype == BUSTYPE_I2C) && (busdev->busdev_u.i2c.address == 0)) {
if ((dev->bus->busType == BUS_TYPE_I2C) && (dev->busType_u.i2c.address == 0)) {
// Default address for BMP085
busdev->busdev_u.i2c.address = BMP085_I2C_ADDR;
dev->busType_u.i2c.address = BMP085_I2C_ADDR;
defaultAddressApplied = true;
}
ack = busReadRegisterBuffer(busdev, BMP085_CHIP_ID__REG, &data, 1); /* read Chip Id */
ack = busReadRegisterBuffer(dev, BMP085_CHIP_ID__REG, &data, 1); /* read Chip Id */
if (ack) {
bmp085.chip_id = BMP085_GET_BITSLICE(data, BMP085_CHIP_ID);
bmp085.oversampling_setting = 3;
if (bmp085.chip_id == BMP085_CHIP_ID) { /* get bitslice */
busDeviceRegister(busdev);
busDeviceRegister(dev);
busReadRegisterBuffer(busdev, BMP085_VERSION_REG, &data, 1); /* read Version reg */
busReadRegisterBuffer(dev, BMP085_VERSION_REG, &data, 1); /* read Version reg */
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 */
bmp085ReadCalibrarionParameters(busdev); /* readout bmp085 calibparam structure */
bmp085ReadCalibrarionParameters(dev); /* readout bmp085 calibparam structure */
baro->ut_delay = UT_DELAY;
baro->up_delay = UP_DELAY;
baro->start_ut = bmp085StartUT;
@ -222,7 +222,7 @@ bool bmp085Detect(const bmp085Config_t *config, baroDev_t *baro)
BMP085_OFF;
if (defaultAddressApplied) {
busdev->busdev_u.i2c.address = 0;
dev->busType_u.i2c.address = 0;
}
return false;
@ -285,12 +285,12 @@ static void bmp085StartUT(baroDev_t *baro)
{
isConversionComplete = false;
busWriteRegisterStart(&baro->busdev, BMP085_CTRL_MEAS_REG, BMP085_T_MEASURE);
busWriteRegisterStart(&baro->dev, BMP085_CTRL_MEAS_REG, BMP085_T_MEASURE);
}
static bool bmp085ReadUT(baroDev_t *baro)
{
if (busBusy(&baro->busdev, NULL)) {
if (busBusy(&baro->dev, NULL)) {
return false;
}
@ -299,14 +299,14 @@ static bool bmp085ReadUT(baroDev_t *baro)
return false;
}
busReadRegisterBufferStart(&baro->busdev, BMP085_ADC_OUT_MSB_REG, sensor_data, BMP085_DATA_TEMP_SIZE);
busReadRegisterBufferStart(&baro->dev, BMP085_ADC_OUT_MSB_REG, sensor_data, BMP085_DATA_TEMP_SIZE);
return true;
}
static bool bmp085GetUT(baroDev_t *baro)
{
if (busBusy(&baro->busdev, NULL)) {
if (busBusy(&baro->dev, NULL)) {
return false;
}
@ -323,12 +323,12 @@ static void bmp085StartUP(baroDev_t *baro)
isConversionComplete = false;
busWriteRegisterStart(&baro->busdev, BMP085_CTRL_MEAS_REG, ctrl_reg_data);
busWriteRegisterStart(&baro->dev, BMP085_CTRL_MEAS_REG, ctrl_reg_data);
}
static bool bmp085ReadUP(baroDev_t *baro)
{
if (busBusy(&baro->busdev, NULL)) {
if (busBusy(&baro->dev, NULL)) {
return false;
}
@ -337,7 +337,7 @@ static bool bmp085ReadUP(baroDev_t *baro)
return false;
}
busReadRegisterBufferStart(&baro->busdev, BMP085_ADC_OUT_MSB_REG, sensor_data, BMP085_DATA_PRES_SIZE);
busReadRegisterBufferStart(&baro->dev, BMP085_ADC_OUT_MSB_REG, sensor_data, BMP085_DATA_PRES_SIZE);
return true;
}
@ -348,7 +348,7 @@ static bool bmp085ReadUP(baroDev_t *baro)
*/
static bool bmp085GetUP(baroDev_t *baro)
{
if (busBusy(&baro->busdev, NULL)) {
if (busBusy(&baro->dev, NULL)) {
return false;
}
@ -370,10 +370,10 @@ STATIC_UNIT_TESTED void bmp085Calculate(int32_t *pressure, int32_t *temperature)
*temperature = temp;
}
static void bmp085ReadCalibrarionParameters(busDevice_t *busdev)
static void bmp085ReadCalibrarionParameters(const extDevice_t *dev)
{
uint8_t data[22];
busReadRegisterBuffer(busdev, BMP085_PROM_START__ADDR, data, BMP085_PROM_DATA__LEN);
busReadRegisterBuffer(dev, BMP085_PROM_START__ADDR, data, BMP085_PROM_DATA__LEN);
/*parameters AC1-AC6*/
bmp085.cal_param.ac1 = data[0] << 8 | data[1];

56
src/main/drivers/barometer/barometer_bmp280.c
View File

@ -107,7 +107,7 @@ STATIC_UNIT_TESTED bmp280_calib_param_t bmp280_cal;
// uncompensated pressure and temperature
int32_t bmp280_up = 0;
int32_t bmp280_ut = 0;
static uint8_t sensor_data[BMP280_DATA_FRAME_SIZE];
static DMA_DATA_ZERO_INIT uint8_t sensor_data[BMP280_DATA_FRAME_SIZE];
static void bmp280StartUT(baroDev_t *baro);
static bool bmp280ReadUT(baroDev_t *baro);
@ -118,32 +118,28 @@ static bool bmp280GetUP(baroDev_t *baro);
STATIC_UNIT_TESTED void bmp280Calculate(int32_t *pressure, int32_t *temperature);
void bmp280BusInit(busDevice_t *busdev)
void bmp280BusInit(const extDevice_t *dev)
{
#ifdef USE_BARO_SPI_BMP280
if (busdev->bustype == BUSTYPE_SPI) {
IOHi(busdev->busdev_u.spi.csnPin); // Disable
IOInit(busdev->busdev_u.spi.csnPin, OWNER_BARO_CS, 0);
IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP);
#ifdef USE_SPI_TRANSACTION
spiBusTransactionInit(busdev, SPI_MODE0_POL_LOW_EDGE_1ST, spiCalculateDivider(BMP280_MAX_SPI_CLK_HZ)); // BMP280 supports Mode 0 or 3
#else
spiBusSetDivisor(busdev, spiCalculateDivider(BMP280_MAX_SPI_CLK_HZ));
#endif
if (dev->bus->busType == BUS_TYPE_SPI) {
IOHi(dev->busType_u.spi.csnPin); // Disable
IOInit(dev->busType_u.spi.csnPin, OWNER_BARO_CS, 0);
IOConfigGPIO(dev->busType_u.spi.csnPin, IOCFG_OUT_PP);
spiSetClkDivisor(dev, spiCalculateDivider(BMP280_MAX_SPI_CLK_HZ));
}
#else
UNUSED(busdev);
UNUSED(dev);
#endif
}
void bmp280BusDeinit(busDevice_t *busdev)
void bmp280BusDeinit(const extDevice_t *dev)
{
#ifdef USE_BARO_SPI_BMP280
if (busdev->bustype == BUSTYPE_SPI) {
spiPreinitByIO(busdev->busdev_u.spi.csnPin);
if (dev->bus->busType == BUS_TYPE_SPI) {
spiPreinitByIO(dev->busType_u.spi.csnPin);
}
#else
UNUSED(busdev);
UNUSED(dev);
#endif
}
@ -153,34 +149,34 @@ bool bmp280Detect(baroDev_t *baro)
{
delay(20);
busDevice_t *busdev = &baro->busdev;
extDevice_t *dev = &baro->dev;
bool defaultAddressApplied = false;
bmp280BusInit(busdev);
bmp280BusInit(dev);
if ((busdev->bustype == BUSTYPE_I2C) && (busdev->busdev_u.i2c.address == 0)) {
if ((dev->bus->busType == BUS_TYPE_I2C) && (dev->busType_u.i2c.address == 0)) {
// Default address for BMP280
busdev->busdev_u.i2c.address = BMP280_I2C_ADDR;
dev->busType_u.i2c.address = BMP280_I2C_ADDR;
defaultAddressApplied = true;
}
busReadRegisterBuffer(busdev, BMP280_CHIP_ID_REG, &bmp280_chip_id, 1); /* read Chip Id */
busReadRegisterBuffer(dev, BMP280_CHIP_ID_REG, &bmp280_chip_id, 1); /* read Chip Id */
if ((bmp280_chip_id != BMP280_DEFAULT_CHIP_ID) && (bmp280_chip_id != BME280_DEFAULT_CHIP_ID)) {
bmp280BusDeinit(busdev);
bmp280BusDeinit(dev);
if (defaultAddressApplied) {
busdev->busdev_u.i2c.address = 0;
dev->busType_u.i2c.address = 0;
}
return false;
}
busDeviceRegister(busdev);
busDeviceRegister(dev);
// read calibration
busReadRegisterBuffer(busdev, BMP280_TEMPERATURE_CALIB_DIG_T1_LSB_REG, (uint8_t *)&bmp280_cal, sizeof(bmp280_calib_param_t));
busReadRegisterBuffer(dev, BMP280_TEMPERATURE_CALIB_DIG_T1_LSB_REG, (uint8_t *)&bmp280_cal, sizeof(bmp280_calib_param_t));
// set oversampling + power mode (forced), and start sampling
busWriteRegister(busdev, BMP280_CTRL_MEAS_REG, BMP280_MODE);
busWriteRegister(dev, BMP280_CTRL_MEAS_REG, BMP280_MODE);
// these are dummy as temperature is measured as part of pressure
baro->combined_read = true;
@ -222,24 +218,24 @@ static void bmp280StartUP(baroDev_t *baro)
{
// start measurement
// set oversampling + power mode (forced), and start sampling
busWriteRegisterStart(&baro->busdev, BMP280_CTRL_MEAS_REG, BMP280_MODE);
busWriteRegisterStart(&baro->dev, BMP280_CTRL_MEAS_REG, BMP280_MODE);
}
static bool bmp280ReadUP(baroDev_t *baro)
{
if (busBusy(&baro->busdev, NULL)) {
if (busBusy(&baro->dev, NULL)) {
return false;
}
// read data from sensor
busReadRegisterBufferStart(&baro->busdev, BMP280_PRESSURE_MSB_REG, sensor_data, BMP280_DATA_FRAME_SIZE);
busReadRegisterBufferStart(&baro->dev, BMP280_PRESSURE_MSB_REG, sensor_data, BMP280_DATA_FRAME_SIZE);
return true;
}
static bool bmp280GetUP(baroDev_t *baro)
{
if (busBusy(&baro->busdev, NULL)) {
if (busBusy(&baro->dev, NULL)) {
return false;
}

64
src/main/drivers/barometer/barometer_bmp388.c
View File

@ -165,7 +165,7 @@ STATIC_UNIT_TESTED bmp388_calib_param_t bmp388_cal;
// uncompensated pressure and temperature
uint32_t bmp388_up = 0;
uint32_t bmp388_ut = 0;
static uint8_t sensor_data[BMP388_DATA_FRAME_SIZE];
static DMA_DATA_ZERO_INIT uint8_t sensor_data[BMP388_DATA_FRAME_SIZE];
STATIC_UNIT_TESTED int64_t t_lin = 0;
@ -190,40 +190,40 @@ void bmp388_extiHandler(extiCallbackRec_t* cb)
baroDev_t *baro = container_of(cb, baroDev_t, exti);
uint8_t intStatus = 0;
busReadRegisterBuffer(&baro->busdev, BMP388_INT_STATUS_REG, &intStatus, 1);
busReadRegisterBuffer(&baro->dev, BMP388_INT_STATUS_REG, &intStatus, 1);
}
#endif
void bmp388BusInit(busDevice_t *busdev)
void bmp388BusInit(const extDevice_t *dev)
{
#ifdef USE_BARO_SPI_BMP388
if (busdev->bustype == BUSTYPE_SPI) {
IOHi(busdev->busdev_u.spi.csnPin); // Disable
IOInit(busdev->busdev_u.spi.csnPin, OWNER_BARO_CS, 0);
IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP);
spiBusSetDivisor(busdev, spiCalculateDivider(BMP388_MAX_SPI_CLK_HZ));
if (dev->bus->busType == BUS_TYPE_SPI) {
IOHi(dev->busType_u.spi.csnPin); // Disable
IOInit(dev->busType_u.spi.csnPin, OWNER_BARO_CS, 0);
IOConfigGPIO(dev->busType_u.spi.csnPin, IOCFG_OUT_PP);
spiSetClkDivisor(dev, spiCalculateDivider(BMP388_MAX_SPI_CLK_HZ));
}
#else
UNUSED(busdev);
UNUSED(dev);
#endif
}
void bmp388BusDeinit(busDevice_t *busdev)
void bmp388BusDeinit(const extDevice_t *dev)
{
#ifdef USE_BARO_SPI_BMP388
if (busdev->bustype == BUSTYPE_SPI) {
spiPreinitByIO(busdev->busdev_u.spi.csnPin);
if (dev->bus->busType == BUS_TYPE_SPI) {
spiPreinitByIO(dev->busType_u.spi.csnPin);
}
#else
UNUSED(busdev);
UNUSED(dev);
#endif
}
void bmp388BeginForcedMeasurement(busDevice_t *busdev)
void bmp388BeginForcedMeasurement(const extDevice_t *dev)
{
// enable pressure measurement, temperature measurement, set power mode and start sampling
uint8_t mode = BMP388_MODE_FORCED << 4 | 1 << 1 | 1 << 0;
busWriteRegisterStart(busdev, BMP388_PWR_CTRL_REG, mode);
busWriteRegisterStart(dev, BMP388_PWR_CTRL_REG, mode);
}
bool bmp388Detect(const bmp388Config_t *config, baroDev_t *baro)
@ -242,28 +242,28 @@ bool bmp388Detect(const bmp388Config_t *config, baroDev_t *baro)
UNUSED(config);
#endif
busDevice_t *busdev = &baro->busdev;
extDevice_t *dev = &baro->dev;
bool defaultAddressApplied = false;
bmp388BusInit(busdev);
bmp388BusInit(dev);
if ((busdev->bustype == BUSTYPE_I2C) && (busdev->busdev_u.i2c.address == 0)) {
if ((dev->bus->busType == BUS_TYPE_I2C) && (dev->busType_u.i2c.address == 0)) {
// Default address for BMP388
busdev->busdev_u.i2c.address = BMP388_I2C_ADDR;
dev->busType_u.i2c.address = BMP388_I2C_ADDR;
defaultAddressApplied = true;
}
busReadRegisterBuffer(busdev, BMP388_CHIP_ID_REG, &bmp388_chip_id, 1);
busReadRegisterBuffer(dev, BMP388_CHIP_ID_REG, &bmp388_chip_id, 1);
if (bmp388_chip_id != BMP388_DEFAULT_CHIP_ID) {
bmp388BusDeinit(busdev);
bmp388BusDeinit(dev);
if (defaultAddressApplied) {
busdev->busdev_u.i2c.address = 0;
dev->busType_u.i2c.address = 0;
}
return false;
}
busDeviceRegister(busdev);
busDeviceRegister(dev);
#ifdef USE_EXTI
if (baroIntIO) {
@ -273,21 +273,21 @@ bool bmp388Detect(const bmp388Config_t *config, baroDev_t *baro)
0 << BMP388_INT_LATCH_BIT |
1 << BMP388_INT_LEVEL_BIT |
0 << BMP388_INT_OD_BIT;
busWriteRegister(busdev, BMP388_INT_CTRL_REG, intCtrlValue);
busWriteRegister(dev, BMP388_INT_CTRL_REG, intCtrlValue);
}
#endif
// read calibration
busReadRegisterBuffer(busdev, BMP388_TRIMMING_NVM_PAR_T1_LSB_REG, (uint8_t *)&bmp388_cal, sizeof(bmp388_calib_param_t));
busReadRegisterBuffer(dev, BMP388_TRIMMING_NVM_PAR_T1_LSB_REG, (uint8_t *)&bmp388_cal, sizeof(bmp388_calib_param_t));
// set oversampling
busWriteRegister(busdev, BMP388_OSR_REG,
busWriteRegister(dev, BMP388_OSR_REG,
((BMP388_PRESSURE_OSR << BMP388_OSR_P_BIT) & BMP388_OSR_P_MASK) |
((BMP388_TEMPERATURE_OSR << BMP388_OSR4_T_BIT) & BMP388_OSR4_T_MASK)
);
bmp388BeginForcedMeasurement(busdev);
bmp388BeginForcedMeasurement(dev);
// these are dummy as temperature is measured as part of pressure
baro->ut_delay = 0;
@ -306,7 +306,7 @@ bool bmp388Detect(const bmp388Config_t *config, baroDev_t *baro)
baro->calculate = bmp388Calculate;
while (busBusy(&baro->busdev, NULL));
while (busBusy(&baro->dev, NULL));
return true;
}
@ -334,24 +334,24 @@ static bool bmp388GetUT(baroDev_t *baro)
static void bmp388StartUP(baroDev_t *baro)
{
// start measurement
bmp388BeginForcedMeasurement(&baro->busdev);
bmp388BeginForcedMeasurement(&baro->dev);
}
static bool bmp388ReadUP(baroDev_t *baro)
{
if (busBusy(&baro->busdev, NULL)) {
if (busBusy(&baro->dev, NULL)) {
return false;
}
// read data from sensor
busReadRegisterBufferStart(&baro->busdev, BMP388_DATA_0_REG, sensor_data, BMP388_DATA_FRAME_SIZE);
busReadRegisterBufferStart(&baro->dev, BMP388_DATA_0_REG, sensor_data, BMP388_DATA_FRAME_SIZE);
return true;
}
static bool bmp388GetUP(baroDev_t *baro)
{
if (busBusy(&baro->busdev, NULL)) {
if (busBusy(&baro->dev, NULL)) {
return false;
}

89
src/main/drivers/barometer/barometer_dps310.c
View File

@ -115,23 +115,23 @@ static baroState_t baroState;
static uint8_t buf[6];
// Helper functions
static uint8_t registerRead(busDevice_t * busDev, uint8_t reg)
static uint8_t registerRead(const extDevice_t *dev, uint8_t reg)
{
return busReadRegister(busDev, reg);
return busReadRegister(dev, reg);
}
static void registerWrite(busDevice_t * busDev, uint8_t reg, uint8_t value)
static void registerWrite(const extDevice_t *dev, uint8_t reg, uint8_t value)
{
busWrite(busDev, reg, value);
busWrite(dev, reg, value);
}
static void registerSetBits(busDevice_t * busDev, uint8_t reg, uint8_t setbits)
static void registerSetBits(const extDevice_t *dev, uint8_t reg, uint8_t setbits)
{
uint8_t val = registerRead(busDev, reg);
uint8_t val = registerRead(dev, reg);
if ((val & setbits) != setbits) {
val |= setbits;
registerWrite(busDev, reg, val);
registerWrite(dev, reg, val);
}
}
@ -145,15 +145,15 @@ static int32_t getTwosComplement(uint32_t raw, uint8_t length)
}
}
static bool deviceConfigure(busDevice_t * busDev)
static bool deviceConfigure(const extDevice_t *dev)
{
// Trigger a chip reset
registerSetBits(busDev, DPS310_REG_RESET, DPS310_RESET_BIT_SOFT_RST);
registerSetBits(dev, DPS310_REG_RESET, DPS310_RESET_BIT_SOFT_RST);
// Sleep 40ms
delay(40);
uint8_t status = registerRead(busDev, DPS310_REG_MEAS_CFG);
uint8_t status = registerRead(dev, DPS310_REG_MEAS_CFG);
// Check if coefficients are available
if ((status & DPS310_MEAS_CFG_COEF_RDY) == 0) {
@ -167,16 +167,15 @@ static bool deviceConfigure(busDevice_t * busDev)
// 1. Read the pressure calibration coefficients (c00, c10, c20, c30, c01, c11, and c21) from the Calibration Coefficient register.
// Note: The coefficients read from the coefficient register are 2's complement numbers.
// Do the read of the coefficients in multiple parts, as the chip will return a read failure when trying to read all at once over I2C.
#define COEFFICIENT_LENGTH 18
#define READ_LENGTH (COEFFICIENT_LENGTH / 2)
uint8_t coef[COEFFICIENT_LENGTH];
if (!busReadBuf(busDev, DPS310_REG_COEF, coef, READ_LENGTH)) {
if (!busReadBuf(dev, DPS310_REG_COEF, coef, READ_LENGTH)) {
return false;
}
if (!busReadBuf(busDev, DPS310_REG_COEF + READ_LENGTH, coef + READ_LENGTH, COEFFICIENT_LENGTH - READ_LENGTH)) {
if (!busReadBuf(dev, DPS310_REG_COEF + READ_LENGTH, coef + READ_LENGTH, COEFFICIENT_LENGTH - READ_LENGTH)) {
return false;
}
@ -208,31 +207,31 @@ static bool deviceConfigure(busDevice_t * busDev)
baroState.calib.c30 = getTwosComplement(((uint32_t)coef[16] << 8) | (uint32_t)coef[17], 16);
// PRS_CFG: pressure measurement rate (32 Hz) and oversampling (16 time standard)
registerSetBits(busDev, DPS310_REG_PRS_CFG, DPS310_PRS_CFG_BIT_PM_RATE_32HZ | DPS310_PRS_CFG_BIT_PM_PRC_16);
registerSetBits(dev, DPS310_REG_PRS_CFG, DPS310_PRS_CFG_BIT_PM_RATE_32HZ | DPS310_PRS_CFG_BIT_PM_PRC_16);
// TMP_CFG: temperature measurement rate (32 Hz) and oversampling (16 times)
const uint8_t TMP_COEF_SRCE = registerRead(busDev, DPS310_REG_COEF_SRCE) & DPS310_COEF_SRCE_BIT_TMP_COEF_SRCE;
registerSetBits(busDev, DPS310_REG_TMP_CFG, DPS310_TMP_CFG_BIT_TMP_RATE_32HZ | DPS310_TMP_CFG_BIT_TMP_PRC_16 | TMP_COEF_SRCE);
const uint8_t TMP_COEF_SRCE = registerRead(dev, DPS310_REG_COEF_SRCE) & DPS310_COEF_SRCE_BIT_TMP_COEF_SRCE;
registerSetBits(dev, DPS310_REG_TMP_CFG, DPS310_TMP_CFG_BIT_TMP_RATE_32HZ | DPS310_TMP_CFG_BIT_TMP_PRC_16 | TMP_COEF_SRCE);
// CFG_REG: set pressure and temperature result bit-shift (required when the oversampling rate is >8 times)
registerSetBits(busDev, DPS310_REG_CFG_REG, DPS310_CFG_REG_BIT_T_SHIFT | DPS310_CFG_REG_BIT_P_SHIFT);
registerSetBits(dev, DPS310_REG_CFG_REG, DPS310_CFG_REG_BIT_T_SHIFT | DPS310_CFG_REG_BIT_P_SHIFT);
// MEAS_CFG: Continuous pressure and temperature measurement
registerSetBits(busDev, DPS310_REG_MEAS_CFG, DPS310_MEAS_CFG_MEAS_CTRL_CONT);
registerSetBits(dev, DPS310_REG_MEAS_CFG, DPS310_MEAS_CFG_MEAS_CTRL_CONT);
return true;
}
static bool dps310ReadUP(baroDev_t *baro)
{
if (busBusy(&baro->busdev, NULL)) {
if (busBusy(&baro->dev, NULL)) {
return false;
}
// 1. Kick off read
// No need to poll for data ready as the conversion rate is 32Hz and this is sampling at 20Hz
// Read PSR_B2, PSR_B1, PSR_B0, TMP_B2, TMP_B1, TMP_B0
busReadRegisterBufferStart(&baro->busdev, DPS310_REG_PSR_B2, buf, 6);
busReadRegisterBufferStart(&baro->dev, DPS310_REG_PSR_B2, buf, 6);
return true;
}
@ -287,14 +286,14 @@ static void deviceCalculate(int32_t *pressure, int32_t *temperature)
#define DETECTION_MAX_RETRY_COUNT 5
static bool deviceDetect(busDevice_t * busDev)
static bool deviceDetect(const extDevice_t *dev)
{
for (int retry = 0; retry < DETECTION_MAX_RETRY_COUNT; retry++) {
uint8_t chipId[1];
delay(100);
bool ack = busReadBuf(busDev, DPS310_REG_ID, chipId, 1);
bool ack = busReadBuf(dev, DPS310_REG_ID, chipId, 1);
if (ack && chipId[0] == DPS310_ID_REV_AND_PROD_ID) {
return true;
@ -328,63 +327,59 @@ static void dps310StartUP(baroDev_t *baro)
UNUSED(baro);
}
static void busDeviceInit(busDevice_t *busdev, resourceOwner_e owner)
static void deviceInit(const extDevice_t *dev, resourceOwner_e owner)
{
#ifdef USE_BARO_SPI_DPS310
if (busdev->bustype == BUSTYPE_SPI) {
IOHi(busdev->busdev_u.spi.csnPin); // Disable
IOInit(busdev->busdev_u.spi.csnPin, owner, 0);
IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP);
#ifdef USE_SPI_TRANSACTION
spiBusTransactionInit(busdev, SPI_MODE0_POL_LOW_EDGE_1ST, spiCalculateDivider(DPS310_MAX_SPI_CLK_HZ)); // DPS310 supports Mode 0 or 3
#else
spiBusSetDivisor(busdev, spiCalculateDivider(DPS310_MAX_SPI_CLK_HZ));
#endif
if (dev->bus->busType == BUS_TYPE_SPI) {
IOHi(dev->busType_u.spi.csnPin); // Disable
IOInit(dev->busType_u.spi.csnPin, owner, 0);
IOConfigGPIO(dev->busType_u.spi.csnPin, IOCFG_OUT_PP);
spiSetClkDivisor(dev, spiCalculateDivider(DPS310_MAX_SPI_CLK_HZ));
}
#else
UNUSED(busdev);
UNUSED(dev);
UNUSED(owner);
#endif
}
static void busDeviceDeInit(busDevice_t *busdev)
static void deviceDeInit(const extDevice_t *dev)
{
#ifdef USE_BARO_SPI_DPS310
if (busdev->bustype == BUSTYPE_SPI) {
spiPreinitByIO(busdev->busdev_u.spi.csnPin);
if (dev->bus->busType == BUS_TYPE_SPI) {
spiPreinitByIO(dev->busType_u.spi.csnPin);
}
#else
UNUSED(busdev);
UNUSED(dev);
#endif
}
bool baroDPS310Detect(baroDev_t *baro)
{
busDevice_t *busdev = &baro->busdev;
extDevice_t *dev = &baro->dev;
bool defaultAddressApplied = false;
busDeviceInit(&baro->busdev, OWNER_BARO_CS);
deviceInit(&baro->dev, OWNER_BARO_CS);
if ((busdev->bustype == BUSTYPE_I2C) && (busdev->busdev_u.i2c.address == 0)) {
if ((dev->bus->busType == BUS_TYPE_I2C) && (dev->busType_u.i2c.address == 0)) {
// Default address for BMP280
busdev->busdev_u.i2c.address = DPS310_I2C_ADDR;
dev->busType_u.i2c.address = DPS310_I2C_ADDR;
defaultAddressApplied = true;
}
if (!deviceDetect(busdev)) {
busDeviceDeInit(busdev);
if (!deviceDetect(dev)) {
deviceDeInit(dev);
if (defaultAddressApplied) {
busdev->busdev_u.i2c.address = 0;
dev->busType_u.i2c.address = 0;
}
return false;
}
if (!deviceConfigure(busdev)) {
busDeviceDeInit(busdev);
if (!deviceConfigure(dev)) {
deviceDeInit(dev);
return false;
}
busDeviceRegister(busdev);
busDeviceRegister(dev);
baro->ut_delay = 0;
baro->start_ut = dps310StartUT;

56
src/main/drivers/barometer/barometer_lps.c
View File

@ -191,34 +191,34 @@
static uint32_t rawP = 0;
static uint16_t rawT = 0;
bool lpsWriteCommand(busDevice_t *busdev, uint8_t cmd, uint8_t byte)
bool lpsWriteCommand(const extDevice_t *dev, uint8_t cmd, uint8_t byte)
{
return spiBusWriteRegister(busdev, cmd, byte);
return spiWriteRegRB(dev, cmd, byte);
}
bool lpsReadCommand(busDevice_t *busdev, uint8_t cmd, uint8_t *data, uint8_t len)
bool lpsReadCommand(const extDevice_t *dev, uint8_t cmd, uint8_t *data, uint8_t len)
{
return spiBusReadRegisterBuffer(busdev, cmd | 0x80 | 0x40, data, len);
return spiReadRegMskBufRB(dev, cmd | 0x80 | 0x40, data, len);
}
bool lpsWriteVerify(busDevice_t *busdev, uint8_t cmd, uint8_t byte)
bool lpsWriteVerify(const extDevice_t *dev, uint8_t cmd, uint8_t byte)
{
uint8_t temp = 0xff;
spiBusWriteRegister(busdev, cmd, byte);
spiBusReadRegisterBuffer(busdev, cmd, &temp, 1);
spiWriteReg(dev, cmd, byte);
temp = spiReadRegMsk(dev, cmd);
if (byte == temp) return true;
return false;
}
static void lpsOn(busDevice_t *busdev, uint8_t CTRL1_val)
static void lpsOn(const extDevice_t *dev, uint8_t CTRL1_val)
{
lpsWriteCommand(busdev, LPS_CTRL1, CTRL1_val | 0x80);
lpsWriteCommand(dev, LPS_CTRL1, CTRL1_val | 0x80);
//Instead of delay let's ready status reg
}
static void lpsOff(busDevice_t *busdev)
static void lpsOff(const extDevice_t *dev)
{
lpsWriteCommand(busdev, LPS_CTRL1, 0x00 | (0x01 << 2));
lpsWriteCommand(dev, LPS_CTRL1, 0x00 | (0x01 << 2));
}
static void lpsNothing(baroDev_t *baro)
@ -236,10 +236,10 @@ static bool lpsNothingBool(baroDev_t *baro)
static bool lpsRead(baroDev_t *baro)
{
uint8_t status = 0x00;
lpsReadCommand(&baro->busdev, LPS_STATUS, &status, 1);
lpsReadCommand(&baro->dev, LPS_STATUS, &status, 1);
if (status & 0x03) {
uint8_t temp[5];
lpsReadCommand(&baro->busdev, LPS_OUT_XL, temp, 5);
lpsReadCommand(&baro->dev, LPS_OUT_XL, temp, 5);
/* Build the raw data */
rawP = temp[0] | (temp[1] << 8) | (temp[2] << 16) | ((temp[2] & 0x80) ? 0xff000000 : 0);
@ -261,36 +261,32 @@ static void lpsCalculate(int32_t *pressure, int32_t *temperature)
bool lpsDetect(baroDev_t *baro)
{
//Detect
busDevice_t *busdev = &baro->busdev;
extDevice_t *dev = &baro->dev;
if (busdev->bustype != BUSTYPE_SPI) {
if (dev->bus->busType != BUS_TYPE_SPI) {
return false;
}
IOInit(busdev->busdev_u.spi.csnPin, OWNER_BARO_CS, 0);
IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP);
IOHi(busdev->busdev_u.spi.csnPin); // Disable
#ifdef USE_SPI_TRANSACTION
spiBusTransactionInit(busdev, SPI_MODE3_POL_HIGH_EDGE_2ND, spiCalculateDivider(LPS_MAX_SPI_CLK_HZ)); // Baro can work only on up to 10Mhz SPI bus
#else
spiBusSetDivisor(busdev, spiCalculateDivider(LPS_MAX_SPI_CLK_HZ)); // Baro can work only on up to 10Mhz SPI bus
#endif
IOInit(dev->busType_u.spi.csnPin, OWNER_BARO_CS, 0);
IOConfigGPIO(dev->busType_u.spi.csnPin, IOCFG_OUT_PP);
IOHi(dev->busType_u.spi.csnPin); // Disable
spiSetClkDivisor(dev, spiCalculateDivider(LPS_MAX_SPI_CLK_HZ));
uint8_t temp = 0x00;
lpsReadCommand(&baro->busdev, LPS_WHO_AM_I, &temp, 1);
lpsReadCommand(&baro->dev, LPS_WHO_AM_I, &temp, 1);
if (temp != LPS25_ID && temp != LPS22_ID && temp != LPS33_ID && temp != LPS35_ID) {
return false;
}
//Init, if writeVerify is false fallback to false on detect
bool ret = false;
lpsOff(busdev);
ret = lpsWriteVerify(busdev, LPS_CTRL2, (0x00 << 1)); if (ret != true) return false;
ret = lpsWriteVerify(busdev, LPS_RES_CONF, (LPS_AVT_64 | LPS_AVP_512)); if (ret != true) return false;
ret = lpsWriteVerify(busdev, LPS_CTRL4, 0x01); if (ret != true) return false;
lpsOn(busdev, (0x04 << 4) | (0x01 << 1) | (0x01 << 2) | (0x01 << 3));
lpsOff(dev);
ret = lpsWriteVerify(dev, LPS_CTRL2, (0x00 << 1)); if (ret != true) return false;
ret = lpsWriteVerify(dev, LPS_RES_CONF, (LPS_AVT_64 | LPS_AVP_512)); if (ret != true) return false;
ret = lpsWriteVerify(dev, LPS_CTRL4, 0x01); if (ret != true) return false;
lpsOn(dev, (0x04 << 4) | (0x01 << 1) | (0x01 << 2) | (0x01 << 3));
lpsReadCommand(busdev, LPS_CTRL1, &temp, 1);
lpsReadCommand(dev, LPS_CTRL1, &temp, 1);
baro->combined_read = true;
baro->ut_delay = 1;

76
src/main/drivers/barometer/barometer_ms5611.c
View File

@ -60,49 +60,45 @@ STATIC_UNIT_TESTED uint32_t ms5611_up; // static result of pressure measurement
STATIC_UNIT_TESTED uint16_t ms5611_c[PROM_NB]; // on-chip ROM
static uint8_t ms5611_osr = CMD_ADC_4096;
#define MS5611_DATA_FRAME_SIZE 3
static uint8_t sensor_data[MS5611_DATA_FRAME_SIZE];
static DMA_DATA_ZERO_INIT uint8_t sensor_data[MS5611_DATA_FRAME_SIZE];
void ms5611BusInit(busDevice_t *busdev)
void ms5611BusInit(const extDevice_t *dev)
{
#ifdef USE_BARO_SPI_MS5611
if (busdev->bustype == BUSTYPE_SPI) {
IOHi(busdev->busdev_u.spi.csnPin); // Disable
IOInit(busdev->busdev_u.spi.csnPin, OWNER_BARO_CS, 0);
IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP);
#ifdef USE_SPI_TRANSACTION
spiBusTransactionInit(busdev, SPI_MODE3_POL_HIGH_EDGE_2ND, spiCalculateDivider(MS5611_MAX_SPI_CLK_HZ));
#else
spiBusSetDivisor(busdev, spiCalculateDivider(MS5611_MAX_SPI_CLK_HZ)); // XXX
#endif
if (dev->bus->busType == BUS_TYPE_SPI) {
IOHi(dev->busType_u.spi.csnPin); // Disable
IOInit(dev->busType_u.spi.csnPin, OWNER_BARO_CS, 0);
IOConfigGPIO(dev->busType_u.spi.csnPin, IOCFG_OUT_PP);
spiSetClkDivisor(dev, spiCalculateDivider(MS5611_MAX_SPI_CLK_HZ));
}
#else
UNUSED(busdev);
UNUSED(dev);
#endif
}
void ms5611BusDeinit(busDevice_t *busdev)
void ms5611BusDeinit(const extDevice_t *dev)
{
#ifdef USE_BARO_SPI_MS5611
if (busdev->bustype == BUSTYPE_SPI) {
spiPreinitByIO(busdev->busdev_u.spi.csnPin);
if (dev->bus->busType == BUS_TYPE_SPI) {
spiPreinitByIO(dev->busType_u.spi.csnPin);
}
#else
UNUSED(busdev);
UNUSED(dev);
#endif
}
static void ms5611Reset(busDevice_t *busdev)
static void ms5611Reset(const extDevice_t *dev)
{
busRawWriteRegister(busdev, CMD_RESET, 1);
busRawWriteRegister(dev, CMD_RESET, 1);
delayMicroseconds(2800);
}
static uint16_t ms5611Prom(busDevice_t *busdev, int8_t coef_num)
static uint16_t ms5611Prom(const extDevice_t *dev, int8_t coef_num)
{
uint8_t rxbuf[2] = { 0, 0 };
busRawReadRegisterBuffer(busdev, CMD_PROM_RD + coef_num * 2, rxbuf, 2); // send PROM READ command
busRawReadRegisterBuffer(dev, CMD_PROM_RD + coef_num * 2, rxbuf, 2); // send PROM READ command
return rxbuf[0] << 8 | rxbuf[1];
}
@ -137,30 +133,30 @@ STATIC_UNIT_TESTED int8_t ms5611CRC(uint16_t *prom)
return -1;
}
static void ms5611ReadAdc(busDevice_t *busdev)
static void ms5611ReadAdc(const extDevice_t *dev)
{
busRawReadRegisterBufferStart(busdev, CMD_ADC_READ, sensor_data, MS5611_DATA_FRAME_SIZE); // read ADC
busRawReadRegisterBufferStart(dev, CMD_ADC_READ, sensor_data, MS5611_DATA_FRAME_SIZE); // read ADC
}
static void ms5611StartUT(baroDev_t *baro)
{
busRawWriteRegisterStart(&baro->busdev, CMD_ADC_CONV + CMD_ADC_D2 + ms5611_osr, 1); // D2 (temperature) conversion start!
busRawWriteRegisterStart(&baro->dev, CMD_ADC_CONV + CMD_ADC_D2 + ms5611_osr, 1); // D2 (temperature) conversion start!
}
static bool ms5611ReadUT(baroDev_t *baro)
{
if (busBusy(&baro->busdev, NULL)) {
if (busBusy(&baro->dev, NULL)) {
return false;
}
ms5611ReadAdc(&baro->busdev);
ms5611ReadAdc(&baro->dev);
return true;
}
static bool ms5611GetUT(baroDev_t *baro)
{
if (busBusy(&baro->busdev, NULL)) {
if (busBusy(&baro->dev, NULL)) {
return false;
}
@ -171,23 +167,23 @@ static bool ms5611GetUT(baroDev_t *baro)
static void ms5611StartUP(baroDev_t *baro)
{
busRawWriteRegisterStart(&baro->busdev, CMD_ADC_CONV + CMD_ADC_D1 + ms5611_osr, 1); // D1 (pressure) conversion start!
busRawWriteRegisterStart(&baro->dev, CMD_ADC_CONV + CMD_ADC_D1 + ms5611_osr, 1); // D1 (pressure) conversion start!
}
static bool ms5611ReadUP(baroDev_t *baro)
{
if (busBusy(&baro->busdev, NULL)) {
if (busBusy(&baro->dev, NULL)) {
return false;
}
ms5611ReadAdc(&baro->busdev);
ms5611ReadAdc(&baro->dev);
return true;
}
static bool ms5611GetUP(baroDev_t *baro)
{
if (busBusy(&baro->busdev, NULL)) {
if (busBusy(&baro->dev, NULL)) {
return false;
}
@ -236,32 +232,32 @@ bool ms5611Detect(baroDev_t *baro)
delay(10); // No idea how long the chip takes to power-up, but let's make it 10ms
busDevice_t *busdev = &baro->busdev;
extDevice_t *dev = &baro->dev;
ms5611BusInit(busdev);
ms5611BusInit(dev);
if ((busdev->bustype == BUSTYPE_I2C) && (busdev->busdev_u.i2c.address == 0)) {
if ((dev->bus->busType == BUS_TYPE_I2C) && (dev->busType_u.i2c.address == 0)) {
// Default address for MS5611
busdev->busdev_u.i2c.address = MS5611_I2C_ADDR;
dev->busType_u.i2c.address = MS5611_I2C_ADDR;
defaultAddressApplied = true;
}
if (!busRawReadRegisterBuffer(busdev, CMD_PROM_RD, &sig, 1) || sig == 0xFF) {
if (!busRawReadRegisterBuffer(dev, CMD_PROM_RD, &sig, 1) || sig == 0xFF) {
goto fail;
}
ms5611Reset(busdev);
ms5611Reset(dev);
// read all coefficients
for (i = 0; i < PROM_NB; i++)
ms5611_c[i] = ms5611Prom(busdev, i);
ms5611_c[i] = ms5611Prom(dev, i);
// check crc, bail out if wrong - we are probably talking to BMP085 w/o XCLR line!
if (ms5611CRC(ms5611_c) != 0) {
goto fail;
}
busDeviceRegister(busdev);
busDeviceRegister(dev);
// TODO prom + CRC
baro->ut_delay = 10000;
@ -277,10 +273,10 @@ bool ms5611Detect(baroDev_t *baro)
return true;
fail:;
ms5611BusDeinit(busdev);
ms5611BusDeinit(dev);
if (defaultAddressApplied) {
busdev->busdev_u.i2c.address = 0;
dev->busType_u.i2c.address = 0;
}
return false;

62
src/main/drivers/barometer/barometer_qmp6988.c
View File

@ -94,7 +94,7 @@ STATIC_UNIT_TESTED qmp6988_calib_param_t qmp6988_cal;
// uncompensated pressure and temperature
int32_t qmp6988_up = 0;
int32_t qmp6988_ut = 0;
static uint8_t sensor_data[QMP6988_DATA_FRAME_SIZE];
static DMA_DATA_ZERO_INIT uint8_t sensor_data[QMP6988_DATA_FRAME_SIZE];
static void qmp6988StartUT(baroDev_t *baro);
static bool qmp6988ReadUT(baroDev_t *baro);
@ -105,34 +105,30 @@ static bool qmp6988GetUP(baroDev_t *baro);
STATIC_UNIT_TESTED void qmp6988Calculate(int32_t *pressure, int32_t *temperature);
void qmp6988BusInit(busDevice_t *busdev)
void qmp6988BusInit(const extDevice_t *dev)
{
#ifdef USE_BARO_SPI_QMP6988
if (busdev->bustype == BUSTYPE_SPI) {
IOHi(busdev->busdev_u.spi.csnPin);
IOInit(busdev->busdev_u.spi.csnPin, OWNER_BARO_CS, 0);
IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP);
#ifdef USE_SPI_TRANSACTION
spiBusTransactionInit(busdev, SPI_MODE3_POL_HIGH_EDGE_2ND, spiCalculateDivider(QMP6988_MAX_SPI_CLK_HZ));
#else
spiBusSetDivisor(busdev, spiCalculateDivider(QMP6988_MAX_SPI_CLK_HZ));
#endif
if (dev->bus->busType == BUS_TYPE_SPI) {
IOHi(dev->busType_u.spi.csnPin);
IOInit(dev->busType_u.spi.csnPin, OWNER_BARO_CS, 0);
IOConfigGPIO(dev->busType_u.spi.csnPin, IOCFG_OUT_PP);
spiSetClkDivisor(dev, spiCalculateDivider(QMP6988_MAX_SPI_CLK_HZ));
}
#else
UNUSED(busdev);
UNUSED(dev);
#endif
}
void qmp6988BusDeinit(busDevice_t *busdev)
void qmp6988BusDeinit(const extDevice_t *dev)
{
#ifdef USE_BARO_SPI_QMP6988
if (busdev->bustype == BUSTYPE_SPI) {
IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_IPU);
IORelease(busdev->busdev_u.spi.csnPin);
IOInit(busdev->busdev_u.spi.csnPin, OWNER_PREINIT, 0);
if (dev->bus->busType == BUS_TYPE_SPI) {
IOConfigGPIO(dev->busType_u.spi.csnPin, IOCFG_IPU);
IORelease(dev->busType_u.spi.csnPin);
IOInit(dev->busType_u.spi.csnPin, OWNER_PREINIT, 0);
}
#else
UNUSED(busdev);
UNUSED(dev);
#endif
}
@ -156,33 +152,33 @@ bool qmp6988Detect(baroDev_t *baro)
delay(20);
busDevice_t *busdev = &baro->busdev;
extDevice_t *dev = &baro->dev;
bool defaultAddressApplied = false;
qmp6988BusInit(busdev);
qmp6988BusInit(dev);
if ((busdev->bustype == BUSTYPE_I2C) && (busdev->busdev_u.i2c.address == 0)) {
busdev->busdev_u.i2c.address = QMP6988_I2C_ADDR;
if ((dev->bus->busType == BUS_TYPE_I2C) && (dev->busType_u.i2c.address == 0)) {
dev->busType_u.i2c.address = QMP6988_I2C_ADDR;
defaultAddressApplied = true;
}
busReadRegisterBuffer(busdev, QMP6988_CHIP_ID_REG, &qmp6988_chip_id, 1); /* read Chip Id */
busReadRegisterBuffer(dev, QMP6988_CHIP_ID_REG, &qmp6988_chip_id, 1); /* read Chip Id */
if (qmp6988_chip_id != QMP6988_DEFAULT_CHIP_ID) {
qmp6988BusDeinit(busdev);
qmp6988BusDeinit(dev);
if (defaultAddressApplied) {
busdev->busdev_u.i2c.address = 0;
dev->busType_u.i2c.address = 0;
}
return false;
}
busDeviceRegister(busdev);
busDeviceRegister(dev);
// SetIIR
busWriteRegister(busdev, QMP6988_SET_IIR_REG, 0x05);
busWriteRegister(dev, QMP6988_SET_IIR_REG, 0x05);
//read OTP
busReadRegisterBuffer(busdev, QMP6988_COE_B00_1_REG, databuf, 25);
busReadRegisterBuffer(dev, QMP6988_COE_B00_1_REG, databuf, 25);
//algo OTP
hw = databuf[0];
@ -265,7 +261,7 @@ bool qmp6988Detect(baroDev_t *baro)
qmp6988_cal.Coe_bp3= (1.30E-16)+(7.90E-17)*(float)Coe_bp3_/32767.0;
// Set power mode and sample times
busWriteRegister(busdev, QMP6988_CTRL_MEAS_REG, QMP6988_PWR_SAMPLE_MODE);
busWriteRegister(dev, QMP6988_CTRL_MEAS_REG, QMP6988_PWR_SAMPLE_MODE);
// these are dummy as temperature is measured as part of pressure
baro->combined_read = true;
@ -306,24 +302,24 @@ static bool qmp6988GetUT(baroDev_t *baro)
static void qmp6988StartUP(baroDev_t *baro)
{
// start measurement
busWriteRegister(&baro->busdev, QMP6988_CTRL_MEAS_REG, QMP6988_PWR_SAMPLE_MODE);
busWriteRegister(&baro->dev, QMP6988_CTRL_MEAS_REG, QMP6988_PWR_SAMPLE_MODE);
}
static bool qmp6988ReadUP(baroDev_t *baro)
{
if (busBusy(&baro->busdev, NULL)) {
if (busBusy(&baro->dev, NULL)) {
return false;
}
// read data from sensor
busReadRegisterBufferStart(&baro->busdev, QMP6988_PRESSURE_MSB_REG, sensor_data, QMP6988_DATA_FRAME_SIZE);
busReadRegisterBufferStart(&baro->dev, QMP6988_PRESSURE_MSB_REG, sensor_data, QMP6988_DATA_FRAME_SIZE);
return true;
}
static bool qmp6988GetUP(baroDev_t *baro)
{
if (busBusy(&baro->busdev, NULL)) {
if (busBusy(&baro->dev, NULL)) {
return false;
}

195
src/main/drivers/bus.c
View File

@ -27,165 +27,138 @@
#include "drivers/bus_i2c_busdev.h"
#include "drivers/bus_spi.h"
bool busRawWriteRegister(const busDevice_t *busdev, uint8_t reg, uint8_t data)
// Access routines where the register is accessed directly
bool busRawWriteRegister(const extDevice_t *dev, uint8_t reg, uint8_t data)
{
#ifdef USE_SPI
if (busdev->bustype == BUSTYPE_SPI) {
#ifdef USE_SPI_TRANSACTION
spiBusTransactionSetup(busdev);
#endif
return spiBusWriteRegister(busdev, reg, data);
if (dev->bus->busType == BUS_TYPE_SPI) {
return spiWriteRegRB(dev, reg, data);
} else
#endif
{
return busWriteRegister(busdev, reg, data);
return busWriteRegister(dev, reg, data);
}
}
bool busWriteRegister(const busDevice_t *busdev, uint8_t reg, uint8_t data)
bool busRawWriteRegisterStart(const extDevice_t *dev, uint8_t reg, uint8_t data)
{
#ifdef USE_SPI
if (dev->bus->busType == BUS_TYPE_SPI) {
return spiWriteRegRB(dev, reg, data);
} else
#endif
{
return busWriteRegisterStart(dev, reg, data);
}
}
bool busRawReadRegisterBuffer(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length)
{
#ifdef USE_SPI
if (dev->bus->busType == BUS_TYPE_SPI) {
return spiReadRegBufRB(dev, reg, data, length);
} else
#endif
{
return busReadRegisterBuffer(dev, reg, data, length);
}
}
bool busRawReadRegisterBufferStart(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length)
{
#ifdef USE_SPI
if (dev->bus->busType == BUS_TYPE_SPI) {
return spiReadRegBufRB(dev, reg, data, length);
} else
#endif
{
return busReadRegisterBufferStart(dev, reg, data, length);
}
}
// Write routines where the register is masked with 0x7f
bool busWriteRegister(const extDevice_t *dev, uint8_t reg, uint8_t data)
{
#if !defined(USE_SPI) && !defined(USE_I2C)
UNUSED(reg);
UNUSED(data);
#endif
switch (busdev->bustype) {
switch (dev->bus->busType) {
#ifdef USE_SPI
case BUSTYPE_SPI:
#ifdef USE_SPI_TRANSACTION
// XXX Watch out fastpath users, if any
return spiBusTransactionWriteRegister(busdev, reg & 0x7f, data);
#else
return spiBusWriteRegister(busdev, reg & 0x7f, data);
#endif
case BUS_TYPE_SPI:
return spiWriteRegRB(dev, reg & 0x7f, data);
#endif
#ifdef USE_I2C
case BUSTYPE_I2C:
return i2cBusWriteRegister(busdev, reg, data);
case BUS_TYPE_I2C:
return i2cBusWriteRegister(dev, reg, data);
#endif
default:
return false;
}
}
bool busRawWriteRegisterStart(const busDevice_t *busdev, uint8_t reg, uint8_t data)
{
#ifdef USE_SPI
if (busdev->bustype == BUSTYPE_SPI) {
#ifdef USE_SPI_TRANSACTION
spiBusTransactionSetup(busdev);
#endif
return spiBusWriteRegister(busdev, reg, data);
} else
#endif
{
return busWriteRegisterStart(busdev, reg, data);
}
}
bool busWriteRegisterStart(const busDevice_t *busdev, uint8_t reg, uint8_t data)
bool busWriteRegisterStart(const extDevice_t *dev, uint8_t reg, uint8_t data)
{
#if !defined(USE_SPI) && !defined(USE_I2C)
UNUSED(reg);
UNUSED(data);
#endif
switch (busdev->bustype) {
switch (dev->bus->busType) {
#ifdef USE_SPI
case BUSTYPE_SPI:
#ifdef USE_SPI_TRANSACTION
// XXX Watch out fastpath users, if any
return spiBusTransactionWriteRegister(busdev, reg & 0x7f, data);
#else
return spiBusWriteRegister(busdev, reg & 0x7f, data);
#endif
case BUS_TYPE_SPI:
return spiWriteRegRB(dev, reg & 0x7f, data);
#endif
#ifdef USE_I2C
case BUSTYPE_I2C:
return i2cBusWriteRegisterStart(busdev, reg, data);
case BUS_TYPE_I2C:
return i2cBusWriteRegisterStart(dev, reg, data);
#endif
default:
return false;
}
}
bool busRawReadRegisterBuffer(const busDevice_t *busdev, uint8_t reg, uint8_t *data, uint8_t length)
{
#ifdef USE_SPI
if (busdev->bustype == BUSTYPE_SPI) {
#ifdef USE_SPI_TRANSACTION
spiBusTransactionSetup(busdev);
#endif
return spiBusRawReadRegisterBuffer(busdev, reg, data, length);
} else
#endif
{
return busReadRegisterBuffer(busdev, reg, data, length);
}
}
bool busReadRegisterBuffer(const busDevice_t *busdev, uint8_t reg, uint8_t *data, uint8_t length)
// Read routines where the register is ORed with 0x80
bool busReadRegisterBuffer(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length)
{
#if !defined(USE_SPI) && !defined(USE_I2C)
UNUSED(reg);
UNUSED(data);
UNUSED(length);
#endif
switch (busdev->bustype) {
switch (dev->bus->busType) {
#ifdef USE_SPI
case BUSTYPE_SPI:
#ifdef USE_SPI_TRANSACTION
// XXX Watch out fastpath users, if any
return spiBusTransactionReadRegisterBuffer(busdev, reg | 0x80, data, length);
#else
return spiBusReadRegisterBuffer(busdev, reg | 0x80, data, length);
#endif
case BUS_TYPE_SPI:
return spiReadRegMskBufRB(dev, reg | 0x80, data, length);
#endif
#ifdef USE_I2C
case BUSTYPE_I2C:
return i2cBusReadRegisterBuffer(busdev, reg, data, length);
case BUS_TYPE_I2C:
return i2cBusReadRegisterBuffer(dev, reg, data, length);
#endif
default:
return false;
}
}
bool busRawReadRegisterBufferStart(const busDevice_t *busdev, uint8_t reg, uint8_t *data, uint8_t length)
{
#ifdef USE_SPI
if (busdev->bustype == BUSTYPE_SPI) {
#ifdef USE_SPI_TRANSACTION
spiBusTransactionSetup(busdev);
#endif
return spiBusRawReadRegisterBuffer(busdev, reg, data, length);
} else
#endif
{
return busReadRegisterBufferStart(busdev, reg, data, length);
}
}
// Start the I2C read, but do not wait for completion
bool busReadRegisterBufferStart(const busDevice_t *busdev, uint8_t reg, uint8_t *data, uint8_t length)
bool busReadRegisterBufferStart(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length)
{
#if !defined(USE_SPI) && !defined(USE_I2C)
UNUSED(reg);
UNUSED(data);
UNUSED(length);
#endif
switch (busdev->bustype) {
switch (dev->bus->busType) {
#ifdef USE_SPI
case BUSTYPE_SPI:
case BUS_TYPE_SPI:
// For SPI allow the transaction to complete
#ifdef USE_SPI_TRANSACTION
// XXX Watch out fastpath users, if any
return spiBusTransactionReadRegisterBuffer(busdev, reg | 0x80, data, length);
#else
return spiBusReadRegisterBuffer(busdev, reg | 0x80, data, length);
#endif
return spiReadRegMskBufRB(dev, reg | 0x80, data, length);
#endif
#ifdef USE_I2C
case BUSTYPE_I2C:
case BUS_TYPE_I2C:
// Initiate the read access
return i2cBusReadRegisterBufferStart(busdev, reg, data, length);
return i2cBusReadRegisterBufferStart(dev, reg, data, length);
#endif
default:
return false;
@ -193,21 +166,21 @@ bool busReadRegisterBufferStart(const busDevice_t *busdev, uint8_t reg, uint8_t
}
// Returns true if bus is still busy
bool busBusy(const busDevice_t *busdev, bool *error)
bool busBusy(const extDevice_t *dev, bool *error)
{
#if !defined(USE_I2C)
UNUSED(error);
#endif
switch (busdev->bustype) {
switch (dev->bus->busType) {
#ifdef USE_SPI
case BUSTYPE_SPI:
case BUS_TYPE_SPI:
// No waiting on SPI
return false;
#endif
#ifdef USE_I2C
case BUSTYPE_I2C:
return i2cBusBusy(busdev, error);
case BUS_TYPE_I2C:
return i2cBusBusy(dev, error);
#endif
default:
@ -215,35 +188,35 @@ bool busBusy(const busDevice_t *busdev, bool *error)
}
}
uint8_t busReadRegister(const busDevice_t *busdev, uint8_t reg)
uint8_t busReadRegister(const extDevice_t *dev, uint8_t reg)
{
#if !defined(USE_SPI) && !defined(USE_I2C)
UNUSED(busdev);
UNUSED(dev);
UNUSED(reg);
return false;
#else
uint8_t data;
busReadRegisterBuffer(busdev, reg, &data, 1);
busReadRegisterBuffer(dev, reg, &data, 1);
return data;
#endif
}
void busDeviceRegister(const busDevice_t *busdev)
void busDeviceRegister(const extDevice_t *dev)
{
#if !defined(USE_SPI) && !defined(USE_I2C)
UNUSED(busdev);
UNUSED(dev);
#endif
switch (busdev->bustype) {
switch (dev->bus->busType) {
#if defined(USE_SPI)
case BUSTYPE_SPI:
spiBusDeviceRegister(busdev);
case BUS_TYPE_SPI:
spiBusDeviceRegister(dev);
break;
#endif
#if defined(USE_I2C)
case BUSTYPE_I2C:
i2cBusDeviceRegister(busdev);
case BUS_TYPE_I2C:
i2cBusDeviceRegister(dev);
break;
#endif

134
src/main/drivers/bus.h
View File

@ -24,55 +24,125 @@
#include "drivers/bus_i2c.h"
#include "drivers/io_types.h"
#include "drivers/dma.h"
typedef enum {
BUSTYPE_NONE = 0,
BUSTYPE_I2C,
BUSTYPE_SPI,
BUSTYPE_MPU_SLAVE, // Slave I2C on SPI master
BUSTYPE_GYRO_AUTO, // Only used by acc/gyro bus auto detection code
BUS_TYPE_NONE = 0,
BUS_TYPE_I2C,
BUS_TYPE_SPI,
BUS_TYPE_MPU_SLAVE, // Slave I2C on SPI master
BUS_TYPE_GYRO_AUTO, // Only used by acc/gyro bus auto detection code
} busType_e;
struct spiDevice_s;
typedef enum {
BUS_READY,
BUS_BUSY,
BUS_ABORT
} busStatus_e;
// Bus interface, independent of connected device
typedef struct busDevice_s {
busType_e bustype;
busType_e busType;
union {
struct deviceSpi_s {
struct busSpi_s {
SPI_TypeDef *instance;
#ifdef USE_SPI_TRANSACTION
struct SPIDevice_s *device; // Back ptr to controller for this device.
// Cached SPI_CR1 for spiBusTransactionXXX
uint16_t modeCache; // XXX cr1Value may be a better name?
#endif
#if defined(USE_HAL_DRIVER)
SPI_HandleTypeDef* handle; // cached here for efficiency
#endif
IO_t csnPin;
uint16_t speed;
bool leadingEdge;
} spi;
struct deviceI2C_s {
struct busI2C_s {
I2CDevice device;
} i2c;
struct busMpuSlave_s {
struct extDevice_s *master;
} mpuSlave;
} busType_u;
bool useDMA;
bool useAtomicWait;
uint8_t deviceCount;
resourceOwner_e owner; // owner of first device to use this bus
dmaChannelDescriptor_t *dmaTx;
dmaChannelDescriptor_t *dmaRx;
uint32_t dmaTxChannel;
uint32_t dmaRxChannel;
#ifndef UNIT_TEST
// Use a reference here as this saves RAM for unused descriptors
#if defined(USE_FULL_LL_DRIVER)
LL_DMA_InitTypeDef *initTx;
LL_DMA_InitTypeDef *initRx;
#else
DMA_InitTypeDef *initTx;
DMA_InitTypeDef *initRx;
#endif
#endif // UNIT_TEST
struct busSegment_s* volatile curSegment;
bool initSegment;
} busDevice_t;
/* Each SPI access may comprise multiple parts, for example, wait/write enable/write/data each of which
* is defined by a segment, with optional callback after each is completed
*/
typedef struct busSegment_s {
uint8_t *txData;
uint8_t *rxData;
int len;
bool negateCS; // Should CS be negated at the end of this segment
busStatus_e (*callback)(uint32_t arg);
} busSegment_t;
// External device has an associated bus and bus dependent address
typedef struct extDevice_s {
busDevice_t *bus;
union {
struct extSpi_s {
uint16_t speed;
IO_t csnPin;
bool leadingEdge;
} spi;
struct extI2C_s {
uint8_t address;
} i2c;
struct deviceMpuSlave_s {
const struct busDevice_s *master;
struct extMpuSlave_s {
uint8_t address;
} mpuSlave;
} busdev_u;
} busDevice_t;
} busType_u;
#ifndef UNIT_TEST
// Cache the init structure for the next DMA transfer to reduce inter-segment delay
#if defined(USE_HAL_DRIVER)
LL_DMA_InitTypeDef initTx;
LL_DMA_InitTypeDef initRx;
#else
DMA_InitTypeDef initTx;
DMA_InitTypeDef initRx;
#endif
#endif // UNIT_TEST
// Support disabling DMA on a per device basis
bool useDMA;
// Per device buffer reference if needed
uint8_t *txBuf, *rxBuf;
// Connected devices on the same bus may support different speeds
uint32_t callbackArg;
} extDevice_t;
#ifdef TARGET_BUS_INIT
void targetBusInit(void);
#endif
bool busRawWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
bool busWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
bool busRawWriteRegisterStart(const busDevice_t *bus, uint8_t reg, uint8_t data);
bool busWriteRegisterStart(const busDevice_t *bus, uint8_t reg, uint8_t data);
bool busRawReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t *data, uint8_t length);
bool busReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t *data, uint8_t length);
uint8_t busReadRegister(const busDevice_t *bus, uint8_t reg);
bool busRawReadRegisterBufferStart(const busDevice_t *busdev, uint8_t reg, uint8_t *data, uint8_t length);
bool busReadRegisterBufferStart(const busDevice_t *busdev, uint8_t reg, uint8_t *data, uint8_t length);
bool busBusy(const busDevice_t *busdev, bool *error);
void busDeviceRegister(const busDevice_t *busdev);
// Access routines where the register is accessed directly
bool busRawWriteRegister(const extDevice_t *dev, uint8_t reg, uint8_t data);
bool busRawWriteRegisterStart(const extDevice_t *dev, uint8_t reg, uint8_t data);
bool busRawReadRegisterBuffer(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length);
bool busRawReadRegisterBufferStart(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length);
// Write routines where the register is masked with 0x7f
bool busWriteRegister(const extDevice_t *dev, uint8_t reg, uint8_t data);
bool busWriteRegisterStart(const extDevice_t *dev, uint8_t reg, uint8_t data);
// Read routines where the register is ORed with 0x80
bool busReadRegisterBuffer(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length);
bool busReadRegisterBufferStart(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length);
uint8_t busReadRegister(const extDevice_t *dev, uint8_t reg);
bool busBusy(const extDevice_t *dev, bool *error);
void busDeviceRegister(const extDevice_t *dev);

44
src/main/drivers/bus_i2c_busdev.c
View File

@ -31,46 +31,62 @@
static uint8_t i2cRegisteredDeviceCount = 0;
bool i2cBusWriteRegister(const busDevice_t *busdev, uint8_t reg, uint8_t data)
bool i2cBusWriteRegister(const extDevice_t *dev, uint8_t reg, uint8_t data)
{
return i2cWrite(busdev->busdev_u.i2c.device, busdev->busdev_u.i2c.address, reg, data);
return i2cWrite(dev->bus->busType_u.i2c.device, dev->busType_u.i2c.address, reg, data);
}
bool i2cBusWriteRegisterStart(const busDevice_t *busdev, uint8_t reg, uint8_t data)
bool i2cBusWriteRegisterStart(const extDevice_t *dev, uint8_t reg, uint8_t data)
{
// Need a static value, not on the stack
static uint8_t byte;
byte = data;
return i2cWriteBuffer(busdev->busdev_u.i2c.device, busdev->busdev_u.i2c.address, reg, sizeof (byte), &byte);
return i2cWriteBuffer(dev->bus->busType_u.i2c.device, dev->busType_u.i2c.address, reg, sizeof (byte), &byte);
}
bool i2cBusReadRegisterBuffer(const busDevice_t *busdev, uint8_t reg, uint8_t *data, uint8_t length)
bool i2cBusReadRegisterBuffer(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length)
{
return i2cRead(busdev->busdev_u.i2c.device, busdev->busdev_u.i2c.address, reg, length, data);
return i2cRead(dev->bus->busType_u.i2c.device, dev->busType_u.i2c.address, reg, length, data);
}
uint8_t i2cBusReadRegister(const busDevice_t *busdev, uint8_t reg)
uint8_t i2cBusReadRegister(const extDevice_t *dev, uint8_t reg)
{
uint8_t data;
i2cRead(busdev->busdev_u.i2c.device, busdev->busdev_u.i2c.address, reg, 1, &data);
i2cRead(dev->bus->busType_u.i2c.device, dev->busType_u.i2c.address, reg, 1, &data);
return data;
}
bool i2cBusReadRegisterBufferStart(const busDevice_t *busdev, uint8_t reg, uint8_t *data, uint8_t length)
bool i2cBusReadRegisterBufferStart(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length)
{
return i2cReadBuffer(busdev->busdev_u.i2c.device, busdev->busdev_u.i2c.address, reg, length, data);
return i2cReadBuffer(dev->bus->busType_u.i2c.device, dev->busType_u.i2c.address, reg, length, data);
}
bool i2cBusBusy(const busDevice_t *busdev, bool *error)
bool i2cBusBusy(const extDevice_t *dev, bool *error)
{
return i2cBusy(busdev->busdev_u.i2c.device, error);
return i2cBusy(dev->bus->busType_u.i2c.device, error);
}
void i2cBusDeviceRegister(const busDevice_t *busdev)
bool i2cBusSetInstance(extDevice_t *dev, uint32_t device)
{
UNUSED(busdev);
// I2C bus structures to associate with external devices
static busDevice_t i2cBus[I2CDEV_COUNT];
if ((device < 1) || (device > I2CDEV_COUNT)) {
return false;
}
dev->bus = &i2cBus[I2C_CFG_TO_DEV(device)];
dev->bus->busType = BUS_TYPE_I2C;
dev->bus->busType_u.i2c.device = I2C_CFG_TO_DEV(device);
return true;
}
void i2cBusDeviceRegister(const extDevice_t *dev)
{
UNUSED(dev);
i2cRegisteredDeviceCount++;
}

16
src/main/drivers/bus_i2c_busdev.h
View File

@ -20,10 +20,12 @@
#pragma once
bool i2cBusWriteRegister(const busDevice_t *busdev, uint8_t reg, uint8_t data);
bool i2cBusWriteRegisterStart(const busDevice_t *busdev, uint8_t reg, uint8_t data);
bool i2cBusReadRegisterBuffer(const busDevice_t *busdev, uint8_t reg, uint8_t *data, uint8_t length);
uint8_t i2cBusReadRegister(const busDevice_t *bus, uint8_t reg);
bool i2cBusReadRegisterBufferStart(const busDevice_t *busdev, uint8_t reg, uint8_t *data, uint8_t length);
bool i2cBusBusy(const busDevice_t *busdev, bool *error);
void i2cBusDeviceRegister(const busDevice_t *busdev);
bool i2cBusWriteRegister(const extDevice_t *dev, uint8_t reg, uint8_t data);
bool i2cBusWriteRegisterStart(const extDevice_t *dev, uint8_t reg, uint8_t data);
bool i2cBusReadRegisterBuffer(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length);
uint8_t i2cBusReadRegister(const extDevice_t *dev, uint8_t reg);
bool i2cBusReadRegisterBufferStart(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length);
bool i2cBusBusy(const extDevice_t *dev, bool *error);
// Associate a device with an I2C bus
bool i2cBusSetInstance(const extDevice_t *dev, uint32_t device);
void i2cBusDeviceRegister(const extDevice_t *dev);

528
src/main/drivers/bus_spi.c
View File

@ -24,18 +24,24 @@
#include "platform.h"
#include "build/atomic.h"
#ifdef USE_SPI
#include "drivers/bus.h"
#include "drivers/bus_spi.h"
#include "drivers/bus_spi_impl.h"
#include "drivers/dma_reqmap.h"
#include "drivers/exti.h"
#include "drivers/io.h"
#include "drivers/motor.h"
#include "drivers/rcc.h"
#include "nvic.h"
static uint8_t spiRegisteredDeviceCount = 0;
spiDevice_t spiDevice[SPIDEV_COUNT];
busDevice_t spiBusDevice[SPIDEV_COUNT];
SPIDevice spiDeviceByInstance(SPI_TypeDef *instance)
{
@ -71,7 +77,7 @@ SPI_TypeDef *spiInstanceByDevice(SPIDevice device)
return spiDevice[device].dev;
}
bool spiInit(SPIDevice device, bool leadingEdge)
bool spiInit(SPIDevice device)
{
switch (device) {
case SPIINVALID:
@ -79,7 +85,7 @@ bool spiInit(SPIDevice device, bool leadingEdge)
case SPIDEV_1:
#ifdef USE_SPI_DEVICE_1
spiInitDevice(device, leadingEdge);
spiInitDevice(device);
return true;
#else
break;
@ -87,7 +93,7 @@ bool spiInit(SPIDevice device, bool leadingEdge)
case SPIDEV_2:
#ifdef USE_SPI_DEVICE_2
spiInitDevice(device, leadingEdge);
spiInitDevice(device);
return true;
#else
break;
@ -95,7 +101,7 @@ bool spiInit(SPIDevice device, bool leadingEdge)
case SPIDEV_3:
#if defined(USE_SPI_DEVICE_3) && !defined(STM32F1)
spiInitDevice(device, leadingEdge);
spiInitDevice(device);
return true;
#else
break;
@ -103,7 +109,7 @@ bool spiInit(SPIDevice device, bool leadingEdge)
case SPIDEV_4:
#if defined(USE_SPI_DEVICE_4)
spiInitDevice(device, leadingEdge);
spiInitDevice(device);
return true;
#else
break;
@ -111,7 +117,7 @@ bool spiInit(SPIDevice device, bool leadingEdge)
case SPIDEV_5:
#if defined(USE_SPI_DEVICE_5)
spiInitDevice(device, leadingEdge);
spiInitDevice(device);
return true;
#else
break;
@ -119,7 +125,7 @@ bool spiInit(SPIDevice device, bool leadingEdge)
case SPIDEV_6:
#if defined(USE_SPI_DEVICE_6)
spiInitDevice(device, leadingEdge);
spiInitDevice(device);
return true;
#else
break;
@ -128,116 +134,248 @@ bool spiInit(SPIDevice device, bool leadingEdge)
return false;
}
uint32_t spiTimeoutUserCallback(SPI_TypeDef *instance)
// Return true if DMA engine is busy
bool spiIsBusy(const extDevice_t *dev)
{
SPIDevice device = spiDeviceByInstance(instance);
if (device == SPIINVALID) {
return -1;
return (dev->bus->curSegment != (busSegment_t *)NULL);
}
// Indicate that the bus on which this device resides may initiate DMA transfers from interrupt context
void spiSetAtomicWait(const extDevice_t *dev)
{
dev->bus->useAtomicWait = true;
}
// Wait for DMA completion and claim the bus driver
void spiWaitClaim(const extDevice_t *dev)
{
// If there is a device on the bus whose driver might call spiSequence from an ISR then an
// atomic access is required to claim the bus, however if not, then interrupts need not be
// disabled as this can result in edge triggered interrupts being missed
if (dev->bus->useAtomicWait) {
// Prevent race condition where the bus appears free, but a gyro interrupt starts a transfer
do {
ATOMIC_BLOCK(NVIC_PRIO_MPU_INT_EXTI) {
if (dev->bus->curSegment == (busSegment_t *)NULL) {
dev->bus->curSegment = (busSegment_t *)0x04;
}
spiDevice[device].errorCount++;
return spiDevice[device].errorCount;
}
bool spiBusTransfer(const busDevice_t *bus, const uint8_t *txData, uint8_t *rxData, int length)
{
IOLo(bus->busdev_u.spi.csnPin);
spiTransfer(bus->busdev_u.spi.instance, txData, rxData, length);
IOHi(bus->busdev_u.spi.csnPin);
return true;
}
uint16_t spiGetErrorCounter(SPI_TypeDef *instance)
{
SPIDevice device = spiDeviceByInstance(instance);
if (device == SPIINVALID) {
return 0;
}
return spiDevice[device].errorCount;
}
void spiResetErrorCounter(SPI_TypeDef *instance)
{
SPIDevice device = spiDeviceByInstance(instance);
if (device != SPIINVALID) {
spiDevice[device].errorCount = 0;
} while (dev->bus->curSegment != (busSegment_t *)0x04);
} else {
// Wait for completion
while (dev->bus->curSegment != (busSegment_t *)NULL);
}
}
bool spiBusIsBusBusy(const busDevice_t *bus)
// Wait for DMA completion
void spiWait(const extDevice_t *dev)
{
return spiIsBusBusy(bus->busdev_u.spi.instance);
// Wait for completion
while (dev->bus->curSegment != (busSegment_t *)NULL);
}
uint8_t spiBusTransferByte(const busDevice_t *bus, uint8_t data)
// Wait for bus to become free, then read/write block of data
void spiReadWriteBuf(const extDevice_t *dev, uint8_t *txData, uint8_t *rxData, int len)
{
return spiTransferByte(bus->busdev_u.spi.instance, data);
// This routine blocks so no need to use static data
busSegment_t segments[] = {
{txData, rxData, len, true, NULL},
{NULL, NULL, 0, true, NULL},
};
// Ensure any prior DMA has completed before continuing
spiWaitClaim(dev);
spiSequence(dev, &segments[0]);
spiWait(dev);
}
void spiBusWriteByte(const busDevice_t *bus, uint8_t data)
// Read/Write a block of data, returning false if the bus is busy
bool spiReadWriteBufRB(const extDevice_t *dev, uint8_t *txData, uint8_t *rxData, int length)
{
IOLo(bus->busdev_u.spi.csnPin);
spiBusTransferByte(bus, data);
IOHi(bus->busdev_u.spi.csnPin);
}
// Ensure any prior DMA has completed before continuing
if (spiIsBusy(dev)) {
return false;
}
bool spiBusRawTransfer(const busDevice_t *bus, const uint8_t *txData, uint8_t *rxData, int len)
{
return spiTransfer(bus->busdev_u.spi.instance, txData, rxData, len);
}
bool spiBusWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
{
IOLo(bus->busdev_u.spi.csnPin);
spiTransferByte(bus->busdev_u.spi.instance, reg);
spiTransferByte(bus->busdev_u.spi.instance, data);
IOHi(bus->busdev_u.spi.csnPin);
spiReadWriteBuf(dev, txData, rxData, length);
return true;
}
bool spiBusRawReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t *data, uint8_t length)
// Wait for bus to become free, then read/write a single byte
uint8_t spiReadWrite(const extDevice_t *dev, uint8_t data)
{
IOLo(bus->busdev_u.spi.csnPin);
spiTransferByte(bus->busdev_u.spi.instance, reg);
spiTransfer(bus->busdev_u.spi.instance, NULL, data, length);
IOHi(bus->busdev_u.spi.csnPin);
uint8_t retval;
// This routine blocks so no need to use static data
busSegment_t segments[] = {
{&data, &retval, sizeof (data), true, NULL},
{NULL, NULL, 0, true, NULL},
};
// Ensure any prior DMA has completed before continuing
spiWaitClaim(dev);
spiSequence(dev, &segments[0]);
spiWait(dev);
return retval;
}
// Wait for bus to become free, then read/write a single byte from a register
uint8_t spiReadWriteReg(const extDevice_t *dev, uint8_t reg, uint8_t data)
{
uint8_t retval;
// This routine blocks so no need to use static data
busSegment_t segments[] = {
{&reg, NULL, sizeof (reg), false, NULL},
{&data, &retval, sizeof (data), true, NULL},
{NULL, NULL, 0, true, NULL},
};
// Ensure any prior DMA has completed before continuing
spiWaitClaim(dev);
spiSequence(dev, &segments[0]);
spiWait(dev);
return retval;
}
// Wait for bus to become free, then write a single byte
void spiWrite(const extDevice_t *dev, uint8_t data)
{
// This routine blocks so no need to use static data
busSegment_t segments[] = {
{&data, NULL, sizeof (data), true, NULL},
{NULL, NULL, 0, true, NULL},
};
// Ensure any prior DMA has completed before continuing
spiWaitClaim(dev);
spiSequence(dev, &segments[0]);
spiWait(dev);
}
// Write data to a register
void spiWriteReg(const extDevice_t *dev, uint8_t reg, uint8_t data)
{
// This routine blocks so no need to use static data
busSegment_t segments[] = {
{&reg, NULL, sizeof (reg), false, NULL},
{&data, NULL, sizeof (data), true, NULL},
{NULL, NULL, 0, true, NULL},
};
// Ensure any prior DMA has completed before continuing
spiWaitClaim(dev);
spiSequence(dev, &segments[0]);
spiWait(dev);
}
// Write data to a register, returning false if the bus is busy
bool spiWriteRegRB(const extDevice_t *dev, uint8_t reg, uint8_t data)
{
// Ensure any prior DMA has completed before continuing
if (spiIsBusy(dev)) {
return false;
}
spiWriteReg(dev, reg, data);
return true;
}
bool spiBusReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t *data, uint8_t length)
// Read a block of data from a register
void spiReadRegBuf(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length)
{
return spiBusRawReadRegisterBuffer(bus, reg | 0x80, data, length);
// This routine blocks so no need to use static data
busSegment_t segments[] = {
{&reg, NULL, sizeof (reg), false, NULL},
{NULL, data, length, true, NULL},
{NULL, NULL, 0, true, NULL},
};
// Ensure any prior DMA has completed before continuing
spiWaitClaim(dev);
spiSequence(dev, &segments[0]);
spiWait(dev);
}
void spiBusWriteRegisterBuffer(const busDevice_t *bus, uint8_t reg, const uint8_t *data, uint8_t length)
// Read a block of data from a register, returning false if the bus is busy
bool spiReadRegBufRB(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length)
{
IOLo(bus->busdev_u.spi.csnPin);
spiTransferByte(bus->busdev_u.spi.instance, reg);
spiTransfer(bus->busdev_u.spi.instance, data, NULL, length);
IOHi(bus->busdev_u.spi.csnPin);
// Ensure any prior DMA has completed before continuing
if (spiIsBusy(dev)) {
return false;
}
spiReadRegBuf(dev, reg, data, length);
return true;
}
uint8_t spiBusRawReadRegister(const busDevice_t *bus, uint8_t reg)
// Read a block of data where the register is ORed with 0x80, returning false if the bus is busy
bool spiReadRegMskBufRB(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length)
{
return spiReadRegBufRB(dev, reg | 0x80, data, length);
}
// Wait for bus to become free, then write a block of data to a register
void spiWriteRegBuf(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint32_t length)
{
// This routine blocks so no need to use static data
busSegment_t segments[] = {
{&reg, NULL, sizeof (reg), false, NULL},
{data, NULL, length, true, NULL},
{NULL, NULL, 0, true, NULL},
};
// Ensure any prior DMA has completed before continuing
spiWaitClaim(dev);
spiSequence(dev, &segments[0]);
spiWait(dev);
}
// Wait for bus to become free, then read a byte from a register
uint8_t spiReadReg(const extDevice_t *dev, uint8_t reg)
{
uint8_t data;
IOLo(bus->busdev_u.spi.csnPin);
spiTransferByte(bus->busdev_u.spi.instance, reg);
spiTransfer(bus->busdev_u.spi.instance, NULL, &data, 1);
IOHi(bus->busdev_u.spi.csnPin);
// This routine blocks so no need to use static data
busSegment_t segments[] = {
{&reg, NULL, sizeof (reg), false, NULL},
{NULL, &data, sizeof (data), true, NULL},
{NULL, NULL, 0, true, NULL},
};
// Ensure any prior DMA has completed before continuing
spiWaitClaim(dev);
spiSequence(dev, &segments[0]);
spiWait(dev);
return data;
}
uint8_t spiBusReadRegister(const busDevice_t *bus, uint8_t reg)
// Wait for bus to become free, then read a byte of data where the register is ORed with 0x80
uint8_t spiReadRegMsk(const extDevice_t *dev, uint8_t reg)
{
return spiBusRawReadRegister(bus, reg | 0x80);
}
void spiBusSetInstance(busDevice_t *bus, SPI_TypeDef *instance)
{
bus->bustype = BUSTYPE_SPI;
bus->busdev_u.spi.instance = instance;
return spiReadReg(dev, reg | 0x80);
}
uint16_t spiCalculateDivider(uint32_t freq)
@ -259,54 +397,225 @@ uint16_t spiCalculateDivider(uint32_t freq)
return divisor;
}
void spiBusSetDivisor(busDevice_t *bus, uint16_t divisor)
// Interrupt handler for SPI receive DMA completion
static void spiRxIrqHandler(dmaChannelDescriptor_t* descriptor)
{
spiSetDivisor(bus->busdev_u.spi.instance, divisor);
// bus->busdev_u.spi.modeCache = bus->busdev_u.spi.instance->CR1;
const extDevice_t *dev = (const extDevice_t *)descriptor->userParam;
if (!dev) {
return;
}
busDevice_t *bus = dev->bus;
if (bus->curSegment->negateCS) {
// Negate Chip Select
IOHi(dev->busType_u.spi.csnPin);
}
spiInternalStopDMA(dev);
#ifdef __DCACHE_PRESENT
#ifdef STM32H7
if (bus->curSegment->rxData &&
((bus->curSegment->rxData < &_dmaram_start__) || (bus->curSegment->rxData >= &_dmaram_end__))) {
#else
if (bus->curSegment->rxData) {
#endif
// Invalidate the D cache covering the area into which data has been read
SCB_InvalidateDCache_by_Addr(
(uint32_t *)((uint32_t)bus->curSegment->rxData & ~CACHE_LINE_MASK),
(((uint32_t)bus->curSegment->rxData & CACHE_LINE_MASK) +
bus->curSegment->len - 1 + CACHE_LINE_SIZE) & ~CACHE_LINE_MASK);
}
#endif // __DCACHE_PRESENT
if (bus->curSegment->callback) {
switch(bus->curSegment->callback(dev->callbackArg)) {
case BUS_BUSY:
// Repeat the last DMA segment
bus->curSegment--;
// Reinitialise the cached init values as segment is not progressing
spiInternalInitStream(dev, true);
break;
case BUS_ABORT:
bus->curSegment = (busSegment_t *)NULL;
return;
case BUS_READY:
default:
// Advance to the next DMA segment
break;
}
}
// Advance through the segment list
bus->curSegment++;
if (bus->curSegment->len == 0) {
// The end of the segment list has been reached, so mark transactions as complete
bus->curSegment = (busSegment_t *)NULL;
} else {
// After the completion of the first segment setup the init structure for the subsequent segment
if (bus->initSegment) {
spiInternalInitStream(dev, false);
bus->initSegment = false;
}
// Launch the next transfer
spiInternalStartDMA(dev);
// Prepare the init structures ready for the next segment to reduce inter-segment time
spiInternalInitStream(dev, true);
}
}
#ifdef USE_SPI_TRANSACTION
// Separate set of spiBusTransactionXXX to keep fast path for acc/gyros.
void spiBusTransactionBegin(const busDevice_t *bus)
// Mark this bus as being SPI and record the first owner to use it
bool spiSetBusInstance(extDevice_t *dev, uint32_t device, resourceOwner_e owner)
{
spiBusTransactionSetup(bus);
IOLo(bus->busdev_u.spi.csnPin);
if (device > SPIDEV_COUNT) {
return false;
}
dev->bus = &spiBusDevice[SPI_CFG_TO_DEV(device)];
if (dev->bus->busType == BUS_TYPE_SPI) {
// This bus has already been initialised
dev->bus->deviceCount++;
return true;
}
busDevice_t *bus = dev->bus;
bus->busType_u.spi.instance = spiInstanceByDevice(SPI_CFG_TO_DEV(device));
if (bus->busType_u.spi.instance == NULL) {
return false;
}
bus->busType = BUS_TYPE_SPI;
dev->useDMA = true;
bus->useDMA = false;
bus->useAtomicWait = false;
bus->deviceCount = 1;
bus->owner = owner;
bus->initTx = &dev->initTx;
bus->initRx = &dev->initRx;
return true;
}
void spiBusTransactionEnd(const busDevice_t *bus)
void spiInitBusDMA()
{
IOHi(bus->busdev_u.spi.csnPin);
uint32_t device;
#ifdef STM32F4
/* Check https://www.st.com/resource/en/errata_sheet/dm00037591-stm32f405407xx-and-stm32f415417xx-device-limitations-stmicroelectronics.pdf
* section 2.1.10 which reports an errata that corruption may occurs on DMA2 if AHB peripherals (eg GPIO ports) are
* access concurrently with APB peripherals (eg SPI busses). Bitbang DSHOT uses DMA2 to write to GPIO ports. If this
* is enabled, then don't enable DMA on an SPI bus using DMA2
*/
const bool dshotBitbangActive = isDshotBitbangActive(&motorConfig()->dev);
#endif
for (device = 0; device < SPIDEV_COUNT; device++) {
busDevice_t *bus = &spiBusDevice[device];
if (bus->busType != BUS_TYPE_SPI) {
// This bus is not in use
continue;
}
dmaIdentifier_e dmaTxIdentifier = DMA_NONE;
dmaIdentifier_e dmaRxIdentifier = DMA_NONE;
for (uint8_t opt = 0; opt < MAX_PERIPHERAL_DMA_OPTIONS; opt++) {
const dmaChannelSpec_t *dmaTxChannelSpec = dmaGetChannelSpecByPeripheral(DMA_PERIPH_SPI_TX, device, opt);
if (dmaTxChannelSpec) {
dmaTxIdentifier = dmaGetIdentifier(dmaTxChannelSpec->ref);
if (dmaGetOwner(dmaTxIdentifier)->owner != OWNER_FREE) {
dmaTxIdentifier = DMA_NONE;
continue;
}
#ifdef STM32F4
if (dshotBitbangActive && (DMA_DEVICE_NO(dmaTxIdentifier) == 2)) {
dmaTxIdentifier = DMA_NONE;
break;
}
#endif
bus->dmaTxChannel = dmaTxChannelSpec->channel;
dmaInit(dmaTxIdentifier, bus->owner, 0);
break;
}
}
for (uint8_t opt = 0; opt < MAX_PERIPHERAL_DMA_OPTIONS; opt++) {
const dmaChannelSpec_t *dmaRxChannelSpec = dmaGetChannelSpecByPeripheral(DMA_PERIPH_SPI_RX, device, opt);
if (dmaRxChannelSpec) {
dmaRxIdentifier = dmaGetIdentifier(dmaRxChannelSpec->ref);
if (dmaGetOwner(dmaRxIdentifier)->owner != OWNER_FREE) {
dmaRxIdentifier = DMA_NONE;
continue;
}
#ifdef STM32F4
if (dshotBitbangActive && (DMA_DEVICE_NO(dmaRxIdentifier) == 2)) {
dmaRxIdentifier = DMA_NONE;
break;
}
#endif
bus->dmaRxChannel = dmaRxChannelSpec->channel;
dmaInit(dmaRxIdentifier, bus->owner, 0);
break;
}
}
if (dmaTxIdentifier && dmaRxIdentifier) {
bus->dmaTx = dmaGetDescriptorByIdentifier(dmaTxIdentifier);
bus->dmaRx = dmaGetDescriptorByIdentifier(dmaRxIdentifier);
// Ensure streams are disabled
spiInternalResetStream(bus->dmaRx);
spiInternalResetStream(bus->dmaTx);
spiInternalResetDescriptors(bus);
/* Note that this driver may be called both from the normal thread of execution, or from USB interrupt
* handlers, so the DMA completion interrupt must be at a higher priority
*/
dmaSetHandler(dmaRxIdentifier, spiRxIrqHandler, NVIC_PRIO_SPI_DMA, 0);
bus->useDMA = true;
}
}
}
bool spiBusTransactionTransfer(const busDevice_t *bus, const uint8_t *txData, uint8_t *rxData, int length)
void spiSetClkDivisor(const extDevice_t *dev, uint16_t divisor)
{
spiBusTransactionSetup(bus);
return spiBusTransfer(bus, txData, rxData, length);
((extDevice_t *)dev)->busType_u.spi.speed = divisor;
}
bool spiBusTransactionWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data)
// Set the clock phase/polarity to be used for accesses by the given device
void spiSetClkPhasePolarity(const extDevice_t *dev, bool leadingEdge)
{
spiBusTransactionSetup(bus);
return spiBusWriteRegister(bus, reg, data);
((extDevice_t *)dev)->busType_u.spi.leadingEdge = leadingEdge;
}
uint8_t spiBusTransactionReadRegister(const busDevice_t *bus, uint8_t reg)
// Enable/disable DMA on a specific device. Enabled by default.
void spiDmaEnable(const extDevice_t *dev, bool enable)
{
spiBusTransactionSetup(bus);
return spiBusReadRegister(bus, reg);
((extDevice_t *)dev)->useDMA = enable;
}
bool spiBusTransactionReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t *data, uint8_t length)
bool spiUseDMA(const extDevice_t *dev)
{
spiBusTransactionSetup(bus);
return spiBusReadRegisterBuffer(bus, reg, data, length);
return dev->bus->useDMA && dev->useDMA;
}
#endif // USE_SPI_TRANSACTION
void spiBusDeviceRegister(const busDevice_t *bus)
void spiBusDeviceRegister(const extDevice_t *dev)
{
UNUSED(bus);
UNUSED(dev);
spiRegisteredDeviceCount++;
}
@ -315,4 +624,9 @@ uint8_t spiGetRegisteredDeviceCount(void)
{
return spiRegisteredDeviceCount;
}
uint8_t spiGetExtDeviceCount(const extDevice_t *dev)
{
return dev->bus->deviceCount;
}
#endif

90
src/main/drivers/bus_spi.h
View File

@ -81,7 +81,6 @@ typedef enum SPIDevice {
#define SPIDEV_COUNT 6
#else
#define SPIDEV_COUNT 4
#endif
// Macros to convert between CLI bus number and SPIDevice.
@ -102,49 +101,67 @@ void spiPreinitRegister(ioTag_t iotag, uint8_t iocfg, uint8_t init);
void spiPreinitByIO(IO_t io);
void spiPreinitByTag(ioTag_t tag);
bool spiInit(SPIDevice device, bool leadingEdge);
void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor);
uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t data);
bool spiIsBusBusy(SPI_TypeDef *instance);
bool spiInit(SPIDevice device);
bool spiTransfer(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData, int len);
// Called after all devices are initialised to enable SPI DMA where streams are available.
void spiInitBusDMA();
uint16_t spiGetErrorCounter(SPI_TypeDef *instance);
void spiResetErrorCounter(SPI_TypeDef *instance);
SPIDevice spiDeviceByInstance(SPI_TypeDef *instance);
SPI_TypeDef *spiInstanceByDevice(SPIDevice device);
//
// BusDevice API
//
bool spiBusIsBusBusy(const busDevice_t *bus);
bool spiBusTransfer(const busDevice_t *bus, const uint8_t *txData, uint8_t *rxData, int length);
uint8_t spiBusTransferByte(const busDevice_t *bus, uint8_t data);
void spiBusWriteByte(const busDevice_t *bus, uint8_t data);
bool spiBusRawTransfer(const busDevice_t *bus, const uint8_t *txData, uint8_t *rxData, int len);
bool spiBusWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
bool spiBusRawReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t *data, uint8_t length);
bool spiBusReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t *data, uint8_t length);
void spiBusWriteRegisterBuffer(const busDevice_t *bus, uint8_t reg, const uint8_t *data, uint8_t length);
uint8_t spiBusRawReadRegister(const busDevice_t *bus, uint8_t reg);
uint8_t spiBusReadRegister(const busDevice_t *bus, uint8_t reg);
void spiBusSetInstance(busDevice_t *bus, SPI_TypeDef *instance);
// Mark a device's associated bus as being SPI and record the first owner to use it
bool spiSetBusInstance(extDevice_t *dev, uint32_t device, resourceOwner_e owner);
// Determine the divisor to use for a given bus frequency
uint16_t spiCalculateDivider(uint32_t freq);
void spiBusSetDivisor(busDevice_t *bus, uint16_t divider);
// Set the clock divisor to be used for accesses by the given device
void spiSetClkDivisor(const extDevice_t *dev, uint16_t divider);
// Set the clock phase/polarity to be used for accesses by the given device
void spiSetClkPhasePolarity(const extDevice_t *dev, bool leadingEdge);
// Enable/disable DMA on a specific device. Enabled by default.
void spiDmaEnable(const extDevice_t *dev, bool enable);
void spiBusTransactionInit(busDevice_t *bus, SPIMode_e mode, uint16_t divider);
void spiBusTransactionSetup(const busDevice_t *bus);
void spiBusTransactionBegin(const busDevice_t *bus);
void spiBusTransactionEnd(const busDevice_t *bus);
bool spiBusTransactionWriteRegister(const busDevice_t *bus, uint8_t reg, uint8_t data);
uint8_t spiBusTransactionReadRegister(const busDevice_t *bus, uint8_t reg);
bool spiBusTransactionReadRegisterBuffer(const busDevice_t *bus, uint8_t reg, uint8_t *data, uint8_t length);
bool spiBusTransactionTransfer(const busDevice_t *bus, const uint8_t *txData, uint8_t *rxData, int length);
// DMA transfer setup and start
void spiSequence(const extDevice_t *dev, busSegment_t *segments);
// Wait for DMA completion
void spiWait(const extDevice_t *dev);
// Indicate that the bus on which this device resides may initiate DMA transfers from interrupt context
void spiSetAtomicWait(const extDevice_t *dev);
// Wait for DMA completion and claim the bus driver - use this when waiting for a prior access to complete before starting a new one
void spiWaitClaim(const extDevice_t *dev);
// Return true if DMA engine is busy
bool spiIsBusy(const extDevice_t *dev);
/*
* Routine naming convention is:
* spi[Read][Write][Reg][Msk][Buf][RB]
*
* Read: Perform a read, returning the value read unless 'Buf' is specified
* Write Perform a write
* ReadWrite: Perform both a read and write, returning the value read unless 'Buf' is specified
* Reg: Register number 'reg' is written prior to the read being performed
* Msk: Register number is logically ORed with 0x80 as some devices indicate a read by accessing a register with bit 7 set
* Buf: Pass data of given length by reference
* RB: Return false immediately if the bus is busy, otherwise complete the access and return true
*/
uint8_t spiReadReg(const extDevice_t *dev, uint8_t reg);
uint8_t spiReadRegMsk(const extDevice_t *dev, uint8_t reg);
void spiReadRegBuf(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length);
bool spiReadRegBufRB(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length);
bool spiReadRegMskBufRB(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint8_t length);
void spiWrite(const extDevice_t *dev, uint8_t data);
void spiWriteReg(const extDevice_t *dev, uint8_t reg, uint8_t data);
bool spiWriteRegRB(const extDevice_t *dev, uint8_t reg, uint8_t data);
uint8_t spiReadWrite(const extDevice_t *dev, uint8_t data);
void spiWriteRegBuf(const extDevice_t *dev, uint8_t reg, uint8_t *data, uint32_t length);
uint8_t spiReadWriteReg(const extDevice_t *dev, uint8_t reg, uint8_t data);
void spiReadWriteBuf(const extDevice_t *dev, uint8_t *txData, uint8_t *rxData, int len);
bool spiReadWriteBufRB(const extDevice_t *dev, uint8_t *txData, uint8_t *rxData, int length);
//
// Config
@ -152,5 +169,8 @@ bool spiBusTransactionTransfer(const busDevice_t *bus, const uint8_t *txData, ui
struct spiPinConfig_s;
void spiPinConfigure(const struct spiPinConfig_s *pConfig);
void spiBusDeviceRegister(const busDevice_t *bus);
bool spiUseDMA(const extDevice_t *dev);
void spiBusDeviceRegister(const extDevice_t *dev);
uint8_t spiGetRegisteredDeviceCount(void);
uint8_t spiGetExtDeviceCount(const extDevice_t *dev);

229
src/main/drivers/bus_spi_hal.c
View File

@ -20,6 +20,8 @@
* HAL version resurrected from v3.1.7 (by jflyper)
*/
// Note that the HAL driver is polled only
#include <stdbool.h>
#include <stdint.h>
#include <strings.h>
@ -38,7 +40,44 @@
#define SPI_TIMEOUT_SYS_TICKS (SPI_TIMEOUT_US / 1000)
void spiInitDevice(SPIDevice device, bool leadingEdge)
// Position of Prescaler bits are different from MCU to MCU
static uint32_t baudRatePrescaler[8] = {
SPI_BAUDRATEPRESCALER_2,
SPI_BAUDRATEPRESCALER_4,
SPI_BAUDRATEPRESCALER_8,
SPI_BAUDRATEPRESCALER_16,
SPI_BAUDRATEPRESCALER_32,
SPI_BAUDRATEPRESCALER_64,
SPI_BAUDRATEPRESCALER_128,
SPI_BAUDRATEPRESCALER_256,
};
static void spiDivisorToBRbits(SPI_TypeDef *instance, uint16_t divisor)
{
SPIDevice device = spiDeviceByInstance(instance);
spiDevice_t *spi = &(spiDevice[device]);
int prescalerIndex = ffs(divisor) - 2; // prescaler begins at "/2"
if (prescalerIndex < 0 || prescalerIndex >= (int)ARRAYLEN(baudRatePrescaler)) {
return;
}
if (spi->hspi.Init.BaudRatePrescaler != baudRatePrescaler[prescalerIndex]) {
spi->hspi.Init.BaudRatePrescaler = baudRatePrescaler[prescalerIndex];
MODIFY_REG(spi->hspi.Instance->CR1, SPI_CR1_BR_Msk, spi->hspi.Init.BaudRatePrescaler);
}
}
static void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor)
{
spiDivisorToBRbits(instance, divisor);
}
void spiInitDevice(SPIDevice device)
{
spiDevice_t *spi = &(spiDevice[device]);
@ -46,8 +85,6 @@ void spiInitDevice(SPIDevice device, bool leadingEdge)
return;
}
spi->leadingEdge = leadingEdge;
// Enable SPI clock
RCC_ClockCmd(spi->rcc, ENABLE);
RCC_ResetCmd(spi->rcc, ENABLE);
@ -91,128 +128,122 @@ void spiInitDevice(SPIDevice device, bool leadingEdge)
spi->hspi.Init.FifoThreshold = SPI_FIFO_THRESHOLD_01DATA;
spi->hspi.Init.MasterKeepIOState = SPI_MASTER_KEEP_IO_STATE_ENABLE; /* Recommanded setting to avoid glitches */
#endif
if (spi->leadingEdge) {
spi->hspi.Init.CLKPolarity = SPI_POLARITY_LOW;
spi->hspi.Init.CLKPhase = SPI_PHASE_1EDGE;
} else {
spi->hspi.Init.CLKPolarity = SPI_POLARITY_HIGH;
spi->hspi.Init.CLKPhase = SPI_PHASE_2EDGE;
}
// Init SPI hardware
HAL_SPI_Init(&spi->hspi);
}
// return uint8_t value or -1 when failure
uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t out)
{
uint8_t in;
spiTransfer(instance, &out, &in, 1);
return in;
}
/**
* Return true if the bus is currently in the middle of a transmission.
*/
bool spiIsBusBusy(SPI_TypeDef *instance)
{
SPIDevice device = spiDeviceByInstance(instance);
if (spiDevice[device].hspi.State == HAL_SPI_STATE_BUSY) {
return true;
} else {
return false;
}
}
bool spiTransfer(SPI_TypeDef *instance, const uint8_t *out, uint8_t *in, int len)
static bool spiInternalReadWriteBufPolled(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData, int len)
{
SPIDevice device = spiDeviceByInstance(instance);
HAL_StatusTypeDef status;
if (!in) {
if (!rxData) {
// Tx only
status = HAL_SPI_Transmit(&spiDevice[device].hspi, out, len, SPI_TIMEOUT_SYS_TICKS);
} else if (!out) {
status = HAL_SPI_Transmit(&spiDevice[device].hspi, txData, len, SPI_TIMEOUT_SYS_TICKS);
} else if(!txData) {
// Rx only
status = HAL_SPI_Receive(&spiDevice[device].hspi, in, len, SPI_TIMEOUT_SYS_TICKS);
status = HAL_SPI_Receive(&spiDevice[device].hspi, rxData, len, SPI_TIMEOUT_SYS_TICKS);
} else {
// Tx and Rx
status = HAL_SPI_TransmitReceive(&spiDevice[device].hspi, out, in, len, SPI_TIMEOUT_SYS_TICKS);
status = HAL_SPI_TransmitReceive(&spiDevice[device].hspi, txData, rxData, len, SPI_TIMEOUT_SYS_TICKS);
}
if (status != HAL_OK) {
spiTimeoutUserCallback(instance);
}
return true;
return (status == HAL_OK);
}
// Position of Prescaler bits are different from MCU to MCU
static uint32_t baudRatePrescaler[8] = {
SPI_BAUDRATEPRESCALER_2,
SPI_BAUDRATEPRESCALER_4,
SPI_BAUDRATEPRESCALER_8,
SPI_BAUDRATEPRESCALER_16,
SPI_BAUDRATEPRESCALER_32,
SPI_BAUDRATEPRESCALER_64,
SPI_BAUDRATEPRESCALER_128,
SPI_BAUDRATEPRESCALER_256,
};
void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor)
void spiInternalInitStream(const extDevice_t *dev, bool preInit)
{
SPIDevice device = spiDeviceByInstance(instance);
UNUSED(dev);
UNUSED(preInit);
}
HAL_SPI_DeInit(&spiDevice[device].hspi);
void spiInternalStartDMA(const extDevice_t *dev)
{
UNUSED(dev);
}
spiDevice_t *spi = &(spiDevice[device]);
void spiInternalStopDMA (const extDevice_t *dev)
{
UNUSED(dev);
}
int prescalerIndex = ffs(divisor) - 2; // prescaler begins at "/2"
void spiInternalResetDescriptors(busDevice_t *bus)
{
UNUSED(bus);
}
if (prescalerIndex < 0 || prescalerIndex >= (int)ARRAYLEN(baudRatePrescaler)) {
void spiInternalResetStream(dmaChannelDescriptor_t *descriptor)
{
UNUSED(descriptor);
}
// Transfer setup and start
void spiSequence(const extDevice_t *dev, busSegment_t *segments)
{
busDevice_t *bus = dev->bus;
SPIDevice device = spiDeviceByInstance(bus->busType_u.spi.instance);
SPI_HandleTypeDef *hspi = &spiDevice[device].hspi;
bus->initSegment = true;
bus->curSegment = segments;
// Switch bus speed
spiSetDivisor(bus->busType_u.spi.instance, dev->busType_u.spi.speed);
// Switch SPI clock polarity/phase if necessary
if (dev->busType_u.spi.leadingEdge != bus->busType_u.spi.leadingEdge) {
if (dev->busType_u.spi.leadingEdge){
hspi->Init.CLKPolarity = SPI_POLARITY_LOW;
hspi->Init.CLKPhase = SPI_PHASE_1EDGE;
} else {
hspi->Init.CLKPolarity = SPI_POLARITY_HIGH;
hspi->Init.CLKPhase = SPI_PHASE_2EDGE;
}
bus->busType_u.spi.leadingEdge = dev->busType_u.spi.leadingEdge;
// Init SPI hardware
HAL_SPI_Init(hspi);
}
// Manually work through the segment list performing a transfer for each
while (bus->curSegment->len) {
// Assert Chip Select
IOLo(dev->busType_u.spi.csnPin);
spiInternalReadWriteBufPolled(
bus->busType_u.spi.instance,
bus->curSegment->txData,
bus->curSegment->rxData,
bus->curSegment->len);
if (bus->curSegment->negateCS) {
// Negate Chip Select
IOHi(dev->busType_u.spi.csnPin);
}
if (bus->curSegment->callback) {
switch(bus->curSegment->callback(dev->callbackArg)) {
case BUS_BUSY:
// Repeat the last DMA segment
bus->curSegment--;
break;
case BUS_ABORT:
bus->curSegment = (busSegment_t *)NULL;
return;
case BUS_READY:
default:
// Advance to the next DMA segment
break;
}
}
bus->curSegment++;
}
spi->hspi.Init.BaudRatePrescaler = baudRatePrescaler[prescalerIndex];
HAL_SPI_Init(&spi->hspi);
bus->curSegment = (busSegment_t *)NULL;
}
#ifdef USE_DMA
DMA_HandleTypeDef* dmaHandleByInstance(SPI_TypeDef *instance)
{
return &spiDevice[spiDeviceByInstance(instance)].hdma;
}
void SPI1_IRQHandler(void)
{
HAL_SPI_IRQHandler(&spiDevice[SPIDEV_1].hspi);
}
void SPI2_IRQHandler(void)
{
HAL_SPI_IRQHandler(&spiDevice[SPIDEV_2].hspi);
}
void SPI3_IRQHandler(void)
{
HAL_SPI_IRQHandler(&spiDevice[SPIDEV_3].hspi);
}
void SPI4_IRQHandler(void)
{
HAL_SPI_IRQHandler(&spiDevice[SPIDEV_4].hspi);
}
void dmaSPIIRQHandler(dmaChannelDescriptor_t* descriptor)
{
SPIDevice device = descriptor->userParam;
if (device != SPIINVALID) {
HAL_DMA_IRQHandler(&spiDevice[device].hdma);
}
}
#endif // USE_DMA
#endif

21
src/main/drivers/bus_spi_impl.h
View File

@ -49,7 +49,7 @@ typedef struct spiHardware_s {
uint8_t af;
#endif
rccPeriphTag_t rcc;
#if defined(USE_DMA) && defined(USE_HAL_DRIVER)
#ifdef USE_DMA
uint8_t dmaIrqHandler;
#endif
} spiHardware_t;
@ -67,23 +67,24 @@ typedef struct SPIDevice_s {
uint8_t mosiAF;
#else
uint8_t af;
#endif
#if defined(HAL_SPI_MODULE_ENABLED)
SPI_HandleTypeDef hspi;
#endif
rccPeriphTag_t rcc;
volatile uint16_t errorCount;
bool leadingEdge;
#if defined(USE_HAL_DRIVER)
SPI_HandleTypeDef hspi;
#ifdef USE_DMA
DMA_HandleTypeDef hdma;
uint8_t dmaIrqHandler;
#endif
#endif
#ifdef USE_SPI_TRANSACTION
uint16_t cr1SoftCopy; // Copy of active CR1 value for this SPI instance
#endif
} spiDevice_t;
extern spiDevice_t spiDevice[SPIDEV_COUNT];
void spiInitDevice(SPIDevice device, bool leadingEdge);
uint32_t spiTimeoutUserCallback(SPI_TypeDef *instance);
void spiInitDevice(SPIDevice device);
void spiInternalInitStream(const extDevice_t *dev, bool preInit);
void spiInternalStartDMA(const extDevice_t *dev);
void spiInternalStopDMA (const extDevice_t *dev);
void spiInternalResetStream(dmaChannelDescriptor_t *descriptor);
void spiInternalResetDescriptors(busDevice_t *bus);

712
src/main/drivers/bus_spi_ll.c
View File

@ -36,7 +36,6 @@
#include "drivers/io.h"
#include "drivers/nvic.h"
#include "drivers/rcc.h"
#include "drivers/time.h"
#ifndef SPI2_SCK_PIN
#define SPI2_NSS_PIN PB12
@ -72,165 +71,29 @@
#define SPI4_NSS_PIN NONE
#endif
#define SPI_DEFAULT_TIMEOUT 10
#ifdef STM32H7
#define IS_DTCM(p) (((uint32_t)p & 0xfffe0000) == 0x20000000)
#elif defined(STM32F7)
#define IS_DTCM(p) (((uint32_t)p & 0xffff0000) == 0x20000000)
#endif
static LL_SPI_InitTypeDef defaultInit =
{
.TransferDirection = SPI_DIRECTION_2LINES,
.Mode = SPI_MODE_MASTER,
.DataWidth = SPI_DATASIZE_8BIT,
.NSS = SPI_NSS_SOFT,
.BaudRate = SPI_BAUDRATEPRESCALER_8,
.BitOrder = SPI_FIRSTBIT_MSB,
.CRCPoly = 7,
.CRCCalculation = SPI_CRCCALCULATION_DISABLE,
.TransferDirection = LL_SPI_FULL_DUPLEX,
.Mode = LL_SPI_MODE_MASTER,
.DataWidth = LL_SPI_DATAWIDTH_8BIT,
.NSS = LL_SPI_NSS_SOFT,
.BaudRate = LL_SPI_BAUDRATEPRESCALER_DIV8,
.BitOrder = LL_SPI_MSB_FIRST,
.CRCCalculation = LL_SPI_CRCCALCULATION_DISABLE,
.ClockPolarity = LL_SPI_POLARITY_HIGH,
.ClockPhase = LL_SPI_PHASE_2EDGE,
};
void spiInitDevice(SPIDevice device, bool leadingEdge)
static uint32_t spiDivisorToBRbits(SPI_TypeDef *instance, uint16_t divisor)
{
spiDevice_t *spi = &(spiDevice[device]);
if (!spi->dev) {
return;
}
#ifndef USE_SPI_TRANSACTION
spi->leadingEdge = leadingEdge;
#else
UNUSED(leadingEdge);
#endif
// Enable SPI clock
RCC_ClockCmd(spi->rcc, ENABLE);
RCC_ResetCmd(spi->rcc, ENABLE);
IOInit(IOGetByTag(spi->sck), OWNER_SPI_SCK, RESOURCE_INDEX(device));
IOInit(IOGetByTag(spi->miso), OWNER_SPI_MISO, RESOURCE_INDEX(device));
IOInit(IOGetByTag(spi->mosi), OWNER_SPI_MOSI, RESOURCE_INDEX(device));
if (spi->leadingEdge == true) {
IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_LOW, spi->sckAF);
} else {
IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_HIGH, spi->sckAF);
}
IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG, spi->misoAF);
IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->mosiAF);
LL_SPI_Disable(spi->dev);
LL_SPI_DeInit(spi->dev);
#ifndef USE_SPI_TRANSACTION
if (spi->leadingEdge) {
defaultInit.ClockPolarity = SPI_POLARITY_LOW;
defaultInit.ClockPhase = SPI_PHASE_1EDGE;
} else
#endif
{
defaultInit.ClockPolarity = SPI_POLARITY_HIGH;
defaultInit.ClockPhase = SPI_PHASE_2EDGE;
}
LL_SPI_SetRxFIFOThreshold(spi->dev, SPI_RXFIFO_THRESHOLD_QF);
LL_SPI_Init(spi->dev, &defaultInit);
LL_SPI_Enable(spi->dev);
}
uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t txByte)
{
timeUs_t timeoutStartUs = microsISR();
while (!LL_SPI_IsActiveFlag_TXE(instance)) {
if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) {
return spiTimeoutUserCallback(instance);
}
}
LL_SPI_TransmitData8(instance, txByte);
timeoutStartUs = microsISR();
while (!CHECK_SPI_RX_DATA_AVAILABLE(instance)) {
if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) {
return spiTimeoutUserCallback(instance);
}
}
return (uint8_t)LL_SPI_ReceiveData8(instance);
}
/**
* Return true if the bus is currently in the middle of a transmission.
*/
bool spiIsBusBusy(SPI_TypeDef *instance)
{
return LL_SPI_GetTxFIFOLevel(instance) != LL_SPI_TX_FIFO_EMPTY
|| LL_SPI_IsActiveFlag_BSY(instance);
}
bool spiTransfer(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData, int len)
{
timeUs_t timeoutStartUs;
// set 16-bit transfer
CLEAR_BIT(instance->CR2, SPI_RXFIFO_THRESHOLD);
while (len > 1) {
timeoutStartUs = microsISR();
while (!LL_SPI_IsActiveFlag_TXE(instance)) {
if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) {
return spiTimeoutUserCallback(instance);
}
}
uint16_t w;
if (txData) {
w = *((uint16_t *)txData);
txData += 2;
} else {
w = 0xFFFF;
}
LL_SPI_TransmitData16(instance, w);
timeoutStartUs = microsISR();
while (!CHECK_SPI_RX_DATA_AVAILABLE(instance)) {
if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) {
return spiTimeoutUserCallback(instance);
}
}
w = LL_SPI_ReceiveData16(instance);
if (rxData) {
*((uint16_t *)rxData) = w;
rxData += 2;
}
len -= 2;
}
// set 8-bit transfer
SET_BIT(instance->CR2, SPI_RXFIFO_THRESHOLD);
if (len) {
timeoutStartUs = microsISR();
while (!LL_SPI_IsActiveFlag_TXE(instance)) {
if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) {
return spiTimeoutUserCallback(instance);
}
}
uint8_t b = txData ? *(txData++) : 0xFF;
LL_SPI_TransmitData8(instance, b);
timeoutStartUs = microsISR();
while (!CHECK_SPI_RX_DATA_AVAILABLE(instance)) {
if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) {
return spiTimeoutUserCallback(instance);
}
}
b = LL_SPI_ReceiveData8(instance);
if (rxData) {
*(rxData++) = b;
}
--len;
}
return true;
}
static uint16_t spiDivisorToBRbits(SPI_TypeDef *instance, uint16_t divisor)
{
#if !(defined(STM32F1) || defined(STM32F3))
#if !defined(STM32H7)
// SPI2 and SPI3 are on APB1/AHB1 which PCLK is half that of APB2/AHB2.
if (instance == SPI2 || instance == SPI3) {
@ -242,65 +105,484 @@ static uint16_t spiDivisorToBRbits(SPI_TypeDef *instance, uint16_t divisor)
divisor = constrain(divisor, 2, 256);
#if defined(STM32H7)
const uint32_t baudRatePrescaler[8] = {
LL_SPI_BAUDRATEPRESCALER_DIV2,
LL_SPI_BAUDRATEPRESCALER_DIV4,
LL_SPI_BAUDRATEPRESCALER_DIV8,
LL_SPI_BAUDRATEPRESCALER_DIV16,
LL_SPI_BAUDRATEPRESCALER_DIV32,
LL_SPI_BAUDRATEPRESCALER_DIV64,
LL_SPI_BAUDRATEPRESCALER_DIV128,
LL_SPI_BAUDRATEPRESCALER_DIV256,
};
int prescalerIndex = ffs(divisor) - 2; // prescaler begins at "/2"
return baudRatePrescaler[prescalerIndex];
#else
return (ffs(divisor) - 2) << SPI_CR1_BR_Pos;
}
void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor)
{
LL_SPI_Disable(instance);
LL_SPI_SetBaudRatePrescaler(instance, spiDivisorToBRbits(instance, divisor));
LL_SPI_Enable(instance);
}
#ifdef USE_SPI_TRANSACTION
void spiBusTransactionInit(busDevice_t *bus, SPIMode_e mode, uint16_t divisor)
{
switch (mode) {
case SPI_MODE0_POL_LOW_EDGE_1ST:
defaultInit.ClockPolarity = SPI_POLARITY_LOW;
defaultInit.ClockPhase = SPI_PHASE_1EDGE;
break;
case SPI_MODE1_POL_LOW_EDGE_2ND:
defaultInit.ClockPolarity = SPI_POLARITY_LOW;
defaultInit.ClockPhase = SPI_PHASE_2EDGE;
break;
case SPI_MODE2_POL_HIGH_EDGE_1ST:
defaultInit.ClockPolarity = SPI_POLARITY_HIGH;
defaultInit.ClockPhase = SPI_PHASE_1EDGE;
break;
case SPI_MODE3_POL_HIGH_EDGE_2ND:
defaultInit.ClockPolarity = SPI_POLARITY_HIGH;
defaultInit.ClockPhase = SPI_PHASE_2EDGE;
break;
}
LL_SPI_Disable(bus->busdev_u.spi.instance);
LL_SPI_DeInit(bus->busdev_u.spi.instance);
LL_SPI_Init(bus->busdev_u.spi.instance, &defaultInit);
LL_SPI_SetBaudRatePrescaler(bus->busdev_u.spi.instance, spiDivisorToBRbits(bus->busdev_u.spi.instance, divisor));
// Configure for 8-bit reads. XXX Is this STM32F303xC specific?
LL_SPI_SetRxFIFOThreshold(bus->busdev_u.spi.instance, SPI_RXFIFO_THRESHOLD_QF);
LL_SPI_Enable(bus->busdev_u.spi.instance);
bus->busdev_u.spi.device = &spiDevice[spiDeviceByInstance(bus->busdev_u.spi.instance)];
bus->busdev_u.spi.modeCache = bus->busdev_u.spi.instance->CR1;
}
void spiBusTransactionSetup(const busDevice_t *bus)
{
// We rely on MSTR bit to detect valid modeCache
if (bus->busdev_u.spi.modeCache && bus->busdev_u.spi.modeCache != bus->busdev_u.spi.device->cr1SoftCopy) {
bus->busdev_u.spi.instance->CR1 = bus->busdev_u.spi.modeCache;
bus->busdev_u.spi.device->cr1SoftCopy = bus->busdev_u.spi.modeCache;
// SCK seems to require some time to switch to a new initial level after CR1 is written.
// Here we buy some time in addition to the software copy save above.
__asm__("nop");
}
}
#endif // USE_SPI_TRANSACTION
#endif
}
void spiInitDevice(SPIDevice device)
{
spiDevice_t *spi = &spiDevice[device];
if (!spi->dev) {
return;
}
// Enable SPI clock
RCC_ClockCmd(spi->rcc, ENABLE);
RCC_ResetCmd(spi->rcc, ENABLE);
IOInit(IOGetByTag(spi->sck), OWNER_SPI_SCK, RESOURCE_INDEX(device));
IOInit(IOGetByTag(spi->miso), OWNER_SPI_MISO, RESOURCE_INDEX(device));
IOInit(IOGetByTag(spi->mosi), OWNER_SPI_MOSI, RESOURCE_INDEX(device));
IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG, spi->misoAF);
IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->mosiAF);
IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_HIGH, spi->sckAF);
LL_SPI_Disable(spi->dev);
LL_SPI_DeInit(spi->dev);
#if defined(STM32H7)
// Prevent glitching when SPI is disabled
LL_SPI_EnableGPIOControl(spi->dev);
LL_SPI_SetFIFOThreshold(spi->dev, LL_SPI_FIFO_TH_01DATA);
LL_SPI_Init(spi->dev, &defaultInit);
#else
LL_SPI_SetRxFIFOThreshold(spi->dev, SPI_RXFIFO_THRESHOLD_QF);
LL_SPI_Init(spi->dev, &defaultInit);
LL_SPI_Enable(spi->dev);
#endif
}
void spiInternalResetDescriptors(busDevice_t *bus)
{
LL_DMA_InitTypeDef *initTx = bus->initTx;
LL_DMA_InitTypeDef *initRx = bus->initRx;
LL_DMA_StructInit(initTx);
#if defined(STM32H7)
initTx->PeriphRequest = bus->dmaTxChannel;
#else
initTx->Channel = bus->dmaTxChannel;
#endif
initTx->Mode = LL_DMA_MODE_NORMAL;
initTx->Direction = LL_DMA_DIRECTION_MEMORY_TO_PERIPH;
#if defined(STM32H7)
initTx->PeriphOrM2MSrcAddress = (uint32_t)&bus->busType_u.spi.instance->TXDR;
#else
initTx->PeriphOrM2MSrcAddress = (uint32_t)&bus->busType_u.spi.instance->DR;
#endif
initTx->Priority = LL_DMA_PRIORITY_LOW;
initTx->PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT;
initTx->PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_BYTE;
initTx->MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_BYTE;
LL_DMA_StructInit(initRx);
#if defined(STM32H7)
initRx->PeriphRequest = bus->dmaRxChannel;
#else
initRx->Channel = bus->dmaRxChannel;
#endif
initRx->Mode = LL_DMA_MODE_NORMAL;
initRx->Direction = LL_DMA_DIRECTION_PERIPH_TO_MEMORY;
#if defined(STM32H7)
initRx->PeriphOrM2MSrcAddress = (uint32_t)&bus->busType_u.spi.instance->RXDR;
#else
initRx->PeriphOrM2MSrcAddress = (uint32_t)&bus->busType_u.spi.instance->DR;
#endif
initRx->Priority = LL_DMA_PRIORITY_LOW;
initRx->PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT;
initRx->PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_BYTE;
}
void spiInternalResetStream(dmaChannelDescriptor_t *descriptor)
{
// Disable the stream
LL_DMA_DisableStream(descriptor->dma, descriptor->stream);
while (LL_DMA_IsEnabledStream(descriptor->dma, descriptor->stream));
// Clear any pending interrupt flags
DMA_CLEAR_FLAG(descriptor, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF);
}
static bool spiInternalReadWriteBufPolled(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData, int len)
{
#if defined(STM32H7)
int txLen = len;
int rxLen = len;
LL_SPI_SetTransferSize(instance, txLen);
LL_SPI_Enable(instance);
LL_SPI_StartMasterTransfer(instance);
while (txLen || rxLen) {
if (txLen && LL_SPI_IsActiveFlag_TXP(instance)) {
uint8_t b = txData ? *(txData++) : 0xFF;
LL_SPI_TransmitData8(instance, b);
txLen--;
}
if (rxLen && LL_SPI_IsActiveFlag_RXP(instance)) {
uint8_t b = LL_SPI_ReceiveData8(instance);
if (rxData) {
*(rxData++) = b;
}
rxLen--;
}
}
while (!LL_SPI_IsActiveFlag_EOT(instance));
LL_SPI_ClearFlag_TXTF(instance);
LL_SPI_Disable(instance);
#else
// set 16-bit transfer
CLEAR_BIT(instance->CR2, SPI_RXFIFO_THRESHOLD);
while (len > 1) {
while (!LL_SPI_IsActiveFlag_TXE(instance));
uint16_t w;
if (txData) {
w = *((uint16_t *)txData);
txData += 2;
} else {
w = 0xFFFF;
}
LL_SPI_TransmitData16(instance, w);
while (!LL_SPI_IsActiveFlag_RXNE(instance));
w = LL_SPI_ReceiveData16(instance);
if (rxData) {
*((uint16_t *)rxData) = w;
rxData += 2;
}
len -= 2;
}
// set 8-bit transfer
SET_BIT(instance->CR2, SPI_RXFIFO_THRESHOLD);
if (len) {
while (!LL_SPI_IsActiveFlag_TXE(instance));
uint8_t b = txData ? *(txData++) : 0xFF;
LL_SPI_TransmitData8(instance, b);
while (!LL_SPI_IsActiveFlag_RXNE(instance));
b = LL_SPI_ReceiveData8(instance);
if (rxData) {
*(rxData++) = b;
}
--len;
}
#endif
return true;
}
void spiInternalInitStream(const extDevice_t *dev, bool preInit)
{
static uint8_t dummyTxByte = 0xff;
static uint8_t dummyRxByte;
busDevice_t *bus = dev->bus;
busSegment_t *segment = bus->curSegment;
if (preInit) {
// Prepare the init structure for the next segment to reduce inter-segment interval
segment++;
if(segment->len == 0) {
// There's no following segment
return;
}
}
uint8_t *txData = segment->txData;
uint8_t *rxData = segment->rxData;
int len = segment->len;
LL_DMA_InitTypeDef *initTx = bus->initTx;
LL_DMA_InitTypeDef *initRx = bus->initRx;
if (txData) {
#ifdef __DCACHE_PRESENT
#ifdef STM32H7
if ((txData < &_dmaram_start__) || (txData >= &_dmaram_end__)) {
#else
// No need to flush DTCM memory
if (!IS_DTCM(txData)) {
#endif
// Flush the D cache to ensure the data to be written is in main memory
SCB_CleanDCache_by_Addr(
(uint32_t *)((uint32_t)txData & ~CACHE_LINE_MASK),
(((uint32_t)txData & CACHE_LINE_MASK) + len - 1 + CACHE_LINE_SIZE) & ~CACHE_LINE_MASK);
}
#endif // __DCACHE_PRESENT
initTx->MemoryOrM2MDstAddress = (uint32_t)txData;
initTx->MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT;
} else {
dummyTxByte = 0xff;
initTx->MemoryOrM2MDstAddress = (uint32_t)&dummyTxByte;
initTx->MemoryOrM2MDstIncMode = LL_DMA_MEMORY_NOINCREMENT;
}
initTx->NbData = len;
if (rxData) {
/* Flush the D cache for the start and end of the receive buffer as
* the cache will be invalidated after the transfer and any valid data
* just before/after must be in memory at that point
*/
#ifdef __DCACHE_PRESENT
// No need to flush/invalidate DTCM memory
#ifdef STM32H7
if ((rxData < &_dmaram_start__) || (rxData >= &_dmaram_end__)) {
#else
// No need to flush DTCM memory
if (!IS_DTCM(rxData)) {
#endif
SCB_CleanInvalidateDCache_by_Addr(
(uint32_t *)((uint32_t)rxData & ~CACHE_LINE_MASK),
(((uint32_t)rxData & CACHE_LINE_MASK) + len - 1 + CACHE_LINE_SIZE) & ~CACHE_LINE_MASK);
}
#endif // __DCACHE_PRESENT
initRx->MemoryOrM2MDstAddress = (uint32_t)rxData;
initRx->MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT;
} else {
initRx->MemoryOrM2MDstAddress = (uint32_t)&dummyRxByte;
initRx->MemoryOrM2MDstIncMode = LL_DMA_MEMORY_NOINCREMENT;
}
// If possible use 16 bit memory writes to prevent atomic access issues on gyro data
if ((initRx->MemoryOrM2MDstAddress & 0x1) || (len & 0x1)) {
initRx->MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_BYTE;
} else {
initRx->MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_HALFWORD;
}
initRx->NbData = len;
}
void spiInternalStartDMA(const extDevice_t *dev)
{
busDevice_t *bus = dev->bus;
// Assert Chip Select
IOLo(dev->busType_u.spi.csnPin);
dmaChannelDescriptor_t *dmaTx = bus->dmaTx;
dmaChannelDescriptor_t *dmaRx = bus->dmaRx;
DMA_Stream_TypeDef *streamRegsTx = (DMA_Stream_TypeDef *)dmaTx->ref;
DMA_Stream_TypeDef *streamRegsRx = (DMA_Stream_TypeDef *)dmaRx->ref;
// Use the correct callback argument
dmaRx->userParam = (uint32_t)dev;
// Clear transfer flags
DMA_CLEAR_FLAG(dmaTx, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF);
DMA_CLEAR_FLAG(dmaRx, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF);
// Disable streams to enable update
LL_DMA_WriteReg(streamRegsTx, CR, 0U);
LL_DMA_WriteReg(streamRegsRx, CR, 0U);
/* Use the Rx interrupt as this occurs once the SPI operation is complete whereas the Tx interrupt
* occurs earlier when the Tx FIFO is empty, but the SPI operation is still in progress
*/
LL_EX_DMA_EnableIT_TC(streamRegsRx);
// Update streams
LL_DMA_Init(dmaTx->dma, dmaTx->stream, bus->initTx);
LL_DMA_Init(dmaRx->dma, dmaRx->stream, bus->initRx);
/* Note from AN4031
*
* If the user enables the used peripheral before the corresponding DMA stream, a “FEIF”
* (FIFO Error Interrupt Flag) may be set due to the fact the DMA is not ready to provide
* the first required data to the peripheral (in case of memory-to-peripheral transfer).
*/
// Enable the SPI DMA Tx & Rx requests
#if defined(STM32H7)
LL_SPI_SetTransferSize(dev->bus->busType_u.spi.instance, dev->bus->curSegment->len);
LL_DMA_EnableStream(dmaTx->dma, dmaTx->stream);
LL_DMA_EnableStream(dmaRx->dma, dmaRx->stream);
SET_BIT(dev->bus->busType_u.spi.instance->CFG1, SPI_CFG1_RXDMAEN | SPI_CFG1_TXDMAEN);
LL_SPI_Enable(dev->bus->busType_u.spi.instance);
LL_SPI_StartMasterTransfer(dev->bus->busType_u.spi.instance);
#else
// Enable streams
LL_DMA_EnableStream(dmaTx->dma, dmaTx->stream);
LL_DMA_EnableStream(dmaRx->dma, dmaRx->stream);
SET_BIT(dev->bus->busType_u.spi.instance->CR2, SPI_CR2_TXDMAEN | SPI_CR2_RXDMAEN);
#endif
}
void spiInternalStopDMA (const extDevice_t *dev)
{
busDevice_t *bus = dev->bus;
dmaChannelDescriptor_t *dmaTx = bus->dmaTx;
dmaChannelDescriptor_t *dmaRx = bus->dmaRx;
SPI_TypeDef *instance = bus->busType_u.spi.instance;
// Disable the DMA engine and SPI interface
LL_DMA_DisableStream(dmaRx->dma, dmaRx->stream);
LL_DMA_DisableStream(dmaTx->dma, dmaTx->stream);
DMA_CLEAR_FLAG(dmaRx, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF);
LL_SPI_DisableDMAReq_TX(instance);
LL_SPI_DisableDMAReq_RX(instance);
#if defined(STM32H7)
LL_SPI_ClearFlag_TXTF(dev->bus->busType_u.spi.instance);
LL_SPI_Disable(dev->bus->busType_u.spi.instance);
#endif
}
// DMA transfer setup and start
void spiSequence(const extDevice_t *dev, busSegment_t *segments)
{
busDevice_t *bus = dev->bus;
SPI_TypeDef *instance = bus->busType_u.spi.instance;
spiDevice_t *spi = &spiDevice[spiDeviceByInstance(instance)];
bool dmaSafe = dev->useDMA;
uint32_t xferLen = 0;
uint32_t segmentCount = 0;
bus->initSegment = true;
bus->curSegment = segments;
// Switch bus speed
#if !defined(STM32H7)
LL_SPI_Disable(instance);
#endif
if (dev->busType_u.spi.speed != bus->busType_u.spi.speed) {
LL_SPI_SetBaudRatePrescaler(instance, spiDivisorToBRbits(instance, dev->busType_u.spi.speed));
bus->busType_u.spi.speed = dev->busType_u.spi.speed;
}
// Switch SPI clock polarity/phase if necessary
if (dev->busType_u.spi.leadingEdge != bus->busType_u.spi.leadingEdge) {
if (dev->busType_u.spi.leadingEdge) {
IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_LOW, spi->sckAF);
LL_SPI_SetClockPhase(instance, LL_SPI_PHASE_1EDGE);
LL_SPI_SetClockPolarity(instance, LL_SPI_POLARITY_LOW);
}
else {
IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_HIGH, spi->sckAF);
LL_SPI_SetClockPhase(instance, LL_SPI_PHASE_2EDGE);
LL_SPI_SetClockPolarity(instance, LL_SPI_POLARITY_HIGH);
}
bus->busType_u.spi.leadingEdge = dev->busType_u.spi.leadingEdge;
}
#if !defined(STM32H7)
LL_SPI_Enable(instance);
#endif
/* Where data is being read into a buffer which is cached, where the start or end of that
* buffer is not cache aligned, there is a risk of corruption of other data in that cache line.
* After the read is complete, the cache lines covering the structure will be invalidated to ensure
* that the processor sees the read data, not what was in cache previously. Unfortunately if
* there is any other data in the area covered by those cache lines, at the start or end of the
* buffer, it too will be invalidated, so had the processor written to those locations during the DMA
* operation those written values will be lost.
*/
// Check that any reads are cache aligned and of multiple cache lines in length
for (busSegment_t *checkSegment = bus->curSegment; checkSegment->len; checkSegment++) {
#ifdef STM32H7
// Check if RX data can be DMAed
if ((checkSegment->rxData) &&
// DTCM can't be accessed by DMA1/2 on the H7
(IS_DTCM(checkSegment->rxData) ||
// Memory declared as DMA_RAM will have an address between &_dmaram_start__ and &_dmaram_end__
(((checkSegment->rxData < &_dmaram_start__) || (checkSegment->rxData >= &_dmaram_end__)) &&
(((uint32_t)checkSegment->rxData & (CACHE_LINE_SIZE - 1)) || (checkSegment->len & (CACHE_LINE_SIZE - 1)))))) {
dmaSafe = false;
break;
}
// Check if TX data can be DMAed
else if ((checkSegment->txData) && IS_DTCM(checkSegment->txData)) {
dmaSafe = false;
break;
}
#elif defined(STM32F7)
if ((checkSegment->rxData) &&
// DTCM is accessible and uncached on the F7
(!IS_DTCM(checkSegment->rxData) &&
(((uint32_t)checkSegment->rxData & (CACHE_LINE_SIZE - 1)) || (checkSegment->len & (CACHE_LINE_SIZE - 1))))) {
dmaSafe = false;
break;
}
#elif defined(STM32F4)
// Check if RX data can be DMAed
if ((checkSegment->rxData) &&
// DTCM can't be accessed by DMA1/2 on the F4
(IS_DTCM(checkSegment->rxData)) {
dmaSafe = false;
break;
}
if ((checkSegment->txData) &&
// DTCM can't be accessed by DMA1/2 on the F4
(IS_DTCM(checkSegment->txData)) {
dmaSafe = false;
break;
}
#endif
// Note that these counts are only valid if dmaSafe is true
segmentCount++;
xferLen += checkSegment->len;
}
// Use DMA if possible
if (bus->useDMA && dmaSafe && ((segmentCount > 1) || (xferLen > 8))) {
// Intialise the init structures for the first transfer
spiInternalInitStream(dev, false);
// Start the transfers
spiInternalStartDMA(dev);
} else {
// Manually work through the segment list performing a transfer for each
while (bus->curSegment->len) {
// Assert Chip Select
IOLo(dev->busType_u.spi.csnPin);
spiInternalReadWriteBufPolled(
bus->busType_u.spi.instance,
bus->curSegment->txData,
bus->curSegment->rxData,
bus->curSegment->len);
if (bus->curSegment->negateCS) {
// Negate Chip Select
IOHi(dev->busType_u.spi.csnPin);
}
if (bus->curSegment->callback) {
switch(bus->curSegment->callback(dev->callbackArg)) {
case BUS_BUSY:
// Repeat the last DMA segment
bus->curSegment--;
break;
case BUS_ABORT:
bus->curSegment = (busSegment_t *)NULL;
return;
case BUS_READY:
default:
// Advance to the next DMA segment
break;
}
}
bus->curSegment++;
}
bus->curSegment = (busSegment_t *)NULL;
}
}
#endif

474
src/main/drivers/bus_spi_stdperiph.c
View File

@ -26,6 +26,9 @@
#ifdef USE_SPI
// STM32F405 can't DMA to/from FASTRAM (CCM SRAM)
#define IS_CCM(p) (((uint32_t)p & 0xffff0000) == 0x10000000)
#include "common/maths.h"
#include "drivers/bus.h"
#include "drivers/bus_spi.h"
@ -33,7 +36,6 @@
#include "drivers/exti.h"
#include "drivers/io.h"
#include "drivers/rcc.h"
#include "drivers/time.h"
static SPI_InitTypeDef defaultInit = {
.SPI_Mode = SPI_Mode_Master,
@ -43,160 +45,19 @@ static SPI_InitTypeDef defaultInit = {
.SPI_FirstBit = SPI_FirstBit_MSB,
.SPI_CRCPolynomial = 7,
.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8,
.SPI_CPOL = SPI_CPOL_High,
.SPI_CPHA = SPI_CPHA_2Edge
};
void spiInitDevice(SPIDevice device, bool leadingEdge)
{
spiDevice_t *spi = &(spiDevice[device]);
if (!spi->dev) {
return;
}
#ifndef USE_SPI_TRANSACTION
spi->leadingEdge = leadingEdge;
#else
UNUSED(leadingEdge);
#endif
// Enable SPI clock
RCC_ClockCmd(spi->rcc, ENABLE);
RCC_ResetCmd(spi->rcc, ENABLE);
IOInit(IOGetByTag(spi->sck), OWNER_SPI_SCK, RESOURCE_INDEX(device));
IOInit(IOGetByTag(spi->miso), OWNER_SPI_MISO, RESOURCE_INDEX(device));
IOInit(IOGetByTag(spi->mosi), OWNER_SPI_MOSI, RESOURCE_INDEX(device));
#if defined(STM32F3) || defined(STM32F4)
IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_CFG, spi->af);
IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_CFG, spi->af);
IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->af);
#elif defined(STM32F10X)
IOConfigGPIO(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG);
IOConfigGPIO(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG);
IOConfigGPIO(IOGetByTag(spi->mosi), SPI_IO_AF_MOSI_CFG);
#else
#error Undefined MCU architecture
#endif
// Init SPI hardware
SPI_I2S_DeInit(spi->dev);
#ifndef USE_SPI_TRANSACTION
if (spi->leadingEdge) {
defaultInit.SPI_CPOL = SPI_CPOL_Low;
defaultInit.SPI_CPHA = SPI_CPHA_1Edge;
} else
#endif
{
defaultInit.SPI_CPOL = SPI_CPOL_High;
defaultInit.SPI_CPHA = SPI_CPHA_2Edge;
}
#ifdef STM32F303xC
// Configure for 8-bit reads.
SPI_RxFIFOThresholdConfig(spi->dev, SPI_RxFIFOThreshold_QF);
#endif
SPI_Init(spi->dev, &defaultInit);
SPI_Cmd(spi->dev, ENABLE);
}
// return uint8_t value or -1 when failure
uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t txByte)
{
timeUs_t timeoutStartUs = microsISR();
DISCARD(instance->DR);
while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_TXE) == RESET) {
if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) {
return spiTimeoutUserCallback(instance);
}
}
#ifdef STM32F303xC
SPI_SendData8(instance, txByte);
#else
SPI_I2S_SendData(instance, txByte);
#endif
timeoutStartUs = microsISR();
while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_RXNE) == RESET) {
if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) {
return spiTimeoutUserCallback(instance);
}
}
#ifdef STM32F303xC
return ((uint8_t)SPI_ReceiveData8(instance));
#else
return ((uint8_t)SPI_I2S_ReceiveData(instance));
#endif
}
/**
* Return true if the bus is currently in the middle of a transmission.
*/
bool spiIsBusBusy(SPI_TypeDef *instance)
{
#ifdef STM32F303xC
return SPI_GetTransmissionFIFOStatus(instance) != SPI_TransmissionFIFOStatus_Empty || SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_BSY) == SET;
#else
return SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_TXE) == RESET || SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_BSY) == SET;
#endif
}
bool spiTransfer(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData, int len)
{
timeUs_t timeoutStartUs;
uint8_t b;
DISCARD(instance->DR);
while (len--) {
b = txData ? *(txData++) : 0xFF;
timeoutStartUs = microsISR();
while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_TXE) == RESET) {
if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) {
return spiTimeoutUserCallback(instance);
}
}
#ifdef STM32F303xC
SPI_SendData8(instance, b);
#else
SPI_I2S_SendData(instance, b);
#endif
timeoutStartUs = microsISR();
while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_RXNE) == RESET) {
if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) {
return spiTimeoutUserCallback(instance);
}
}
#ifdef STM32F303xC
b = SPI_ReceiveData8(instance);
#else
b = SPI_I2S_ReceiveData(instance);
#endif
if (rxData) {
*(rxData++) = b;
}
}
return true;
}
static uint16_t spiDivisorToBRbits(SPI_TypeDef *instance, uint16_t divisor)
{
#if !(defined(STM32F1) || defined(STM32F3))
// SPI2 and SPI3 are on APB1/AHB1 which PCLK is half that of APB2/AHB2.
#if defined(STM32F410xx) || defined(STM32F411xE)
UNUSED(instance);
#else
if (instance == SPI2 || instance == SPI3) {
divisor /= 2; // Safe for divisor == 0 or 1
}
#else
UNUSED(instance);
#endif
divisor = constrain(divisor, 2, 256);
@ -212,61 +73,294 @@ static void spiSetDivisorBRreg(SPI_TypeDef *instance, uint16_t divisor)
#undef BR_BITS
}
void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor)
void spiInitDevice(SPIDevice device)
{
spiDevice_t *spi = &(spiDevice[device]);
if (!spi->dev) {
return;
}
// Enable SPI clock
RCC_ClockCmd(spi->rcc, ENABLE);
RCC_ResetCmd(spi->rcc, ENABLE);
IOInit(IOGetByTag(spi->sck), OWNER_SPI_SCK, RESOURCE_INDEX(device));
IOInit(IOGetByTag(spi->miso), OWNER_SPI_MISO, RESOURCE_INDEX(device));
IOInit(IOGetByTag(spi->mosi), OWNER_SPI_MOSI, RESOURCE_INDEX(device));
IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG, spi->af);
IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG, spi->af);
IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->af);
// Init SPI hardware
SPI_I2S_DeInit(spi->dev);
SPI_I2S_DMACmd(spi->dev, SPI_I2S_DMAReq_Tx | SPI_I2S_DMAReq_Rx, DISABLE);
SPI_Init(spi->dev, &defaultInit);
SPI_Cmd(spi->dev, ENABLE);
}
void spiInternalResetDescriptors(busDevice_t *bus)
{
DMA_InitTypeDef *initTx = bus->initTx;
DMA_InitTypeDef *initRx = bus->initRx;
DMA_StructInit(initTx);
initTx->DMA_Channel = bus->dmaTxChannel;
initTx->DMA_DIR = DMA_DIR_MemoryToPeripheral;
initTx->DMA_Mode = DMA_Mode_Normal;
initTx->DMA_PeripheralBaseAddr = (uint32_t)&bus->busType_u.spi.instance->DR;
initTx->DMA_Priority = DMA_Priority_Low;
initTx->DMA_PeripheralInc = DMA_PeripheralInc_Disable;
initTx->DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
initTx->DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
DMA_StructInit(initRx);
initRx->DMA_Channel = bus->dmaRxChannel;
initRx->DMA_DIR = DMA_DIR_PeripheralToMemory;
initRx->DMA_Mode = DMA_Mode_Normal;
initRx->DMA_PeripheralBaseAddr = (uint32_t)&bus->busType_u.spi.instance->DR;
initRx->DMA_Priority = DMA_Priority_Low;
initRx->DMA_PeripheralInc = DMA_PeripheralInc_Disable;
initRx->DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
}
void spiInternalResetStream(dmaChannelDescriptor_t *descriptor)
{
DMA_Stream_TypeDef *streamRegs = (DMA_Stream_TypeDef *)descriptor->ref;
// Disable the stream
streamRegs->CR = 0U;
// Clear any pending interrupt flags
DMA_CLEAR_FLAG(descriptor, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF);
}
static bool spiInternalReadWriteBufPolled(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData, int len)
{
uint8_t b;
while (len--) {
b = txData ? *(txData++) : 0xFF;
while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_TXE) == RESET);
SPI_I2S_SendData(instance, b);
while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_RXNE) == RESET);
b = SPI_I2S_ReceiveData(instance);
if (rxData) {
*(rxData++) = b;
}
}
return true;
}
void spiInternalInitStream(const extDevice_t *dev, bool preInit)
{
static uint8_t dummyTxByte = 0xff;
static uint8_t dummyRxByte;
busDevice_t *bus = dev->bus;
volatile busSegment_t *segment = bus->curSegment;
if (preInit) {
// Prepare the init structure for the next segment to reduce inter-segment interval
segment++;
if(segment->len == 0) {
// There's no following segment
return;
}
}
uint8_t *txData = segment->txData;
uint8_t *rxData = segment->rxData;
int len = segment->len;
DMA_InitTypeDef *initTx = bus->initTx;
DMA_InitTypeDef *initRx = bus->initRx;
if (txData) {
initTx->DMA_Memory0BaseAddr = (uint32_t)txData;
initTx->DMA_MemoryInc = DMA_MemoryInc_Enable;
} else {
dummyTxByte = 0xff;
initTx->DMA_Memory0BaseAddr = (uint32_t)&dummyTxByte;
initTx->DMA_MemoryInc = DMA_MemoryInc_Disable;
}
initTx->DMA_BufferSize = len;
if (rxData) {
initRx->DMA_Memory0BaseAddr = (uint32_t)rxData;
initRx->DMA_MemoryInc = DMA_MemoryInc_Enable;
} else {
initRx->DMA_Memory0BaseAddr = (uint32_t)&dummyRxByte;
initRx->DMA_MemoryInc = DMA_MemoryInc_Disable;
}
// If possible use 16 bit memory writes to prevent atomic access issues on gyro data
if ((initRx->DMA_Memory0BaseAddr & 0x1) || (len & 0x1))
{
initRx->DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
} else {
initRx->DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
}
initRx->DMA_BufferSize = len;
}
void spiInternalStartDMA(const extDevice_t *dev)
{
// Assert Chip Select
IOLo(dev->busType_u.spi.csnPin);
dmaChannelDescriptor_t *dmaTx = dev->bus->dmaTx;
dmaChannelDescriptor_t *dmaRx = dev->bus->dmaRx;
DMA_Stream_TypeDef *streamRegsTx = (DMA_Stream_TypeDef *)dmaTx->ref;
DMA_Stream_TypeDef *streamRegsRx = (DMA_Stream_TypeDef *)dmaRx->ref;
// Use the correct callback argument
dmaRx->userParam = (uint32_t)dev;
// Clear transfer flags
DMA_CLEAR_FLAG(dmaTx, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF);
DMA_CLEAR_FLAG(dmaRx, DMA_IT_HTIF | DMA_IT_TEIF | DMA_IT_TCIF);
// Disable streams to enable update
streamRegsTx->CR = 0U;
streamRegsRx->CR = 0U;
/* Use the Rx interrupt as this occurs once the SPI operation is complete whereas the Tx interrupt
* occurs earlier when the Tx FIFO is empty, but the SPI operation is still in progress
*/
DMA_ITConfig(streamRegsRx, DMA_IT_TC, ENABLE);
// Update streams
DMA_Init(streamRegsTx, dev->bus->initTx);
DMA_Init(streamRegsRx, dev->bus->initRx);
/* Note from AN4031
*
* If the user enables the used peripheral before the corresponding DMA stream, a “FEIF”
* (FIFO Error Interrupt Flag) may be set due to the fact the DMA is not ready to provide
* the first required data to the peripheral (in case of memory-to-peripheral transfer).
*/
// Enable streams
DMA_Cmd(streamRegsTx, ENABLE);
DMA_Cmd(streamRegsRx, ENABLE);
/* Enable the SPI DMA Tx & Rx requests */
SPI_I2S_DMACmd(dev->bus->busType_u.spi.instance, SPI_I2S_DMAReq_Tx | SPI_I2S_DMAReq_Rx, ENABLE);
}
void spiInternalStopDMA (const extDevice_t *dev)
{
dmaChannelDescriptor_t *dmaTx = dev->bus->dmaTx;
dmaChannelDescriptor_t *dmaRx = dev->bus->dmaRx;
DMA_Stream_TypeDef *streamRegsTx = (DMA_Stream_TypeDef *)dmaTx->ref;
DMA_Stream_TypeDef *streamRegsRx = (DMA_Stream_TypeDef *)dmaRx->ref;
SPI_TypeDef *instance = dev->bus->busType_u.spi.instance;
// Disable streams
streamRegsTx->CR = 0U;
streamRegsRx->CR = 0U;
SPI_I2S_DMACmd(instance, SPI_I2S_DMAReq_Tx | SPI_I2S_DMAReq_Rx, DISABLE);
}
// DMA transfer setup and start
void spiSequence(const extDevice_t *dev, busSegment_t *segments)
{
busDevice_t *bus = dev->bus;
SPI_TypeDef *instance = bus->busType_u.spi.instance;
bool dmaSafe = dev->useDMA;
uint32_t xferLen = 0;
uint32_t segmentCount = 0;
dev->bus->initSegment = true;
dev->bus->curSegment = segments;
SPI_Cmd(instance, DISABLE);
spiSetDivisorBRreg(instance, divisor);
// Switch bus speed
if (dev->busType_u.spi.speed != bus->busType_u.spi.speed) {
spiSetDivisorBRreg(bus->busType_u.spi.instance, dev->busType_u.spi.speed);
bus->busType_u.spi.speed = dev->busType_u.spi.speed;
}
if (dev->busType_u.spi.leadingEdge != bus->busType_u.spi.leadingEdge) {
// Switch SPI clock polarity/phase
instance->CR1 &= ~(SPI_CPOL_High | SPI_CPHA_2Edge);
// Apply setting
if (dev->busType_u.spi.leadingEdge) {
instance->CR1 |= SPI_CPOL_Low | SPI_CPHA_1Edge;
} else
{
instance->CR1 |= SPI_CPOL_High | SPI_CPHA_2Edge;
}
bus->busType_u.spi.leadingEdge = dev->busType_u.spi.leadingEdge;
}
SPI_Cmd(instance, ENABLE);
}
#ifdef USE_SPI_TRANSACTION
void spiBusTransactionInit(busDevice_t *bus, SPIMode_e mode, uint16_t divider)
{
switch (mode) {
case SPI_MODE0_POL_LOW_EDGE_1ST:
defaultInit.SPI_CPOL = SPI_CPOL_Low;
defaultInit.SPI_CPHA = SPI_CPHA_1Edge;
break;
case SPI_MODE1_POL_LOW_EDGE_2ND:
defaultInit.SPI_CPOL = SPI_CPOL_Low;
defaultInit.SPI_CPHA = SPI_CPHA_2Edge;
break;
case SPI_MODE2_POL_HIGH_EDGE_1ST:
defaultInit.SPI_CPOL = SPI_CPOL_High;
defaultInit.SPI_CPHA = SPI_CPHA_1Edge;
break;
case SPI_MODE3_POL_HIGH_EDGE_2ND:
defaultInit.SPI_CPOL = SPI_CPOL_High;
defaultInit.SPI_CPHA = SPI_CPHA_2Edge;
// Check that any there are no attempts to DMA to/from CCD SRAM
for (busSegment_t *checkSegment = bus->curSegment; checkSegment->len; checkSegment++) {
if (((checkSegment->rxData) && IS_CCM(checkSegment->rxData)) ||
((checkSegment->txData) && IS_CCM(checkSegment->txData))) {
dmaSafe = false;
break;
}
// Note that these counts are only valid if dmaSafe is true
segmentCount++;
xferLen += checkSegment->len;
}
// Use DMA if possible
if (bus->useDMA && dmaSafe && ((segmentCount > 1) || (xferLen > 8))) {
// Intialise the init structures for the first transfer
spiInternalInitStream(dev, false);
// Initialize the SPI instance to setup CR1
// Start the transfers
spiInternalStartDMA(dev);
} else {
// Manually work through the segment list performing a transfer for each
while (bus->curSegment->len) {
// Assert Chip Select
IOLo(dev->busType_u.spi.csnPin);
SPI_Init(bus->busdev_u.spi.instance, &defaultInit);
spiSetDivisorBRreg(bus->busdev_u.spi.instance, divider);
#ifdef STM32F303xC
// Configure for 8-bit reads.
SPI_RxFIFOThresholdConfig(bus->busdev_u.spi.instance, SPI_RxFIFOThreshold_QF);
#endif
spiInternalReadWriteBufPolled(
bus->busType_u.spi.instance,
bus->curSegment->txData,
bus->curSegment->rxData,
bus->curSegment->len);
bus->busdev_u.spi.modeCache = bus->busdev_u.spi.instance->CR1;
bus->busdev_u.spi.device = &spiDevice[spiDeviceByInstance(bus->busdev_u.spi.instance)];
}
if (bus->curSegment->negateCS) {
// Negate Chip Select
IOHi(dev->busType_u.spi.csnPin);
}
void spiBusTransactionSetup(const busDevice_t *bus)
{
// We rely on MSTR bit to detect valid modeCache
if (bus->curSegment->callback) {
switch(bus->curSegment->callback(dev->callbackArg)) {
case BUS_BUSY:
// Repeat the last DMA segment
bus->curSegment--;
break;
if (bus->busdev_u.spi.modeCache && bus->busdev_u.spi.modeCache != bus->busdev_u.spi.device->cr1SoftCopy) {
bus->busdev_u.spi.instance->CR1 = bus->busdev_u.spi.modeCache;
bus->busdev_u.spi.device->cr1SoftCopy = bus->busdev_u.spi.modeCache;
case BUS_ABORT:
bus->curSegment = (busSegment_t *)NULL;
return;
// SCK seems to require some time to switch to a new initial level after CR1 is written.
// Here we buy some time in addition to the software copy save above.
__asm__("nop");
case BUS_READY:
default:
// Advance to the next DMA segment
break;
}
}
bus->curSegment++;
}
bus->curSegment = (busSegment_t *)NULL;
}
}
#endif // USE_SPI_TRANSACTION
#endif

3
src/main/drivers/compass/compass.h
View File

@ -30,7 +30,8 @@ typedef struct magDev_s {
sensorMagInitFuncPtr init; // initialize function
sensorMagReadFuncPtr read; // read 3 axis data function
extiCallbackRec_t exti;
busDevice_t busdev;
extDevice_t dev;
busDevice_t bus; // For MPU slave bus instance
sensor_align_e magAlignment;
fp_rotationMatrix_t rotationMatrix;
ioTag_t magIntExtiTag;

171
src/main/drivers/compass/compass_ak8963.c
View File

@ -103,35 +103,35 @@
#if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))
static bool ak8963SpiWriteRegisterDelay(const busDevice_t *bus, uint8_t reg, uint8_t data)
static bool ak8963SpiWriteRegisterDelay(const extDevice_t *dev, uint8_t reg, uint8_t data)
{
spiBusWriteRegister(bus, reg, data);
spiWriteReg(dev, reg, data);
delayMicroseconds(10);
return true;
}
static bool ak8963SlaveReadRegisterBuffer(const busDevice_t *slavedev, uint8_t reg, uint8_t *buf, uint8_t len)
static bool ak8963SlaveReadRegisterBuffer(const extDevice_t *slaveDev, uint8_t reg, uint8_t *buf, uint8_t len)
{
const busDevice_t *bus = slavedev->busdev_u.mpuSlave.master;
extDevice_t *dev = slaveDev->bus->busType_u.mpuSlave.master;
ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_ADDR, slavedev->busdev_u.mpuSlave.address | READ_FLAG); // set I2C slave address for read
ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_REG, reg); // set I2C slave register
ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_CTRL, (len & 0x0F) | I2C_SLV0_EN); // read number of bytes
ak8963SpiWriteRegisterDelay(dev, MPU_RA_I2C_SLV0_ADDR, slaveDev->busType_u.mpuSlave.address | READ_FLAG); // set I2C slave address for read
ak8963SpiWriteRegisterDelay(dev, MPU_RA_I2C_SLV0_REG, reg); // set I2C slave register
ak8963SpiWriteRegisterDelay(dev, MPU_RA_I2C_SLV0_CTRL, (len & 0x0F) | I2C_SLV0_EN); // read number of bytes
delay(4);
__disable_irq();
bool ack = spiBusReadRegisterBuffer(bus, MPU_RA_EXT_SENS_DATA_00, buf, len); // read I2C
bool ack = spiReadRegMskBufRB(dev, MPU_RA_EXT_SENS_DATA_00, buf, len); // read I2C
__enable_irq();
return ack;
}
static bool ak8963SlaveWriteRegister(const busDevice_t *slavedev, uint8_t reg, uint8_t data)
static bool ak8963SlaveWriteRegister(const extDevice_t *slaveDev, uint8_t reg, uint8_t data)
{
const busDevice_t *bus = slavedev->busdev_u.mpuSlave.master;
extDevice_t *dev = slaveDev->bus->busType_u.mpuSlave.master;
ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_ADDR, slavedev->busdev_u.mpuSlave.address); // set I2C slave address for write
ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_REG, reg); // set I2C slave register
ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_DO, data); // set I2C sLave value
ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_CTRL, (1 & 0x0F) | I2C_SLV0_EN); // write 1 byte
ak8963SpiWriteRegisterDelay(dev, MPU_RA_I2C_SLV0_ADDR, slaveDev->busType_u.mpuSlave.address); // set I2C slave address for write
ak8963SpiWriteRegisterDelay(dev, MPU_RA_I2C_SLV0_REG, reg); // set I2C slave register
ak8963SpiWriteRegisterDelay(dev, MPU_RA_I2C_SLV0_DO, data); // set I2C sLave value
ak8963SpiWriteRegisterDelay(dev, MPU_RA_I2C_SLV0_CTRL, (1 & 0x0F) | I2C_SLV0_EN); // write 1 byte
return true;
}
@ -143,19 +143,19 @@ typedef struct queuedReadState_s {
static queuedReadState_t queuedRead = { false, 0, 0};
static bool ak8963SlaveStartRead(const busDevice_t *slavedev, uint8_t reg, uint8_t len)
static bool ak8963SlaveStartRead(const extDevice_t *slaveDev, uint8_t reg, uint8_t len)
{
if (queuedRead.waiting) {
return false;
}
const busDevice_t *bus = slavedev->busdev_u.mpuSlave.master;
extDevice_t *dev = slaveDev->bus->busType_u.mpuSlave.master;
queuedRead.len = len;
ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_ADDR, slavedev->busdev_u.mpuSlave.address | READ_FLAG); // set I2C slave address for read
ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_REG, reg); // set I2C slave register
ak8963SpiWriteRegisterDelay(bus, MPU_RA_I2C_SLV0_CTRL, (len & 0x0F) | I2C_SLV0_EN); // read number of bytes
ak8963SpiWriteRegisterDelay(dev, MPU_RA_I2C_SLV0_ADDR, slaveDev->busType_u.mpuSlave.address | READ_FLAG); // set I2C slave address for read
ak8963SpiWriteRegisterDelay(dev, MPU_RA_I2C_SLV0_REG, reg); // set I2C slave register
ak8963SpiWriteRegisterDelay(dev, MPU_RA_I2C_SLV0_CTRL, (len & 0x0F) | I2C_SLV0_EN); // read number of bytes
queuedRead.readStartedAt = micros();
queuedRead.waiting = true;
@ -180,11 +180,11 @@ static uint32_t ak8963SlaveQueuedReadTimeRemaining(void)
return timeRemaining;
}
static bool ak8963SlaveCompleteRead(const busDevice_t *slavedev, uint8_t *buf)
static bool ak8963SlaveCompleteRead(const extDevice_t *slaveDev, uint8_t *buf)
{
uint32_t timeRemaining = ak8963SlaveQueuedReadTimeRemaining();
const busDevice_t *bus = slavedev->busdev_u.mpuSlave.master;
extDevice_t *dev = slaveDev->bus->busType_u.mpuSlave.master;
if (timeRemaining > 0) {
delayMicroseconds(timeRemaining);
@ -192,11 +192,10 @@ static bool ak8963SlaveCompleteRead(const busDevice_t *slavedev, uint8_t *buf)
queuedRead.waiting = false;
spiBusReadRegisterBuffer(bus, MPU_RA_EXT_SENS_DATA_00, buf, queuedRead.len); // read I2C buffer
return true;
return spiReadRegMskBufRB(dev, MPU_RA_EXT_SENS_DATA_00, buf, queuedRead.len); // read I2C buffer
}
static bool ak8963SlaveReadData(const busDevice_t *busdev, uint8_t *buf)
static bool ak8963SlaveReadData(const extDevice_t *dev, uint8_t *buf)
{
typedef enum {
CHECK_STATUS = 0,
@ -216,7 +215,7 @@ static bool ak8963SlaveReadData(const busDevice_t *busdev, uint8_t *buf)
restart:
switch (state) {
case CHECK_STATUS: {
ak8963SlaveStartRead(busdev, AK8963_MAG_REG_ST1, 1);
ak8963SlaveStartRead(dev, AK8963_MAG_REG_ST1, 1);
state = WAITING_FOR_STATUS;
return false;
}
@ -227,7 +226,7 @@ restart:
return false;
}
ack = ak8963SlaveCompleteRead(busdev, &buf[0]);
ack = ak8963SlaveCompleteRead(dev, &buf[0]);
uint8_t status = buf[0];
@ -242,7 +241,7 @@ restart:
}
// read the 6 bytes of data and the status2 register
ak8963SlaveStartRead(busdev, AK8963_MAG_REG_HXL, 7);
ak8963SlaveStartRead(dev, AK8963_MAG_REG_HXL, 7);
state = WAITING_FOR_DATA;
return false;
@ -254,7 +253,7 @@ restart:
return false;
}
ack = ak8963SlaveCompleteRead(busdev, &buf[0]);
ack = ak8963SlaveCompleteRead(dev, &buf[0]);
state = CHECK_STATUS;
}
}
@ -263,37 +262,37 @@ restart:
}
#endif
static bool ak8963ReadRegisterBuffer(const busDevice_t *busdev, uint8_t reg, uint8_t *buf, uint8_t len)
static bool ak8963ReadRegisterBuffer(const extDevice_t *dev, uint8_t reg, uint8_t *buf, uint8_t len)
{
#if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))
if (busdev->bustype == BUSTYPE_MPU_SLAVE) {
return ak8963SlaveReadRegisterBuffer(busdev, reg, buf, len);
if (dev->bus->busType == BUS_TYPE_MPU_SLAVE) {
return ak8963SlaveReadRegisterBuffer(dev, reg, buf, len);
}
#endif
return busReadRegisterBuffer(busdev, reg, buf, len);
return busReadRegisterBuffer(dev, reg, buf, len);
}
static bool ak8963WriteRegister(const busDevice_t *busdev, uint8_t reg, uint8_t data)
static bool ak8963WriteRegister(const extDevice_t *dev, uint8_t reg, uint8_t data)
{
#if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))
if (busdev->bustype == BUSTYPE_MPU_SLAVE) {
return ak8963SlaveWriteRegister(busdev, reg, data);
if (dev->bus->busType == BUS_TYPE_MPU_SLAVE) {
return ak8963SlaveWriteRegister(dev, reg, data);
}
#endif
return busWriteRegister(busdev, reg, data);
return busWriteRegister(dev, reg, data);
}
static bool ak8963DirectReadData(const busDevice_t *busdev, uint8_t *buf)
static bool ak8963DirectReadData(const extDevice_t *dev, uint8_t *buf)
{
uint8_t status;
bool ack = ak8963ReadRegisterBuffer(busdev, AK8963_MAG_REG_ST1, &status, 1);
bool ack = ak8963ReadRegisterBuffer(dev, AK8963_MAG_REG_ST1, &status, 1);
if (!ack || (status & ST1_DATA_READY) == 0) {
return false;
}
return ak8963ReadRegisterBuffer(busdev, AK8963_MAG_REG_HXL, buf, 7);
return ak8963ReadRegisterBuffer(dev, AK8963_MAG_REG_HXL, buf, 7);
}
static int16_t parseMag(uint8_t *raw, int16_t gain) {
@ -306,19 +305,19 @@ static bool ak8963Read(magDev_t *mag, int16_t *magData)
bool ack = false;
uint8_t buf[7];
const busDevice_t *busdev = &mag->busdev;
extDevice_t *dev = &mag->dev;
switch (busdev->bustype) {
switch (dev->bus->busType) {
#if defined(USE_MAG_SPI_AK8963) || defined(USE_MAG_AK8963)
case BUSTYPE_I2C:
case BUSTYPE_SPI:
ack = ak8963DirectReadData(busdev, buf);
case BUS_TYPE_I2C:
case BUS_TYPE_SPI:
ack = ak8963DirectReadData(dev, buf);
break;
#endif
#if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))
case BUSTYPE_MPU_SLAVE:
ack = ak8963SlaveReadData(busdev, buf);
case BUS_TYPE_MPU_SLAVE:
ack = ak8963SlaveReadData(dev, buf);
break;
#endif
default:
@ -330,7 +329,7 @@ static bool ak8963Read(magDev_t *mag, int16_t *magData)
return false;
}
ak8963WriteRegister(busdev, AK8963_MAG_REG_CNTL1, CNTL1_BIT_16_BIT | CNTL1_MODE_ONCE); // start reading again uint8_t status2 = buf[6];
ak8963WriteRegister(dev, AK8963_MAG_REG_CNTL1, CNTL1_BIT_16_BIT | CNTL1_MODE_ONCE); // start reading again uint8_t status2 = buf[6];
if (status2 & ST2_MAG_SENSOR_OVERFLOW) {
return false;
@ -348,59 +347,55 @@ static bool ak8963Init(magDev_t *mag)
uint8_t asa[3];
uint8_t status;
const busDevice_t *busdev = &mag->busdev;
extDevice_t *dev = &mag->dev;
busDeviceRegister(busdev);
busDeviceRegister(dev);
ak8963WriteRegister(busdev, AK8963_MAG_REG_CNTL1, CNTL1_MODE_POWER_DOWN); // power down before entering fuse mode
ak8963WriteRegister(busdev, AK8963_MAG_REG_CNTL1, CNTL1_MODE_FUSE_ROM); // Enter Fuse ROM access mode
ak8963ReadRegisterBuffer(busdev, AK8963_MAG_REG_ASAX, asa, sizeof(asa)); // Read the x-, y-, and z-axis calibration values
ak8963WriteRegister(dev, AK8963_MAG_REG_CNTL1, CNTL1_MODE_POWER_DOWN); // power down before entering fuse mode
ak8963WriteRegister(dev, AK8963_MAG_REG_CNTL1, CNTL1_MODE_FUSE_ROM); // Enter Fuse ROM access mode
ak8963ReadRegisterBuffer(dev, AK8963_MAG_REG_ASAX, asa, sizeof(asa)); // Read the x-, y-, and z-axis calibration values
mag->magGain[X] = asa[X] + 128;
mag->magGain[Y] = asa[Y] + 128;
mag->magGain[Z] = asa[Z] + 128;
ak8963WriteRegister(busdev, AK8963_MAG_REG_CNTL1, CNTL1_MODE_POWER_DOWN); // power down after reading.
ak8963WriteRegister(dev, AK8963_MAG_REG_CNTL1, CNTL1_MODE_POWER_DOWN); // power down after reading.
// Clear status registers
ak8963ReadRegisterBuffer(busdev, AK8963_MAG_REG_ST1, &status, 1);
ak8963ReadRegisterBuffer(busdev, AK8963_MAG_REG_ST2, &status, 1);
ak8963ReadRegisterBuffer(dev, AK8963_MAG_REG_ST1, &status, 1);
ak8963ReadRegisterBuffer(dev, AK8963_MAG_REG_ST2, &status, 1);
// Trigger first measurement
ak8963WriteRegister(busdev, AK8963_MAG_REG_CNTL1, CNTL1_BIT_16_BIT | CNTL1_MODE_ONCE);
ak8963WriteRegister(dev, AK8963_MAG_REG_CNTL1, CNTL1_BIT_16_BIT | CNTL1_MODE_ONCE);
return true;
}
void ak8963BusInit(busDevice_t *busdev)
void ak8963BusInit(const extDevice_t *dev)
{
switch (busdev->bustype) {
switch (dev->bus->busType) {
#ifdef USE_MAG_AK8963
case BUSTYPE_I2C:
UNUSED(busdev);
case BUS_TYPE_I2C:
UNUSED(dev);
break;
#endif
#ifdef USE_MAG_SPI_AK8963
case BUSTYPE_SPI:
IOHi(busdev->busdev_u.spi.csnPin); // Disable
IOInit(busdev->busdev_u.spi.csnPin, OWNER_COMPASS_CS, 0);
IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP);
#ifdef USE_SPI_TRANSACTION
spiBusTransactionInit(busdev, SPI_MODE3_POL_HIGH_EDGE_2ND, spiCalculateDivider(AK8963_MAX_SPI_CLK_HZ));
#else
spiBusSetDivisor(busdev, spiCalculateDivider(AK8963_MAX_SPI_CLK_HZ));
#endif
case BUS_TYPE_SPI:
IOHi(dev->busType_u.spi.csnPin); // Disable
IOInit(dev->busType_u.spi.csnPin, OWNER_COMPASS_CS, 0);
IOConfigGPIO(dev->busType_u.spi.csnPin, IOCFG_OUT_PP);
spiSetClkDivisor(dev, spiCalculateDivider(AK8963_MAX_SPI_CLK_HZ));
break;
#endif
#if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))
case BUSTYPE_MPU_SLAVE:
case BUS_TYPE_MPU_SLAVE:
rescheduleTask(TASK_COMPASS, TASK_PERIOD_HZ(40));
// initialze I2C master via SPI bus
ak8963SpiWriteRegisterDelay(busdev->busdev_u.mpuSlave.master, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN);
ak8963SpiWriteRegisterDelay(busdev->busdev_u.mpuSlave.master, MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz
ak8963SpiWriteRegisterDelay(busdev->busdev_u.mpuSlave.master, MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only
ak8963SpiWriteRegisterDelay(dev->bus->busType_u.mpuSlave.master, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN);
ak8963SpiWriteRegisterDelay(dev->bus->busType_u.mpuSlave.master, MPU_RA_I2C_MST_CTRL, 0x0D); // I2C multi-master / 400kHz
ak8963SpiWriteRegisterDelay(dev->bus->busType_u.mpuSlave.master, MPU_RA_USER_CTRL, 0x30); // I2C master mode, SPI mode only
break;
#endif
default:
@ -408,24 +403,24 @@ void ak8963BusInit(busDevice_t *busdev)
}
}
void ak8963BusDeInit(const busDevice_t *busdev)
void ak8963BusDeInit(const extDevice_t *dev)
{
switch (busdev->bustype) {
switch (dev->bus->busType) {
#ifdef USE_MAG_AK8963
case BUSTYPE_I2C:
UNUSED(busdev);
case BUS_TYPE_I2C:
UNUSED(dev);
break;
#endif
#ifdef USE_MAG_SPI_AK8963
case BUSTYPE_SPI:
spiPreinitByIO(busdev->busdev_u.spi.csnPin);
case BUS_TYPE_SPI:
spiPreinitByIO(dev->busType_u.spi.csnPin);
break;
#endif
#if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))
case BUSTYPE_MPU_SLAVE:
ak8963SpiWriteRegisterDelay(busdev->busdev_u.mpuSlave.master, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR);
case BUS_TYPE_MPU_SLAVE:
ak8963SpiWriteRegisterDelay(dev->bus->busType_u.mpuSlave.master, MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR);
break;
#endif
default:
@ -437,18 +432,18 @@ bool ak8963Detect(magDev_t *mag)
{
uint8_t sig = 0;
busDevice_t *busdev = &mag->busdev;
extDevice_t *dev = &mag->dev;
if ((busdev->bustype == BUSTYPE_I2C || busdev->bustype == BUSTYPE_MPU_SLAVE) && busdev->busdev_u.mpuSlave.address == 0) {
busdev->busdev_u.mpuSlave.address = AK8963_MAG_I2C_ADDRESS;
if ((dev->bus->busType == BUS_TYPE_I2C || dev->bus->busType == BUS_TYPE_MPU_SLAVE) && dev->busType_u.mpuSlave.address == 0) {
dev->busType_u.mpuSlave.address = AK8963_MAG_I2C_ADDRESS;
}
ak8963BusInit(busdev);
ak8963BusInit(dev);
ak8963WriteRegister(busdev, AK8963_MAG_REG_CNTL2, CNTL2_SOFT_RESET); // reset MAG
ak8963WriteRegister(dev, AK8963_MAG_REG_CNTL2, CNTL2_SOFT_RESET); // reset MAG
delay(4);
bool ack = ak8963ReadRegisterBuffer(busdev, AK8963_MAG_REG_WIA, &sig, 1); // check for AK8963
bool ack = ak8963ReadRegisterBuffer(dev, AK8963_MAG_REG_WIA, &sig, 1); // check for AK8963
if (ack && sig == AK8963_DEVICE_ID) // 0x48 / 01001000 / 'H'
{
@ -458,7 +453,7 @@ bool ak8963Detect(magDev_t *mag)
return true;
}
ak8963BusDeInit(busdev);
ak8963BusDeInit(dev);
return false;
}

36
src/main/drivers/compass/compass_ak8975.c
View File

@ -82,32 +82,32 @@ static bool ak8975Init(magDev_t *mag)
uint8_t asa[3];
uint8_t status;
const busDevice_t *busdev = &mag->busdev;
extDevice_t *dev = &mag->dev;
busDeviceRegister(busdev);
busDeviceRegister(dev);
busWriteRegister(busdev, AK8975_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down before entering fuse mode
busWriteRegister(dev, AK8975_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down before entering fuse mode
delay(20);
busWriteRegister(busdev, AK8975_MAG_REG_CNTL, CNTL_MODE_FUSE_ROM); // Enter Fuse ROM access mode
busWriteRegister(dev, AK8975_MAG_REG_CNTL, CNTL_MODE_FUSE_ROM); // Enter Fuse ROM access mode
delay(10);
busReadRegisterBuffer(busdev, AK8975_MAG_REG_ASAX, asa, sizeof(asa)); // Read the x-, y-, and z-axis asa values
busReadRegisterBuffer(dev, AK8975_MAG_REG_ASAX, asa, sizeof(asa)); // Read the x-, y-, and z-axis asa values
delay(10);
mag->magGain[X] = asa[X] + 128;
mag->magGain[Y] = asa[Y] + 128;
mag->magGain[Z] = asa[Z] + 128;
busWriteRegister(busdev, AK8975_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down after reading.
busWriteRegister(dev, AK8975_MAG_REG_CNTL, CNTL_MODE_POWER_DOWN); // power down after reading.
delay(10);
// Clear status registers
busReadRegisterBuffer(busdev, AK8975_MAG_REG_ST1, &status, 1);
busReadRegisterBuffer(busdev, AK8975_MAG_REG_ST2, &status, 1);
busReadRegisterBuffer(dev, AK8975_MAG_REG_ST1, &status, 1);
busReadRegisterBuffer(dev, AK8975_MAG_REG_ST2, &status, 1);
// Trigger first measurement
busWriteRegister(busdev, AK8975_MAG_REG_CNTL, CNTL_BIT_16_BIT | CNTL_MODE_ONCE);
busWriteRegister(dev, AK8975_MAG_REG_CNTL, CNTL_BIT_16_BIT | CNTL_MODE_ONCE);
return true;
}
@ -122,21 +122,21 @@ static bool ak8975Read(magDev_t *mag, int16_t *magData)
uint8_t status;
uint8_t buf[6];
const busDevice_t *busdev = &mag->busdev;
extDevice_t *dev = &mag->dev;
ack = busReadRegisterBuffer(busdev, AK8975_MAG_REG_ST1, &status, 1);
ack = busReadRegisterBuffer(dev, AK8975_MAG_REG_ST1, &status, 1);
if (!ack || (status & ST1_REG_DATA_READY) == 0) {
return false;
}
busReadRegisterBuffer(busdev, AK8975_MAG_REG_HXL, buf, 6); // read from AK8975_MAG_REG_HXL to AK8975_MAG_REG_HZH
busReadRegisterBuffer(dev, AK8975_MAG_REG_HXL, buf, 6); // read from AK8975_MAG_REG_HXL to AK8975_MAG_REG_HZH
ack = busReadRegisterBuffer(busdev, AK8975_MAG_REG_ST2, &status, 1);
ack = busReadRegisterBuffer(dev, AK8975_MAG_REG_ST2, &status, 1);
if (!ack) {
return false;
}
busWriteRegister(busdev, AK8975_MAG_REG_CNTL, CNTL_BIT_16_BIT | CNTL_MODE_ONCE); // start reading again uint8_t status2 = buf[6];
busWriteRegister(dev, AK8975_MAG_REG_CNTL, CNTL_BIT_16_BIT | CNTL_MODE_ONCE); // start reading again uint8_t status2 = buf[6];
if (status & ST2_REG_DATA_ERROR) {
return false;
@ -157,13 +157,13 @@ bool ak8975Detect(magDev_t *mag)
{
uint8_t sig = 0;
busDevice_t *busdev = &mag->busdev;
extDevice_t *dev = &mag->dev;
if (busdev->bustype == BUSTYPE_I2C && busdev->busdev_u.i2c.address == 0) {
busdev->busdev_u.i2c.address = AK8975_MAG_I2C_ADDRESS;
if (dev->bus->busType == BUS_TYPE_I2C && dev->busType_u.i2c.address == 0) {
dev->busType_u.i2c.address = AK8975_MAG_I2C_ADDRESS;
}
bool ack = busReadRegisterBuffer(busdev, AK8975_MAG_REG_WIA, &sig, 1);
bool ack = busReadRegisterBuffer(dev, AK8975_MAG_REG_WIA, &sig, 1);
if (!ack || sig != AK8975_DEVICE_ID) { // 0x48 / 01001000 / 'H'
return false;

41
src/main/drivers/compass/compass_hmc5883l.c
View File

@ -186,20 +186,15 @@ static void hmc5883lConfigureDataReadyInterruptHandling(magDev_t* mag)
}
#ifdef USE_MAG_SPI_HMC5883
static void hmc5883SpiInit(busDevice_t *busdev)
static void hmc5883SpiInit(const extDevice_t *dev)
{
busDeviceRegister(busdev);
busDeviceRegister(dev);
IOHi(busdev->busdev_u.spi.csnPin); // Disable
IOHi(dev->busType_u.spi.csnPin); // Disable
IOInit(busdev->busdev_u.spi.csnPin, OWNER_COMPASS_CS, 0);
IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP);
#ifdef USE_SPI_TRANSACTION
spiBusTransactionInit(busdev, SPI_MODE3_POL_HIGH_EDGE_2ND, spiCalculateDivider(HMC5883_MAX_SPI_CLK_HZ));
#else
spiBusSetDivisor(busdev, spiCalculateDivider(HMC5883_MAX_SPI_CLK_HZ));
#endif
IOInit(dev->busType_u.spi.csnPin, OWNER_COMPASS_CS, 0);
IOConfigGPIO(dev->busType_u.spi.csnPin, IOCFG_OUT_PP);
spiSetClkDivisor(dev, spiCalculateDivider(HMC5883_MAX_SPI_CLK_HZ));
}
#endif
@ -207,9 +202,9 @@ static bool hmc5883lRead(magDev_t *mag, int16_t *magData)
{
uint8_t buf[6];
busDevice_t *busdev = &mag->busdev;
extDevice_t *dev = &mag->dev;
bool ack = busReadRegisterBuffer(busdev, HMC58X3_REG_DATA, buf, 6);
bool ack = busReadRegisterBuffer(dev, HMC58X3_REG_DATA, buf, 6);
if (!ack) {
return false;
@ -225,12 +220,12 @@ static bool hmc5883lRead(magDev_t *mag, int16_t *magData)
static bool hmc5883lInit(magDev_t *mag)
{
busDevice_t *busdev = &mag->busdev;
extDevice_t *dev = &mag->dev;
// leave test mode
busWriteRegister(busdev, HMC58X3_REG_CONFA, HMC_CONFA_8_SAMLES | HMC_CONFA_DOR_15HZ | HMC_CONFA_NORMAL); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode
busWriteRegister(busdev, HMC58X3_REG_CONFB, HMC_CONFB_GAIN_1_3GA); // Configuration Register B -- 001 00000 configuration gain 1.3Ga
busWriteRegister(busdev, HMC58X3_REG_MODE, HMC_MODE_CONTINOUS); // Mode register -- 000000 00 continuous Conversion Mode
busWriteRegister(dev, HMC58X3_REG_CONFA, HMC_CONFA_8_SAMLES | HMC_CONFA_DOR_15HZ | HMC_CONFA_NORMAL); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode
busWriteRegister(dev, HMC58X3_REG_CONFB, HMC_CONFB_GAIN_1_3GA); // Configuration Register B -- 001 00000 configuration gain 1.3Ga
busWriteRegister(dev, HMC58X3_REG_MODE, HMC_MODE_CONTINOUS); // Mode register -- 000000 00 continuous Conversion Mode
delay(100);
@ -240,23 +235,23 @@ static bool hmc5883lInit(magDev_t *mag)
bool hmc5883lDetect(magDev_t* mag)
{
busDevice_t *busdev = &mag->busdev;
extDevice_t *dev = &mag->dev;
uint8_t sig = 0;
#ifdef USE_MAG_SPI_HMC5883
if (busdev->bustype == BUSTYPE_SPI) {
hmc5883SpiInit(&mag->busdev);
if (dev->bus->busType == BUS_TYPE_SPI) {
hmc5883SpiInit(dev);
}
#endif
#ifdef USE_MAG_HMC5883
if (busdev->bustype == BUSTYPE_I2C && busdev->busdev_u.i2c.address == 0) {
busdev->busdev_u.i2c.address = HMC5883_MAG_I2C_ADDRESS;
if (dev->bus->busType == BUS_TYPE_I2C && dev->busType_u.i2c.address == 0) {
dev->busType_u.i2c.address = HMC5883_MAG_I2C_ADDRESS;
}
#endif
bool ack = busReadRegisterBuffer(&mag->busdev, HMC58X3_REG_IDA, &sig, 1);
bool ack = busReadRegisterBuffer(&mag->dev, HMC58X3_REG_IDA, &sig, 1);
if (!ack || sig != HMC5883_DEVICE_ID) {
return false;

26
src/main/drivers/compass/compass_lis3mdl.c
View File

@ -105,9 +105,9 @@ static bool lis3mdlRead(magDev_t * mag, int16_t *magData)
{
uint8_t buf[6];
busDevice_t *busdev = &mag->busdev;
extDevice_t *dev = &mag->dev;
bool ack = busReadRegisterBuffer(busdev, LIS3MDL_REG_OUT_X_L, buf, 6);
bool ack = busReadRegisterBuffer(dev, LIS3MDL_REG_OUT_X_L, buf, 6);
if (!ack) {
return false;
@ -122,15 +122,15 @@ static bool lis3mdlRead(magDev_t * mag, int16_t *magData)
static bool lis3mdlInit(magDev_t *mag)
{
busDevice_t *busdev = &mag->busdev;
extDevice_t *dev = &mag->dev;
busDeviceRegister(busdev);
busDeviceRegister(dev);
busWriteRegister(busdev, LIS3MDL_REG_CTRL_REG2, LIS3MDL_FS_4GAUSS);
busWriteRegister(busdev, LIS3MDL_REG_CTRL_REG1, LIS3MDL_TEMP_EN | LIS3MDL_OM_ULTRA_HI_PROF | LIS3MDL_DO_80);
busWriteRegister(busdev, LIS3MDL_REG_CTRL_REG5, LIS3MDL_BDU);
busWriteRegister(busdev, LIS3MDL_REG_CTRL_REG4, LIS3MDL_ZOM_UHP);
busWriteRegister(busdev, LIS3MDL_REG_CTRL_REG3, 0x00);
busWriteRegister(dev, LIS3MDL_REG_CTRL_REG2, LIS3MDL_FS_4GAUSS);
busWriteRegister(dev, LIS3MDL_REG_CTRL_REG1, LIS3MDL_TEMP_EN | LIS3MDL_OM_ULTRA_HI_PROF | LIS3MDL_DO_80);
busWriteRegister(dev, LIS3MDL_REG_CTRL_REG5, LIS3MDL_BDU);
busWriteRegister(dev, LIS3MDL_REG_CTRL_REG4, LIS3MDL_ZOM_UHP);
busWriteRegister(dev, LIS3MDL_REG_CTRL_REG3, 0x00);
delay(100);
@ -139,15 +139,15 @@ static bool lis3mdlInit(magDev_t *mag)
bool lis3mdlDetect(magDev_t * mag)
{
busDevice_t *busdev = &mag->busdev;
extDevice_t *dev = &mag->dev;
uint8_t sig = 0;
if (busdev->bustype == BUSTYPE_I2C && busdev->busdev_u.i2c.address == 0) {
busdev->busdev_u.i2c.address = LIS3MDL_MAG_I2C_ADDRESS;
if (dev->bus->busType == BUS_TYPE_I2C && dev->busType_u.i2c.address == 0) {
dev->busType_u.i2c.address = LIS3MDL_MAG_I2C_ADDRESS;
}
bool ack = busReadRegisterBuffer(&mag->busdev, LIS3MDL_REG_WHO_AM_I, &sig, 1);
bool ack = busReadRegisterBuffer(&mag->dev, LIS3MDL_REG_WHO_AM_I, &sig, 1);
if (!ack || sig != LIS3MDL_DEVICE_ID) {
return false;

19
src/main/drivers/compass/compass_mpu925x_ak8963.c
View File

@ -37,7 +37,7 @@
#define AK8963_MAG_I2C_ADDRESS 0x0C
#define MPU9250_BIT_RESET 0x80
static bool mpu925xDeviceDetect(busDevice_t * dev)
static bool mpu925xDeviceDetect(const extDevice_t * dev)
{
busWriteRegister(dev, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
delay(150);
@ -52,22 +52,21 @@ static bool mpu925xDeviceDetect(busDevice_t * dev)
bool mpu925Xak8963CompassDetect(magDev_t * mag)
{
busDevice_t *busdev = &mag->busdev;
busdev->busdev_u.i2c.address = MPU925X_I2C_ADDRESS;
busDeviceRegister(busdev);
if (busdev == NULL || !mpu925xDeviceDetect(busdev)) {
extDevice_t *dev = &mag->dev;
busDeviceRegister(dev);
if (!mpu925xDeviceDetect(dev->bus->busType_u.mpuSlave.master)) {
return false;
}
// set bypass mode on mpu9250
busWriteRegister(busdev, MPU_RA_INT_PIN_CFG, 0x02);
busWriteRegister(dev->bus->busType_u.mpuSlave.master, MPU_RA_INT_PIN_CFG, 0x02);
delay(150);
// now we have ak8963 alike on the bus
busdev->busdev_u.i2c.address = AK8963_MAG_I2C_ADDRESS;
busDeviceRegister(busdev);
dev->busType_u.i2c.address = AK8963_MAG_I2C_ADDRESS;
busDeviceRegister(dev);
if(!ak8963Detect(mag)) {
// if ak8963 is not detected, reset the MPU to disable bypass mode
busdev->busdev_u.i2c.address = MPU925X_I2C_ADDRESS;
busWriteRegister(busdev, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
dev->busType_u.i2c.address = MPU925X_I2C_ADDRESS;
busWriteRegister(dev, MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
return false;
} else {
return true;

28
src/main/drivers/compass/compass_qmc5883l.c
View File

@ -74,15 +74,13 @@
static bool qmc5883lInit(magDev_t *magDev)
{
UNUSED(magDev);
bool ack = true;
busDevice_t *busdev = &magDev->busdev;
extDevice_t *dev = &magDev->dev;
busDeviceRegister(busdev);
busDeviceRegister(dev);
ack = ack && busWriteRegister(busdev, 0x0B, 0x01);
ack = ack && busWriteRegister(busdev, QMC5883L_REG_CONF1, QMC5883L_MODE_CONTINUOUS | QMC5883L_ODR_200HZ | QMC5883L_OSR_512 | QMC5883L_RNG_8G);
ack = ack && busWriteRegister(dev, 0x0B, 0x01);
ack = ack && busWriteRegister(dev, QMC5883L_REG_CONF1, QMC5883L_MODE_CONTINUOUS | QMC5883L_ODR_200HZ | QMC5883L_OSR_512 | QMC5883L_RNG_8G);
if (!ack) {
return false;
@ -101,15 +99,15 @@ static bool qmc5883lRead(magDev_t *magDev, int16_t *magData)
magData[Y] = 0;
magData[Z] = 0;
busDevice_t *busdev = &magDev->busdev;
extDevice_t *dev = &magDev->dev;
bool ack = busReadRegisterBuffer(busdev, QMC5883L_REG_STATUS, &status, 1);
bool ack = busReadRegisterBuffer(dev, QMC5883L_REG_STATUS, &status, 1);
if (!ack || (status & 0x04) == 0) {
return false;
}
ack = busReadRegisterBuffer(busdev, QMC5883L_REG_DATA_OUTPUT_X, buf, 6);
ack = busReadRegisterBuffer(dev, QMC5883L_REG_DATA_OUTPUT_X, buf, 6);
if (!ack) {
return false;
}
@ -124,22 +122,22 @@ static bool qmc5883lRead(magDev_t *magDev, int16_t *magData)
bool qmc5883lDetect(magDev_t *magDev)
{
busDevice_t *busdev = &magDev->busdev;
extDevice_t *dev = &magDev->dev;
if (busdev->bustype == BUSTYPE_I2C && busdev->busdev_u.i2c.address == 0) {
busdev->busdev_u.i2c.address = QMC5883L_MAG_I2C_ADDRESS;
if (dev->bus->busType == BUS_TYPE_I2C && dev->busType_u.i2c.address == 0) {
dev->busType_u.i2c.address = QMC5883L_MAG_I2C_ADDRESS;
}
// Must write reset first - don't care about the result
busWriteRegister(busdev, QMC5883L_REG_CONF2, QMC5883L_RST);
busWriteRegister(dev, QMC5883L_REG_CONF2, QMC5883L_RST);
delay(20);
uint8_t sig = 0;
bool ack = busReadRegisterBuffer(busdev, QMC5883L_REG_ID, &sig, 1);
bool ack = busReadRegisterBuffer(dev, QMC5883L_REG_ID, &sig, 1);
if (ack && sig == QMC5883_ID_VAL) {
// Should be in standby mode after soft reset and sensor is really present
// Reading ChipID of 0xFF alone is not sufficient to be sure the QMC is present
ack = busReadRegisterBuffer(busdev, QMC5883L_REG_CONF1, &sig, 1);
ack = busReadRegisterBuffer(dev, QMC5883L_REG_CONF1, &sig, 1);
if (ack && sig != QMC5883L_MODE_STANDBY) {
return false;
}

52
src/main/drivers/display_ug2864hsweg01.c
View File

@ -173,15 +173,15 @@ static const uint8_t multiWiiFont[][5] = { // Refer to "Times New Roman" Font Da
{ 0x7A, 0x7E, 0x7E, 0x7E, 0x7A }, // (131) - 0x00C8 Vertical Bargraph - 6 (full)
};
static bool i2c_OLED_send_cmd(busDevice_t *bus, uint8_t command)
static bool i2c_OLED_send_cmd(const extDevice_t *dev, uint8_t command)
{
return i2cWrite(bus->busdev_u.i2c.device, bus->busdev_u.i2c.address, 0x80, command);
return i2cWrite(dev->bus->busType_u.i2c.device, dev->busType_u.i2c.address, 0x80, command);
}
static bool i2c_OLED_send_cmdarray(busDevice_t *bus, const uint8_t *commands, size_t len)
static bool i2c_OLED_send_cmdarray(const extDevice_t *dev, const uint8_t *commands, size_t len)
{
for (size_t i = 0 ; i < len ; i++) {
if (!i2c_OLED_send_cmd(bus, commands[i])) {
if (!i2c_OLED_send_cmd(dev, commands[i])) {
return false;
}
}
@ -189,12 +189,12 @@ static bool i2c_OLED_send_cmdarray(busDevice_t *bus, const uint8_t *commands, si
return true;
}
static bool i2c_OLED_send_byte(busDevice_t *bus, uint8_t val)
static bool i2c_OLED_send_byte(const extDevice_t *dev, uint8_t val)
{
return i2cWrite(bus->busdev_u.i2c.device, bus->busdev_u.i2c.address, 0x40, val);
return i2cWrite(dev->bus->busType_u.i2c.device, dev->busType_u.i2c.address, 0x40, val);
}
void i2c_OLED_clear_display_quick(busDevice_t *bus)
void i2c_OLED_clear_display_quick(const extDevice_t *dev)
{
static const uint8_t i2c_OLED_cmd_clear_display_quick[] = {
0xb0, // set page address to 0
@ -203,14 +203,14 @@ void i2c_OLED_clear_display_quick(busDevice_t *bus)
0x10, // Set high col address to 0
};
i2c_OLED_send_cmdarray(bus, i2c_OLED_cmd_clear_display_quick, ARRAYLEN(i2c_OLED_cmd_clear_display_quick));
i2c_OLED_send_cmdarray(dev, i2c_OLED_cmd_clear_display_quick, ARRAYLEN(i2c_OLED_cmd_clear_display_quick));
for (uint16_t i = 0; i < 1024; i++) { // fill the display's RAM with graphic... 128*64 pixel picture
i2c_OLED_send_byte(bus, 0x00); // clear
i2c_OLED_send_byte(dev, 0x00); // clear
}
}
void i2c_OLED_clear_display(busDevice_t *bus)
void i2c_OLED_clear_display(const extDevice_t *dev)
{
static const uint8_t i2c_OLED_cmd_clear_display_pre[] = {
0xa6, // Set Normal Display
@ -219,9 +219,9 @@ void i2c_OLED_clear_display(busDevice_t *bus)
0x00, // Set Memory Addressing Mode to Horizontal addressing mode
};
i2c_OLED_send_cmdarray(bus, i2c_OLED_cmd_clear_display_pre, ARRAYLEN(i2c_OLED_cmd_clear_display_pre));
i2c_OLED_send_cmdarray(dev, i2c_OLED_cmd_clear_display_pre, ARRAYLEN(i2c_OLED_cmd_clear_display_pre));
i2c_OLED_clear_display_quick(bus);
i2c_OLED_clear_display_quick(dev);
static const uint8_t i2c_OLED_cmd_clear_display_post[] = {
0x81, // Setup CONTRAST CONTROL, following byte is the contrast Value... always a 2 byte instruction
@ -229,10 +229,10 @@ void i2c_OLED_clear_display(busDevice_t *bus)
0xaf, // display on
};
i2c_OLED_send_cmdarray(bus, i2c_OLED_cmd_clear_display_post, ARRAYLEN(i2c_OLED_cmd_clear_display_post));
i2c_OLED_send_cmdarray(dev, i2c_OLED_cmd_clear_display_post, ARRAYLEN(i2c_OLED_cmd_clear_display_post));
}
void i2c_OLED_set_xy(busDevice_t *bus, uint8_t col, uint8_t row)
void i2c_OLED_set_xy(const extDevice_t *dev, uint8_t col, uint8_t row)
{
uint8_t i2c_OLED_cmd_set_xy[] = {
0xb0 + row, //set page address
@ -240,31 +240,31 @@ void i2c_OLED_set_xy(busDevice_t *bus, uint8_t col, uint8_t row)
0x10 + (((CHARACTER_WIDTH_TOTAL * col) >> 4) & 0x0f) //set high col address
};
i2c_OLED_send_cmdarray(bus, i2c_OLED_cmd_set_xy, ARRAYLEN(i2c_OLED_cmd_set_xy));
i2c_OLED_send_cmdarray(dev, i2c_OLED_cmd_set_xy, ARRAYLEN(i2c_OLED_cmd_set_xy));
}
void i2c_OLED_set_line(busDevice_t *bus, uint8_t row)
void i2c_OLED_set_line(const extDevice_t *dev, uint8_t row)
{
i2c_OLED_set_xy(bus, 0, row);
i2c_OLED_set_xy(dev, 0, row);
}
void i2c_OLED_send_char(busDevice_t *bus, unsigned char ascii)
void i2c_OLED_send_char(const extDevice_t *dev, unsigned char ascii)
{
unsigned char i;
uint8_t buffer;
for (i = 0; i < 5; i++) {
buffer = multiWiiFont[ascii - 32][i];
buffer ^= CHAR_FORMAT; // apply
i2c_OLED_send_byte(bus, buffer);
i2c_OLED_send_byte(dev, buffer);
}
i2c_OLED_send_byte(bus, CHAR_FORMAT); // the gap
i2c_OLED_send_byte(dev, CHAR_FORMAT); // the gap
}
void i2c_OLED_send_string(busDevice_t *bus, const char *string)
void i2c_OLED_send_string(const extDevice_t *dev, const char *string)
{
// Sends a string of chars until null terminator
while (*string) {
i2c_OLED_send_char(bus, *string);
i2c_OLED_send_char(dev, *string);
string++;
}
}
@ -273,11 +273,11 @@ void i2c_OLED_send_string(busDevice_t *bus, const char *string)
* according to http://www.adafruit.com/datasheets/UG-2864HSWEG01.pdf Chapter 4.4 Page 15
*/
bool ug2864hsweg01InitI2C(busDevice_t *bus)
bool ug2864hsweg01InitI2C(const extDevice_t *dev)
{
// Set display OFF
if (!i2c_OLED_send_cmd(bus, 0xAE)) {
if (!i2c_OLED_send_cmd(dev, 0xAE)) {
return false;
}
@ -306,9 +306,9 @@ bool ug2864hsweg01InitI2C(busDevice_t *bus)
0xAF, // Set display On
};
i2c_OLED_send_cmdarray(bus, i2c_OLED_cmd_init, ARRAYLEN(i2c_OLED_cmd_init));
i2c_OLED_send_cmdarray(dev, i2c_OLED_cmd_init, ARRAYLEN(i2c_OLED_cmd_init));
i2c_OLED_clear_display(bus);
i2c_OLED_clear_display(dev);
return true;
}

14
src/main/drivers/display_ug2864hsweg01.h
View File

@ -39,11 +39,11 @@
#define VERTICAL_BARGRAPH_ZERO_CHARACTER (128 + 32)
#define VERTICAL_BARGRAPH_CHARACTER_COUNT 7
bool ug2864hsweg01InitI2C(busDevice_t *bus);
bool ug2864hsweg01InitI2C(const extDevice_t *dev);
void i2c_OLED_set_xy(busDevice_t *bus, uint8_t col, uint8_t row);
void i2c_OLED_set_line(busDevice_t *bus, uint8_t row);
void i2c_OLED_send_char(busDevice_t *bus, unsigned char ascii);
void i2c_OLED_send_string(busDevice_t *bus, const char *string);
void i2c_OLED_clear_display(busDevice_t *bus);
void i2c_OLED_clear_display_quick(busDevice_t *bus);
void i2c_OLED_set_xy(const extDevice_t *dev, uint8_t col, uint8_t row);
void i2c_OLED_set_line(const extDevice_t *dev, uint8_t row);
void i2c_OLED_send_char(const extDevice_t *dev, unsigned char ascii);
void i2c_OLED_send_string(const extDevice_t *dev, const char *string);
void i2c_OLED_clear_display(const extDevice_t *dev);
void i2c_OLED_clear_display_quick(const extDevice_t *dev);

3
src/main/drivers/dma.h
View File

@ -22,6 +22,9 @@
#include "drivers/resource.h"
#define CACHE_LINE_SIZE 32
#define CACHE_LINE_MASK (CACHE_LINE_SIZE - 1)
// dmaResource_t is a opaque data type which represents a single DMA engine,
// called and implemented differently in different families of STM32s.
// The opaque data type provides uniform handling of the engine in source code.

4
src/main/drivers/dshot_bitbang.c
View File

@ -70,8 +70,8 @@ dshotBitbangStatus_e bbStatus;
// as the buffer region is attributed as not cachable.
// If this is not desirable, we should use manual cache invalidation.
#ifdef USE_DSHOT_CACHE_MGMT
#define BB_OUTPUT_BUFFER_ATTRIBUTE __attribute__((aligned(32)))
#define BB_INPUT_BUFFER_ATTRIBUTE __attribute__((aligned(32)))
#define BB_OUTPUT_BUFFER_ATTRIBUTE DMA_RW_AXI __attribute__((aligned(32)))
#define BB_INPUT_BUFFER_ATTRIBUTE DMA_RW_AXI __attribute__((aligned(32)))
#else
#if defined(STM32F4)
#define BB_OUTPUT_BUFFER_ATTRIBUTE

57
src/main/drivers/flash.c
View File

@ -44,8 +44,8 @@
// 5 MHz max SPI init frequency
#define FLASH_MAX_SPI_INIT_CLK 5000000
static busDevice_t busInstance;
static busDevice_t *busdev;
static extDevice_t devInstance;
static extDevice_t *dev;
static flashDevice_t flashDevice;
static flashPartitionTable_t flashPartitionTable;
@ -135,44 +135,34 @@ void flashPreInit(const flashConfig_t *flashConfig)
static bool flashSpiInit(const flashConfig_t *flashConfig)
{
// Read chip identification and send it to device detect
busdev = &busInstance;
dev = &devInstance;
if (flashConfig->csTag) {
busdev->busdev_u.spi.csnPin = IOGetByTag(flashConfig->csTag);
dev->busType_u.spi.csnPin = IOGetByTag(flashConfig->csTag);
} else {
return false;
}
if (!IOIsFreeOrPreinit(busdev->busdev_u.spi.csnPin)) {
if (!IOIsFreeOrPreinit(dev->busType_u.spi.csnPin)) {
return false;
}
busdev->bustype = BUSTYPE_SPI;
SPI_TypeDef *instance = spiInstanceByDevice(SPI_CFG_TO_DEV(flashConfig->spiDevice));
if (!instance) {
if (!spiSetBusInstance(dev, flashConfig->spiDevice, OWNER_FLASH_CS)) {
return false;
}
spiBusSetInstance(busdev, instance);
// Set the callback argument when calling back to this driver for DMA completion
dev->callbackArg = (uint32_t)&flashDevice;
IOInit(busdev->busdev_u.spi.csnPin, OWNER_FLASH_CS, 0);
IOConfigGPIO(busdev->busdev_u.spi.csnPin, SPI_IO_CS_CFG);
IOHi(busdev->busdev_u.spi.csnPin);
IOInit(dev->busType_u.spi.csnPin, OWNER_FLASH_CS, 0);
IOConfigGPIO(dev->busType_u.spi.csnPin, SPI_IO_CS_CFG);
IOHi(dev->busType_u.spi.csnPin);
#ifdef USE_SPI_TRANSACTION
spiBusTransactionInit(busdev, SPI_MODE3_POL_HIGH_EDGE_2ND, spiCalculateDivider(FLASH_MAX_SPI_INIT_CLK));
#else
#ifndef FLASH_SPI_SHARED
spiSetDivisor(busdev->busdev_u.spi.instance, spiCalculateDivider(FLASH_MAX_SPI_INIT_CLK));
#endif
#endif
//Maximum speed for standard READ command is 20mHz, other commands tolerate 25mHz
spiSetClkDivisor(dev, spiCalculateDivider(FLASH_MAX_SPI_INIT_CLK));
flashDevice.io.mode = FLASHIO_SPI;
flashDevice.io.handle.busdev = busdev;
const uint8_t out[] = { FLASH_INSTRUCTION_RDID, 0, 0, 0, 0 };
flashDevice.io.handle.dev = dev;
delay(50); // short delay required after initialisation of SPI device instance.
@ -180,18 +170,12 @@ static bool flashSpiInit(const flashConfig_t *flashConfig)
* Some newer chips require one dummy byte to be read; we can read
* 4 bytes for these chips while retaining backward compatibility.
*/
uint8_t readIdResponse[5];
readIdResponse[1] = readIdResponse[2] = 0;
uint8_t readIdResponse[4] = { 0 };
// Clearing the CS bit terminates the command early so we don't have to read the chip UID:
#ifdef USE_SPI_TRANSACTION
spiBusTransactionTransfer(busdev, out, readIdResponse, sizeof(out));
#else
spiBusTransfer(busdev, out, readIdResponse, sizeof(out));
#endif
spiReadRegBuf(dev, FLASH_INSTRUCTION_RDID, readIdResponse, sizeof (readIdResponse));
// Manufacturer, memory type, and capacity
uint32_t chipID = (readIdResponse[1] << 16) | (readIdResponse[2] << 8) | (readIdResponse[3]);
uint32_t chipID = (readIdResponse[0] << 16) | (readIdResponse[1] << 8) | (readIdResponse[2]);
#ifdef USE_FLASH_M25P16
if (m25p16_detect(&flashDevice, chipID)) {
@ -258,36 +242,43 @@ bool flashWaitForReady(void)
void flashEraseSector(uint32_t address)
{
flashDevice.callback = NULL;
flashDevice.vTable->eraseSector(&flashDevice, address);
}
void flashEraseCompletely(void)
{
flashDevice.callback = NULL;
flashDevice.vTable->eraseCompletely(&flashDevice);
}
void flashPageProgramBegin(uint32_t address)
{
flashDevice.callback = NULL;
flashDevice.vTable->pageProgramBegin(&flashDevice, address);
}
void flashPageProgramContinue(const uint8_t *data, int length)
{
flashDevice.callback = NULL;
flashDevice.vTable->pageProgramContinue(&flashDevice, data, length);
}
void flashPageProgramFinish(void)
{
flashDevice.callback = NULL;
flashDevice.vTable->pageProgramFinish(&flashDevice);
}
void flashPageProgram(uint32_t address, const uint8_t *data, int length)
{
flashDevice.callback = NULL;
flashDevice.vTable->pageProgram(&flashDevice, address, data, length);
}
int flashReadBytes(uint32_t address, uint8_t *buffer, int length)
{
flashDevice.callback = NULL;
return flashDevice.vTable->readBytes(&flashDevice, address, buffer, length);
}

4
src/main/drivers/flash_impl.h
View File

@ -36,7 +36,7 @@ typedef enum {
typedef struct flashDeviceIO_s {
union {
busDevice_t *busdev; // Device interface dependent handle (spi/i2c)
extDevice_t *dev; // Device interface dependent handle (spi/i2c)
#ifdef USE_QUADSPI
QUADSPI_TypeDef *quadSpi;
#endif
@ -55,6 +55,8 @@ typedef struct flashDevice_s {
bool couldBeBusy;
uint32_t timeoutAt;
flashDeviceIO_t io;
void (*callback)(uint32_t arg);
uint32_t callbackArg;
} flashDevice_t;
typedef struct flashVTable_s {

277
src/main/drivers/flash_m25p16.c
View File

@ -113,106 +113,46 @@ struct {
{ 0x000000, 0, 0, 0, 0 }
};
// IMPORTANT: Timeout values are currently required to be set to the highest value required by any of the supported flash chips by this driver.
// The timeout we expect between being able to issue page program instructions
#define DEFAULT_TIMEOUT_MILLIS 6
#define SECTOR_ERASE_TIMEOUT_MILLIS 5000
// etracer65 notes: For bulk erase The 25Q16 takes about 3 seconds and the 25Q128 takes about 49
#define BULK_ERASE_TIMEOUT_MILLIS 50000
#define M25P16_PAGESIZE 256
STATIC_ASSERT(M25P16_PAGESIZE < FLASH_MAX_PAGE_SIZE, M25P16_PAGESIZE_too_small);
const flashVTable_t m25p16_vTable;
#ifndef USE_SPI_TRANSACTION
static void m25p16_disable(busDevice_t *bus)
static uint8_t m25p16_readStatus(flashDevice_t *fdevice)
{
IOHi(bus->busdev_u.spi.csnPin);
__NOP();
STATIC_DMA_DATA_AUTO uint8_t readStatus[2] = { M25P16_INSTRUCTION_READ_STATUS_REG, 0 };
STATIC_DMA_DATA_AUTO uint8_t readyStatus[2];
spiReadWriteBuf(fdevice->io.handle.dev, readStatus, readyStatus, sizeof (readStatus));
return readyStatus[1];
}
static void m25p16_enable(busDevice_t *bus)
{
__NOP();
IOLo(bus->busdev_u.spi.csnPin);
}
#endif
static void m25p16_transfer(busDevice_t *bus, const uint8_t *txData, uint8_t *rxData, int len)
{
#ifdef USE_SPI_TRANSACTION
spiBusTransactionTransfer(bus, txData, rxData, len);
#else
m25p16_enable(bus);
spiTransfer(bus->busdev_u.spi.instance, txData, rxData, len);
m25p16_disable(bus);
#endif
}
/**
* Send the given command byte to the device.
*/
static void m25p16_performOneByteCommand(busDevice_t *bus, uint8_t command)
{
#ifdef USE_SPI_TRANSACTION
m25p16_transfer(bus, &command, NULL, 1);
#else
m25p16_enable(bus);
spiTransferByte(bus->busdev_u.spi.instance, command);
m25p16_disable(bus);
#endif
}
/**
* The flash requires this write enable command to be sent before commands that would cause
* a write like program and erase.
*/
static void m25p16_writeEnable(flashDevice_t *fdevice)
{
m25p16_performOneByteCommand(fdevice->io.handle.busdev, M25P16_INSTRUCTION_WRITE_ENABLE);
// Assume that we're about to do some writing, so the device is just about to become busy
fdevice->couldBeBusy = true;
}
static uint8_t m25p16_readStatus(busDevice_t *bus)
{
const uint8_t command[2] = { M25P16_INSTRUCTION_READ_STATUS_REG, 0 };
uint8_t in[2];
m25p16_transfer(bus, command, in, sizeof(command));
return in[1];
}
static bool m25p16_isReady(flashDevice_t *fdevice)
{
// If we're waiting on DMA completion, then SPI is busy
if (fdevice->io.handle.dev->bus->useDMA && spiIsBusy(fdevice->io.handle.dev)) {
return false;
}
// If couldBeBusy is false, don't bother to poll the flash chip for its status
fdevice->couldBeBusy = fdevice->couldBeBusy && ((m25p16_readStatus(fdevice->io.handle.busdev) & M25P16_STATUS_FLAG_WRITE_IN_PROGRESS) != 0);
if (!fdevice->couldBeBusy) {
return true;
}
// Poll the FLASH device to see if it's busy
fdevice->couldBeBusy = ((m25p16_readStatus(fdevice) & M25P16_STATUS_FLAG_WRITE_IN_PROGRESS) != 0);
return !fdevice->couldBeBusy;
}
static void m25p16_setTimeout(flashDevice_t *fdevice, uint32_t timeoutMillis)
{
uint32_t now = millis();
fdevice->timeoutAt = now + timeoutMillis;
}
static bool m25p16_waitForReady(flashDevice_t *fdevice)
{
while (!m25p16_isReady(fdevice)) {
uint32_t now = millis();
if (cmp32(now, fdevice->timeoutAt) >= 0) {
return false;
}
}
while (!m25p16_isReady(fdevice));
fdevice->timeoutAt = 0;
return true;
}
@ -252,13 +192,15 @@ bool m25p16_detect(flashDevice_t *fdevice, uint32_t chipID)
geometry->totalSize = geometry->sectorSize * geometry->sectors;
// Adjust the SPI bus clock frequency
#ifndef FLASH_SPI_SHARED
spiSetDivisor(fdevice->io.handle.busdev->busdev_u.spi.instance, spiCalculateDivider(maxReadClkSPIHz));
#endif
spiSetClkDivisor(fdevice->io.handle.dev, spiCalculateDivider(maxReadClkSPIHz));
if (fdevice->geometry.totalSize > 16 * 1024 * 1024) {
if (geometry->totalSize > 16 * 1024 * 1024) {
fdevice->isLargeFlash = true;
m25p16_performOneByteCommand(fdevice->io.handle.busdev, W25Q256_INSTRUCTION_ENTER_4BYTE_ADDRESS_MODE);
// This routine blocks so no need to use static data
uint8_t modeSet[] = { W25Q256_INSTRUCTION_ENTER_4BYTE_ADDRESS_MODE };
spiReadWriteBuf(fdevice->io.handle.dev, modeSet, NULL, sizeof (modeSet));
}
fdevice->couldBeBusy = true; // Just for luck we'll assume the chip could be busy even though it isn't specced to be
@ -276,70 +218,130 @@ static void m25p16_setCommandAddress(uint8_t *buf, uint32_t address, bool useLon
*buf = address & 0xff;
}
// Called in ISR context
// A write enable has just been issued
busStatus_e m25p16_callbackWriteEnable(uint32_t arg)
{
flashDevice_t *fdevice = (flashDevice_t *)arg;
// As a write has just occurred, the device could be busy
fdevice->couldBeBusy = true;
return BUS_READY;
}
// Called in ISR context
// Check if the status was busy and if so repeat the poll
busStatus_e m25p16_callbackReady(uint32_t arg)
{
flashDevice_t *fdevice = (flashDevice_t *)arg;
extDevice_t *dev = fdevice->io.handle.dev;
uint8_t readyPoll = dev->bus->curSegment->rxData[1];
if (readyPoll & M25P16_STATUS_FLAG_WRITE_IN_PROGRESS) {
return BUS_BUSY;
}
// Bus is now known not to be busy
fdevice->couldBeBusy = false;
return BUS_READY;
}
/**
* Erase a sector full of bytes to all 1's at the given byte offset in the flash chip.
*/
static void m25p16_eraseSector(flashDevice_t *fdevice, uint32_t address)
{
uint8_t out[5] = { M25P16_INSTRUCTION_SECTOR_ERASE };
STATIC_DMA_DATA_AUTO uint8_t sectorErase[5] = { M25P16_INSTRUCTION_SECTOR_ERASE };
STATIC_DMA_DATA_AUTO uint8_t readStatus[2] = { M25P16_INSTRUCTION_READ_STATUS_REG, 0 };
STATIC_DMA_DATA_AUTO uint8_t readyStatus[2];
STATIC_DMA_DATA_AUTO uint8_t writeEnable[] = { M25P16_INSTRUCTION_WRITE_ENABLE };
busSegment_t segments[] = {
{readStatus, readyStatus, sizeof (readStatus), true, m25p16_callbackReady},
{writeEnable, NULL, sizeof (writeEnable), true, m25p16_callbackWriteEnable},
{sectorErase, NULL, fdevice->isLargeFlash ? 5 : 4, true, NULL},
{NULL, NULL, 0, true, NULL},
};
m25p16_setCommandAddress(&out[1], address, fdevice->isLargeFlash);
// Ensure any prior DMA has completed before continuing
spiWaitClaim(fdevice->io.handle.dev);
m25p16_waitForReady(fdevice);
m25p16_setCommandAddress(&sectorErase[1], address, fdevice->isLargeFlash);
m25p16_writeEnable(fdevice);
spiSequence(fdevice->io.handle.dev, &segments[0]);
m25p16_transfer(fdevice->io.handle.busdev, out, NULL, fdevice->isLargeFlash ? 5 : 4);
m25p16_setTimeout(fdevice, SECTOR_ERASE_TIMEOUT_MILLIS);
// Block pending completion of SPI access, but the erase will be ongoing
spiWait(fdevice->io.handle.dev);
}
static void m25p16_eraseCompletely(flashDevice_t *fdevice)
{
m25p16_waitForReady(fdevice);
STATIC_DMA_DATA_AUTO uint8_t readStatus[2] = { M25P16_INSTRUCTION_READ_STATUS_REG, 0 };
STATIC_DMA_DATA_AUTO uint8_t readyStatus[2];
STATIC_DMA_DATA_AUTO uint8_t writeEnable[] = { M25P16_INSTRUCTION_WRITE_ENABLE };
STATIC_DMA_DATA_AUTO uint8_t bulkErase[] = { M25P16_INSTRUCTION_BULK_ERASE };
busSegment_t segments[] = {
{readStatus, readyStatus, sizeof (readStatus), true, m25p16_callbackReady},
{writeEnable, NULL, sizeof (writeEnable), true, m25p16_callbackWriteEnable},
{bulkErase, NULL, sizeof (bulkErase), true, NULL},
{NULL, NULL, 0, true, NULL},
};
m25p16_writeEnable(fdevice);
// Ensure any prior DMA has completed before continuing
spiWaitClaim(fdevice->io.handle.dev);
m25p16_performOneByteCommand(fdevice->io.handle.busdev, M25P16_INSTRUCTION_BULK_ERASE);
spiSequence(fdevice->io.handle.dev, &segments[0]);
m25p16_setTimeout(fdevice, BULK_ERASE_TIMEOUT_MILLIS);
// Block pending completion of SPI access, but the erase will be ongoing
spiWait(fdevice->io.handle.dev);
}
static void m25p16_pageProgramBegin(flashDevice_t *fdevice, uint32_t address)
{
UNUSED(fdevice);
fdevice->currentWriteAddress = address;
}
static void m25p16_pageProgramContinue(flashDevice_t *fdevice, const uint8_t *data, int length)
{
uint8_t command[5] = { M25P16_INSTRUCTION_PAGE_PROGRAM };
// The segment list cannot be in automatic storage as this routine is non-blocking
STATIC_DMA_DATA_AUTO uint8_t readStatus[2] = { M25P16_INSTRUCTION_READ_STATUS_REG, 0 };
STATIC_DMA_DATA_AUTO uint8_t readyStatus[2];
STATIC_DMA_DATA_AUTO uint8_t writeEnable[] = { M25P16_INSTRUCTION_WRITE_ENABLE };
STATIC_DMA_DATA_AUTO uint8_t pageProgram[5] = { M25P16_INSTRUCTION_PAGE_PROGRAM };
m25p16_setCommandAddress(&command[1], fdevice->currentWriteAddress, fdevice->isLargeFlash);
static busSegment_t segments[] = {
{readStatus, readyStatus, sizeof (readStatus), true, m25p16_callbackReady},
{writeEnable, NULL, sizeof (writeEnable), true, m25p16_callbackWriteEnable},
{pageProgram, NULL, 0, false, NULL},
{NULL, NULL, 0, true, NULL},
{NULL, NULL, 0, true, NULL},
};
m25p16_waitForReady(fdevice);
// Ensure any prior DMA has completed before continuing
spiWaitClaim(fdevice->io.handle.dev);
m25p16_writeEnable(fdevice);
// Patch the pageProgram segment
segments[2].len = fdevice->isLargeFlash ? 5 : 4;
m25p16_setCommandAddress(&pageProgram[1], fdevice->currentWriteAddress, fdevice->isLargeFlash);
#ifdef USE_SPI_TRANSACTION
spiBusTransactionBegin(fdevice->io.handle.busdev);
#else
m25p16_enable(fdevice->io.handle.busdev);
#endif
// Patch the data segment
segments[3].txData = (uint8_t *)data;
segments[3].len = length;
spiTransfer(fdevice->io.handle.busdev->busdev_u.spi.instance, command, NULL, fdevice->isLargeFlash ? 5 : 4);
spiTransfer(fdevice->io.handle.busdev->busdev_u.spi.instance, data, NULL, length);
spiSequence(fdevice->io.handle.dev, fdevice->couldBeBusy ? &segments[0] : &segments[1]);
#ifdef USE_SPI_TRANSACTION
spiBusTransactionEnd(fdevice->io.handle.busdev);
#else
m25p16_disable(fdevice->io.handle.busdev);
#endif
if (fdevice->callback == NULL) {
// No callback was provided so block
spiWait(fdevice->io.handle.dev);
}
fdevice->currentWriteAddress += length;
m25p16_setTimeout(fdevice, DEFAULT_TIMEOUT_MILLIS);
}
static void m25p16_pageProgramFinish(flashDevice_t *fdevice)
@ -379,38 +381,31 @@ static void m25p16_pageProgram(flashDevice_t *fdevice, uint32_t address, const u
*/
static int m25p16_readBytes(flashDevice_t *fdevice, uint32_t address, uint8_t *buffer, int length)
{
uint8_t command[5] = { M25P16_INSTRUCTION_READ_BYTES };
STATIC_DMA_DATA_AUTO uint8_t readStatus[2] = { M25P16_INSTRUCTION_READ_STATUS_REG, 0 };
STATIC_DMA_DATA_AUTO uint8_t readyStatus[2];
STATIC_DMA_DATA_AUTO uint8_t readBytes[5] = { M25P16_INSTRUCTION_READ_BYTES };
m25p16_setCommandAddress(&command[1], address, fdevice->isLargeFlash);
// Ensure any prior DMA has completed before continuing
spiWaitClaim(fdevice->io.handle.dev);
if (!m25p16_waitForReady(fdevice)) {
return 0;
}
busSegment_t segments[] = {
{readStatus, readyStatus, sizeof (readStatus), true, m25p16_callbackReady},
{readBytes, NULL, fdevice->isLargeFlash ? 5 : 4, false, NULL},
{NULL, buffer, length, true, NULL},
{NULL, NULL, 0, true, NULL},
};
#ifndef FLASH_SPI_SHARED
spiSetDivisor(fdevice->io.handle.busdev->busdev_u.spi.instance, spiCalculateDivider(maxReadClkSPIHz));
#endif
// Patch the readBytes command
m25p16_setCommandAddress(&readBytes[1], address, fdevice->isLargeFlash);
#ifdef USE_SPI_TRANSACTION
spiBusTransactionBegin(fdevice->io.handle.busdev);
#else
m25p16_enable(fdevice->io.handle.busdev);
#endif
spiSetClkDivisor(fdevice->io.handle.dev, spiCalculateDivider(maxReadClkSPIHz));
spiTransfer(fdevice->io.handle.busdev->busdev_u.spi.instance, command, NULL, fdevice->isLargeFlash ? 5 : 4);
spiTransfer(fdevice->io.handle.busdev->busdev_u.spi.instance, NULL, buffer, length);
spiSequence(fdevice->io.handle.dev, fdevice->couldBeBusy ? &segments[0] : &segments[1]);
#ifdef USE_SPI_TRANSACTION
spiBusTransactionEnd(fdevice->io.handle.busdev);
#else
m25p16_disable(fdevice->io.handle.busdev);
#endif
// Block until code is re-factored to exploit non-blocking
spiWait(fdevice->io.handle.dev);
#ifndef FLASH_SPI_SHARED
spiSetDivisor(fdevice->io.handle.busdev->busdev_u.spi.instance, spiCalculateDivider(maxClkSPIHz));
#endif
m25p16_setTimeout(fdevice, DEFAULT_TIMEOUT_MILLIS);
spiSetClkDivisor(fdevice->io.handle.dev, spiCalculateDivider(maxClkSPIHz));
return length;
}

43
src/main/drivers/flash_w25m.c
View File

@ -62,7 +62,7 @@ static flashDevice_t dieDevice[MAX_DIE_COUNT];
static int dieCount;
static uint32_t dieSize;
static void w25m_dieSelect(busDevice_t *busdev, int die)
static void w25m_dieSelect(const extDevice_t *dev, int die)
{
static int activeDie = -1;
@ -72,11 +72,18 @@ static void w25m_dieSelect(busDevice_t *busdev, int die)
uint8_t command[2] = { W25M_INSTRUCTION_SOFTWARE_DIE_SELECT, die };
#ifdef SPI_BUS_TRANSACTION
spiBusTransactionTransfer(busdev, command, NULL, 2);
#else
spiBusTransfer(busdev, command, NULL, 2);
#endif
busSegment_t segments[] = {
{command, NULL, sizeof (command), true, NULL},
{NULL, NULL, 0, true, NULL},
};
// Ensure any prior DMA has completed before continuing
spiWait(dev);
spiSequence(dev, &segments[0]);
// Block pending completion of SPI access, but the erase will be ongoing
spiWait(dev);
activeDie = die;
}
@ -85,7 +92,7 @@ static bool w25m_isReady(flashDevice_t *fdevice)
{
for (int die = 0 ; die < dieCount ; die++) {
if (dieDevice[die].couldBeBusy) {
w25m_dieSelect(fdevice->io.handle.busdev, die);
w25m_dieSelect(fdevice->io.handle.dev, die);
if (!dieDevice[die].vTable->isReady(&dieDevice[die])) {
return false;
}
@ -98,7 +105,7 @@ static bool w25m_isReady(flashDevice_t *fdevice)
static bool w25m_waitForReady(flashDevice_t *fdevice)
{
for (int die = 0 ; die < dieCount ; die++) {
w25m_dieSelect(fdevice->io.handle.busdev, die);
w25m_dieSelect(fdevice->io.handle.dev, die);
if (!dieDevice[die].vTable->waitForReady(&dieDevice[die])) {
return false;
}
@ -117,8 +124,8 @@ bool w25m_detect(flashDevice_t *fdevice, uint32_t chipID)
dieCount = 2;
for (int die = 0 ; die < dieCount ; die++) {
w25m_dieSelect(fdevice->io.handle.busdev, die);
dieDevice[die].io.handle.busdev = fdevice->io.handle.busdev;
w25m_dieSelect(fdevice->io.handle.dev, die);
dieDevice[die].io.handle.dev = fdevice->io.handle.dev;
dieDevice[die].io.mode = fdevice->io.mode;
m25p16_detect(&dieDevice[die], JEDEC_ID_WINBOND_W25Q256);
}
@ -133,8 +140,8 @@ bool w25m_detect(flashDevice_t *fdevice, uint32_t chipID)
dieCount = 2;
for (int die = 0 ; die < dieCount ; die++) {
w25m_dieSelect(fdevice->io.handle.busdev, die);
dieDevice[die].io.handle.busdev = fdevice->io.handle.busdev;
w25m_dieSelect(fdevice->io.handle.dev, die);
dieDevice[die].io.handle.dev = fdevice->io.handle.dev;
dieDevice[die].io.mode = fdevice->io.mode;
w25n01g_detect(&dieDevice[die], JEDEC_ID_WINBOND_W25N01GV);
}
@ -167,7 +174,7 @@ void w25m_eraseSector(flashDevice_t *fdevice, uint32_t address)
{
int dieNumber = address / dieSize;
w25m_dieSelect(fdevice->io.handle.busdev, dieNumber);
w25m_dieSelect(fdevice->io.handle.dev, dieNumber);
dieDevice[dieNumber].vTable->eraseSector(&dieDevice[dieNumber], address % dieSize);
}
@ -175,7 +182,7 @@ void w25m_eraseSector(flashDevice_t *fdevice, uint32_t address)
void w25m_eraseCompletely(flashDevice_t *fdevice)
{
for (int dieNumber = 0 ; dieNumber < dieCount ; dieNumber++) {
w25m_dieSelect(fdevice->io.handle.busdev, dieNumber);
w25m_dieSelect(fdevice->io.handle.dev, dieNumber);
dieDevice[dieNumber].vTable->eraseCompletely(&dieDevice[dieNumber]);
}
}
@ -185,12 +192,10 @@ static int currentWriteDie;
void w25m_pageProgramBegin(flashDevice_t *fdevice, uint32_t address)
{
UNUSED(fdevice);
currentWriteDie = address / dieSize;
w25m_dieSelect(fdevice->io.handle.busdev, currentWriteDie);
w25m_dieSelect(fdevice->io.handle.dev, currentWriteDie);
currentWriteAddress = address % dieSize;
dieDevice[currentWriteDie].vTable->pageProgramBegin(&dieDevice[currentWriteDie], currentWriteAddress);
dieDevice[currentWriteDie].vTable->pageProgramBegin(&dieDevice[currentWriteDie], address);
}
void w25m_pageProgramContinue(flashDevice_t *fdevice, const uint8_t *data, int length)
@ -230,7 +235,7 @@ int w25m_readBytes(flashDevice_t *fdevice, uint32_t address, uint8_t *buffer, in
uint32_t dieAddress = address % dieSize;
tlen = MIN(dieAddress + rlen, dieSize) - dieAddress;
w25m_dieSelect(fdevice->io.handle.busdev, dieNumber);
w25m_dieSelect(fdevice->io.handle.dev, dieNumber);
rbytes = dieDevice[dieNumber].vTable->readBytes(&dieDevice[dieNumber], dieAddress, buffer, tlen);

200
src/main/drivers/flash_w25n01g.c
View File

@ -133,11 +133,6 @@ typedef struct bblut_s {
uint16_t lba;
} bblut_t;
// These will be gone
#define DISABLE(busdev) IOHi((busdev)->busdev_u.spi.csnPin); __NOP()
#define ENABLE(busdev) __NOP(); IOLo((busdev)->busdev_u.spi.csnPin)
static bool w25n01g_waitForReady(flashDevice_t *fdevice);
static void w25n01g_setTimeout(flashDevice_t *fdevice, uint32_t timeoutMillis)
@ -152,12 +147,20 @@ static void w25n01g_setTimeout(flashDevice_t *fdevice, uint32_t timeoutMillis)
static void w25n01g_performOneByteCommand(flashDeviceIO_t *io, uint8_t command)
{
if (io->mode == FLASHIO_SPI) {
busDevice_t *busdev = io->handle.busdev;
extDevice_t *dev = io->handle.dev;
ENABLE(busdev);
spiTransferByte(busdev->busdev_u.spi.instance, command);
DISABLE(busdev);
busSegment_t segments[] = {
{&command, NULL, sizeof (command), true, NULL},
{NULL, NULL, 0, true, NULL},
};
// Ensure any prior DMA has completed before continuing
spiWait(dev);
spiSequence(dev, &segments[0]);
// Block pending completion of SPI access
spiWait(dev);
}
#ifdef USE_QUADSPI
else if (io->mode == FLASHIO_QUADSPI) {
@ -170,13 +173,22 @@ static void w25n01g_performOneByteCommand(flashDeviceIO_t *io, uint8_t command)
static void w25n01g_performCommandWithPageAddress(flashDeviceIO_t *io, uint8_t command, uint32_t pageAddress)
{
if (io->mode == FLASHIO_SPI) {
busDevice_t *busdev = io->handle.busdev;
extDevice_t *dev = io->handle.dev;
const uint8_t cmd[] = { command, 0, (pageAddress >> 8) & 0xff, (pageAddress >> 0) & 0xff};
uint8_t cmd[] = { command, 0, (pageAddress >> 8) & 0xff, (pageAddress >> 0) & 0xff};
ENABLE(busdev);
spiTransfer(busdev->busdev_u.spi.instance, cmd, NULL, sizeof(cmd));
DISABLE(busdev);
busSegment_t segments[] = {
{cmd, NULL, sizeof (cmd), true, NULL},
{NULL, NULL, 0, true, NULL},
};
// Ensure any prior DMA has completed before continuing
spiWait(dev);
spiSequence(dev, &segments[0]);
// Block pending completion of SPI access
spiWait(dev);
}
#ifdef USE_QUADSPI
else if (io->mode == FLASHIO_QUADSPI) {
@ -190,14 +202,23 @@ static void w25n01g_performCommandWithPageAddress(flashDeviceIO_t *io, uint8_t c
static uint8_t w25n01g_readRegister(flashDeviceIO_t *io, uint8_t reg)
{
if (io->mode == FLASHIO_SPI) {
busDevice_t *busdev = io->handle.busdev;
extDevice_t *dev = io->handle.dev;
ENABLE(busdev);
const uint8_t cmd[3] = { W25N01G_INSTRUCTION_READ_STATUS_REG, reg, 0 };
uint8_t cmd[3] = { W25N01G_INSTRUCTION_READ_STATUS_REG, reg, 0 };
uint8_t in[3];
spiTransfer(busdev->busdev_u.spi.instance, cmd, in, sizeof(cmd));
DISABLE(busdev);
busSegment_t segments[] = {
{cmd, in, sizeof (cmd), true, NULL},
{NULL, NULL, 0, true, NULL},
};
// Ensure any prior DMA has completed before continuing
spiWait(dev);
spiSequence(dev, &segments[0]);
// Block pending completion of SPI access
spiWait(dev);
return in[2];
}
@ -218,12 +239,21 @@ static uint8_t w25n01g_readRegister(flashDeviceIO_t *io, uint8_t reg)
static void w25n01g_writeRegister(flashDeviceIO_t *io, uint8_t reg, uint8_t data)
{
if (io->mode == FLASHIO_SPI) {
busDevice_t *busdev = io->handle.busdev;
const uint8_t cmd[3] = { W25N01G_INSTRUCTION_WRITE_STATUS_REG, reg, data };
extDevice_t *dev = io->handle.dev;
uint8_t cmd[3] = { W25N01G_INSTRUCTION_WRITE_STATUS_REG, reg, data };
ENABLE(busdev);
spiTransfer(busdev->busdev_u.spi.instance, cmd, NULL, sizeof(cmd));
DISABLE(busdev);
busSegment_t segments[] = {
{cmd, NULL, sizeof (cmd), true, NULL},
{NULL, NULL, 0, true, NULL},
};
// Ensure any prior DMA has completed before continuing
spiWait(dev);
spiSequence(dev, &segments[0]);
// Block pending completion of SPI access
spiWait(dev);
}
#ifdef USE_QUADSPI
else if (io->mode == FLASHIO_QUADSPI) {
@ -246,7 +276,7 @@ static void w25n01g_deviceReset(flashDevice_t *fdevice)
// Protection for upper 1/32 (BP[3:0] = 0101, TB=0), WP-E on; to protect bad block replacement area
// DON'T DO THIS. This will prevent writes through the bblut as well.
// w25n01g_writeRegister(busdev, W25N01G_PROT_REG, W25N01G_PROT_PB0_ENABLE|W25N01G_PROT_PB2_ENABLE|W25N01G_PROT_WP_E_ENABLE);
// w25n01g_writeRegister(dev, W25N01G_PROT_REG, W25N01G_PROT_PB0_ENABLE|W25N01G_PROT_PB2_ENABLE|W25N01G_PROT_WP_E_ENABLE);
// No protection, WP-E off, WP-E prevents use of IO2
w25n01g_writeRegister(io, W25N01G_PROT_REG, W25N01G_PROT_CLEAR);
@ -380,13 +410,22 @@ static void w25n01g_programDataLoad(flashDevice_t *fdevice, uint16_t columnAddre
w25n01g_waitForReady(fdevice);
if (fdevice->io.mode == FLASHIO_SPI) {
busDevice_t *busdev = fdevice->io.handle.busdev;
const uint8_t cmd[] = { W25N01G_INSTRUCTION_PROGRAM_DATA_LOAD, columnAddress >> 8, columnAddress & 0xff };
extDevice_t *dev = fdevice->io.handle.dev;
uint8_t cmd[] = { W25N01G_INSTRUCTION_PROGRAM_DATA_LOAD, columnAddress >> 8, columnAddress & 0xff };
ENABLE(busdev);
spiTransfer(busdev->busdev_u.spi.instance, cmd, NULL, sizeof(cmd));
spiTransfer(busdev->busdev_u.spi.instance, data, NULL, length);
DISABLE(busdev);
busSegment_t segments[] = {
{cmd, NULL, sizeof (cmd), true, NULL},
{(uint8_t *)data, NULL, length, true, NULL},
{NULL, NULL, 0, true, NULL},
};
// Ensure any prior DMA has completed before continuing
spiWait(dev);
spiSequence(dev, &segments[0]);
// Block pending completion of SPI access
spiWait(dev);
}
#ifdef USE_QUADSPI
else if (fdevice->io.mode == FLASHIO_QUADSPI) {
@ -401,18 +440,26 @@ static void w25n01g_programDataLoad(flashDevice_t *fdevice, uint16_t columnAddre
static void w25n01g_randomProgramDataLoad(flashDevice_t *fdevice, uint16_t columnAddress, const uint8_t *data, int length)
{
const uint8_t cmd[] = { W25N01G_INSTRUCTION_RANDOM_PROGRAM_DATA_LOAD, columnAddress >> 8, columnAddress & 0xff };
uint8_t cmd[] = { W25N01G_INSTRUCTION_RANDOM_PROGRAM_DATA_LOAD, columnAddress >> 8, columnAddress & 0xff };
w25n01g_waitForReady(fdevice);
if (fdevice->io.mode == FLASHIO_SPI) {
busDevice_t *busdev = fdevice->io.handle.busdev;
extDevice_t *dev = fdevice->io.handle.dev;
ENABLE(busdev);
spiTransfer(busdev->busdev_u.spi.instance, cmd, NULL, sizeof(cmd));
spiTransfer(busdev->busdev_u.spi.instance, data, NULL, length);
DISABLE(busdev);
busSegment_t segments[] = {
{cmd, NULL, sizeof (cmd), true, NULL},
{(uint8_t *)data, NULL, length, true, NULL},
{NULL, NULL, 0, true, NULL},
};
// Ensure any prior DMA has completed before continuing
spiWait(dev);
spiSequence(dev, &segments[0]);
// Block pending completion of SPI access
spiWait(dev);
}
#ifdef USE_QUADSPI
else if (fdevice->io.mode == FLASHIO_QUADSPI) {
@ -496,8 +543,6 @@ void w25n01g_pageProgramBegin(flashDevice_t *fdevice, uint32_t address)
void w25n01g_pageProgramContinue(flashDevice_t *fdevice, const uint8_t *data, int length)
{
// Check for page boundary overrun
w25n01g_waitForReady(fdevice);
w25n01g_writeEnable(fdevice);
@ -617,7 +662,7 @@ int w25n01g_readBytes(flashDevice_t *fdevice, uint32_t address, uint8_t *buffer,
currentPage = targetPage;
}
uint16_t column = W25N01G_LINEAR_TO_COLUMN(address);
int column = W25N01G_LINEAR_TO_COLUMN(address);
uint16_t transferLength;
if (length > W25N01G_PAGE_SIZE - column) {
@ -627,7 +672,7 @@ int w25n01g_readBytes(flashDevice_t *fdevice, uint32_t address, uint8_t *buffer,
}
if (fdevice->io.mode == FLASHIO_SPI) {
busDevice_t *busdev = fdevice->io.handle.busdev;
extDevice_t *dev = fdevice->io.handle.dev;
uint8_t cmd[4];
cmd[0] = W25N01G_INSTRUCTION_READ_DATA;
@ -635,11 +680,19 @@ int w25n01g_readBytes(flashDevice_t *fdevice, uint32_t address, uint8_t *buffer,
cmd[2] = (column >> 0) & 0xff;
cmd[3] = 0;
ENABLE(busdev);
spiTransfer(busdev->busdev_u.spi.instance, cmd, NULL, sizeof(cmd));
spiTransfer(busdev->busdev_u.spi.instance, NULL, buffer, length);
DISABLE(busdev);
busSegment_t segments[] = {
{cmd, NULL, sizeof (cmd), false, NULL},
{NULL, buffer, length, true, NULL},
{NULL, NULL, 0, true, NULL},
};
// Ensure any prior DMA has completed before continuing
spiWait(dev);
spiSequence(dev, &segments[0]);
// Block pending completion of SPI access
spiWait(dev);
}
#ifdef USE_QUADSPI
else if (fdevice->io.mode == FLASHIO_QUADSPI) {
@ -687,7 +740,7 @@ int w25n01g_readExtensionBytes(flashDevice_t *fdevice, uint32_t address, uint8_t
uint32_t column = 2048;
if (fdevice->io.mode == FLASHIO_SPI) {
busDevice_t *busdev = fdevice->io.handle.busdev;
extDevice_t *dev = fdevice->io.handle.dev;
uint8_t cmd[4];
cmd[0] = W25N01G_INSTRUCTION_READ_DATA;
@ -695,10 +748,19 @@ int w25n01g_readExtensionBytes(flashDevice_t *fdevice, uint32_t address, uint8_t
cmd[2] = (column >> 0) & 0xff;
cmd[3] = 0;
ENABLE(busdev);
spiTransfer(busdev->busdev_u.spi.instance, cmd, NULL, sizeof(cmd));
spiTransfer(busdev->busdev_u.spi.instance, NULL, buffer, length);
DISABLE(busdev);
busSegment_t segments[] = {
{cmd, NULL, sizeof (cmd), false, NULL},
{NULL, buffer, length, true, NULL},
{NULL, NULL, 0, true, NULL},
};
// Ensure any prior DMA has completed before continuing
spiWait(dev);
spiSequence(dev, &segments[0]);
// Block pending completion of SPI access
spiWait(dev);
}
#ifdef USE_QUADSPI
@ -740,28 +802,35 @@ const flashVTable_t w25n01g_vTable = {
void w25n01g_readBBLUT(flashDevice_t *fdevice, bblut_t *bblut, int lutsize)
{
uint8_t in[4];
if (fdevice->io.mode == FLASHIO_SPI) {
busDevice_t *busdev = fdevice->io.handle.busdev;
extDevice_t *dev = fdevice->io.handle.dev;
uint8_t cmd[4];
cmd[0] = W25N01G_INSTRUCTION_READ_BBM_LUT;
cmd[1] = 0;
ENABLE(busdev);
busSegment_t segments[] = {
{cmd, NULL, sizeof (cmd), false, NULL},
{NULL, in, sizeof (in), true, NULL},
{NULL, NULL, 0, true, NULL},
};
spiTransfer(busdev->busdev_u.spi.instance, cmd, NULL, 2);
// Ensure any prior DMA has completed before continuing
spiWait(dev);
spiSequence(dev, &segments[0]);
// Block pending completion of SPI access
spiWait(dev);
for (int i = 0 ; i < lutsize ; i++) {
spiTransfer(busdev->busdev_u.spi.instance, NULL, in, 4);
spiReadWriteBuf(dev, NULL, in, 4);
bblut[i].pba = (in[0] << 16)|in[1];
bblut[i].lba = (in[2] << 16)|in[3];
}
DISABLE(busdev);
}
#ifdef USE_QUADSPI
else if (fdevice->io.mode == FLASHIO_QUADSPI) {
@ -788,13 +857,22 @@ void w25n01g_writeBBLUT(flashDevice_t *fdevice, uint16_t lba, uint16_t pba)
w25n01g_waitForReady(fdevice);
if (fdevice->io.mode == FLASHIO_SPI) {
busDevice_t *busdev = fdevice->io.handle.busdev;
extDevice_t *dev = fdevice->io.handle.dev;
uint8_t cmd[5] = { W25N01G_INSTRUCTION_BB_MANAGEMENT, lba >> 8, lba, pba >> 8, pba };
ENABLE(busdev);
spiTransfer(busdev->busdev_u.spi.instance, cmd, NULL, sizeof(cmd));
DISABLE(busdev);
busSegment_t segments[] = {
{cmd, NULL, sizeof (cmd), true, NULL},
{NULL, NULL, 0, true, NULL},
};
// Ensure any prior DMA has completed before continuing
spiWait(dev);
spiSequence(dev, &segments[0]);
// Block pending completion of SPI access
spiWait(dev);
}
#ifdef USE_QUADSPI
else if (fdevice->io.mode == FLASHIO_QUADSPI) {

2
src/main/drivers/light_ws2811strip.c
View File

@ -52,7 +52,7 @@
#define WS2811_DMA_BUF_CACHE_ALIGN_BYTES ((WS2811_DMA_BUF_BYTES + 0x20) & ~0x1f)
// Size of array to create a cache aligned buffer
#define WS2811_DMA_BUF_CACHE_ALIGN_LENGTH (WS2811_DMA_BUF_CACHE_ALIGN_BYTES / sizeof (uint32_t))
__attribute__((aligned(32))) uint32_t ledStripDMABuffer[WS2811_DMA_BUF_CACHE_ALIGN_LENGTH];
DMA_RW_AXI __attribute__((aligned(32))) uint32_t ledStripDMABuffer[WS2811_DMA_BUF_CACHE_ALIGN_LENGTH];
#else
#if defined(STM32F1) || defined(STM32F3)
uint8_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE];

344
src/main/drivers/max7456.c
View File

@ -42,10 +42,9 @@
#include "drivers/time.h"
// 20 MHz max SPI frequency
// 10 MHz max SPI frequency
#define MAX7456_MAX_SPI_CLK_HZ 10000000
// 1 MHz restore SPI frequency for shared SPI bus
#define MAX7456_MAX_SPI_SHARED_CLK 1000000
#define MAX7456_INIT_MAX_SPI_CLK_HZ 5000000
// DEBUG_MAX7456_SIGNAL
#define DEBUG_MAX7456_SIGNAL_MODEREG 0
@ -177,27 +176,17 @@
#define CHARS_PER_LINE 30 // XXX Should be related to VIDEO_BUFFER_CHARS_*?
// On shared SPI bus we want to change clock for OSD chip and restore for other devices.
#ifdef USE_SPI_TRANSACTION
#define __spiBusTransactionBegin(busdev) spiBusTransactionBegin(busdev)
#define __spiBusTransactionEnd(busdev) spiBusTransactionEnd(busdev)
#else
#define __spiBusTransactionBegin(busdev) {spiBusSetDivisor(busdev, max7456SpiClock);IOLo((busdev)->busdev_u.spi.csnPin);}
#define __spiBusTransactionEnd(busdev) {IOHi((busdev)->busdev_u.spi.csnPin);spiSetDivisor((busdev)->busdev_u.spi.instance, spiCalculateDivider(MAX7456_MAX_SPI_SHARED_CLK));}
#endif
#define MAX7456_SUPPORTED_LAYER_COUNT (DISPLAYPORT_LAYER_BACKGROUND + 1)
typedef struct max7456Layer_s {
uint8_t buffer[VIDEO_BUFFER_CHARS_PAL];
} max7456Layer_t;
static max7456Layer_t displayLayers[MAX7456_SUPPORTED_LAYER_COUNT];
static DMA_DATA_ZERO_INIT max7456Layer_t displayLayers[MAX7456_SUPPORTED_LAYER_COUNT];
static displayPortLayer_e activeLayer = DISPLAYPORT_LAYER_FOREGROUND;
busDevice_t max7456BusDevice;
busDevice_t *busdev = &max7456BusDevice;
extDevice_t max7456Device;
extDevice_t *dev = &max7456Device;
static bool max7456DeviceDetected = false;
static uint16_t max7456SpiClock;
@ -213,9 +202,6 @@ static uint8_t shadowBuffer[VIDEO_BUFFER_CHARS_PAL];
//Max chars to update in one idle
#define MAX_CHARS2UPDATE 100
#ifdef MAX7456_DMA_CHANNEL_TX
volatile bool dmaTransactionInProgress = false;
#endif
static uint8_t spiBuff[MAX_CHARS2UPDATE*6];
@ -249,128 +235,6 @@ static uint8_t *getActiveLayerBuffer(void)
return getLayerBuffer(activeLayer);
}
static uint8_t max7456Send(uint8_t add, uint8_t data)
{
spiTransferByte(busdev->busdev_u.spi.instance, add);
return spiTransferByte(busdev->busdev_u.spi.instance, data);
}
#ifdef MAX7456_DMA_CHANNEL_TX
static void max7456SendDma(void* tx_buffer, void* rx_buffer, uint16_t buffer_size)
{
DMA_InitTypeDef DMA_InitStructure;
#ifdef MAX7456_DMA_CHANNEL_RX
static uint16_t dummy[] = {0xffff};
#else
UNUSED(rx_buffer);
#endif
while (dmaTransactionInProgress); // Wait for prev DMA transaction
DMA_DeInit(MAX7456_DMA_CHANNEL_TX);
#ifdef MAX7456_DMA_CHANNEL_RX
DMA_DeInit(MAX7456_DMA_CHANNEL_RX);
#endif
// Common to both channels
DMA_StructInit(&DMA_InitStructure);
DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)(&(busdev->busdev_u.spi.instance->DR));
DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
DMA_InitStructure.DMA_BufferSize = buffer_size;
DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
DMA_InitStructure.DMA_Priority = DMA_Priority_Low;
#ifdef MAX7456_DMA_CHANNEL_RX
// Rx Channel
#ifdef STM32F4
DMA_InitStructure.DMA_Memory0BaseAddr = rx_buffer ? (uint32_t)rx_buffer : (uint32_t)(dummy);
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory;
#else
DMA_InitStructure.DMA_MemoryBaseAddr = rx_buffer ? (uint32_t)rx_buffer : (uint32_t)(dummy);
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
#endif
DMA_InitStructure.DMA_MemoryInc = rx_buffer ? DMA_MemoryInc_Enable : DMA_MemoryInc_Disable;
DMA_Init(MAX7456_DMA_CHANNEL_RX, &DMA_InitStructure);
DMA_Cmd(MAX7456_DMA_CHANNEL_RX, ENABLE);
#endif
// Tx channel
#ifdef STM32F4
DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)tx_buffer; //max7456_screen;
DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
#else
DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)tx_buffer; //max7456_screen;
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST;
#endif
DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
DMA_Init(MAX7456_DMA_CHANNEL_TX, &DMA_InitStructure);
DMA_Cmd(MAX7456_DMA_CHANNEL_TX, ENABLE);
#ifdef MAX7456_DMA_CHANNEL_RX
DMA_ITConfig(MAX7456_DMA_CHANNEL_RX, DMA_IT_TC, ENABLE);
#else
DMA_ITConfig(MAX7456_DMA_CHANNEL_TX, DMA_IT_TC, ENABLE);
#endif
// Enable SPI TX/RX request
__spiBusTransactionBegin(busdev);
dmaTransactionInProgress = true;
SPI_I2S_DMACmd(busdev->busdev_u.spi.instance,
#ifdef MAX7456_DMA_CHANNEL_RX
SPI_I2S_DMAReq_Rx |
#endif
SPI_I2S_DMAReq_Tx, ENABLE);
}
void max7456_dma_irq_handler(dmaChannelDescriptor_t* descriptor)
{
if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TCIF)) {
#ifdef MAX7456_DMA_CHANNEL_RX
DMA_Cmd(MAX7456_DMA_CHANNEL_RX, DISABLE);
#endif
// Make sure SPI DMA transfer is complete
while (SPI_I2S_GetFlagStatus (busdev->busdev_u.spi.instance, SPI_I2S_FLAG_TXE) == RESET) {};
while (SPI_I2S_GetFlagStatus (busdev->busdev_u.spi.instance, SPI_I2S_FLAG_BSY) == SET) {};
// Empty RX buffer. RX DMA takes care of it if enabled.
// This should be done after transmission finish!!!
while (SPI_I2S_GetFlagStatus(busdev->busdev_u.spi.instance, SPI_I2S_FLAG_RXNE) == SET) {
busdev->busdev_u.spi.instance->DR;
}
DMA_Cmd(MAX7456_DMA_CHANNEL_TX, DISABLE);
DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF);
SPI_I2S_DMACmd(busdev->busdev_u.spi.instance,
#ifdef MAX7456_DMA_CHANNEL_RX
SPI_I2S_DMAReq_Rx |
#endif
SPI_I2S_DMAReq_Tx, DISABLE);
__spiBusTransactionEnd(busdev);
dmaTransactionInProgress = false;
}
if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_HTIF)) {
DMA_CLEAR_FLAG(descriptor, DMA_IT_HTIF);
}
if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TEIF)) {
DMA_CLEAR_FLAG(descriptor, DMA_IT_TEIF);
}
}
#endif
static void max7456SetRegisterVM1(void)
{
uint8_t backgroundGray = BACKGROUND_BRIGHTNESS_28; // this is the device default background gray level
@ -391,7 +255,7 @@ static void max7456SetRegisterVM1(void)
}
}
vm1Register |= (backgroundGray << 4);
max7456Send(MAX7456ADD_VM1, vm1Register);
spiWriteReg(dev, MAX7456ADD_VM1, vm1Register);
}
uint8_t max7456GetRowsCount(void)
@ -412,15 +276,11 @@ static void max7456ClearLayer(displayPortLayer_e layer)
memset(getLayerBuffer(layer), 0x20, VIDEO_BUFFER_CHARS_PAL);
}
void max7456ReInit(void)
{
uint8_t srdata = 0;
static bool firstInit = true;
__spiBusTransactionBegin(busdev);
switch (videoSignalCfg) {
case VIDEO_SYSTEM_PAL:
videoSignalReg = VIDEO_MODE_PAL | OSD_ENABLE;
@ -431,7 +291,7 @@ void max7456ReInit(void)
break;
case VIDEO_SYSTEM_AUTO:
srdata = max7456Send(MAX7456ADD_STAT, 0x00);
srdata = spiReadRegMsk(dev, MAX7456ADD_STAT);
if (VIN_IS_NTSC(srdata)) {
videoSignalReg = VIDEO_MODE_NTSC | OSD_ENABLE;
@ -454,19 +314,17 @@ void max7456ReInit(void)
previousBlackWhiteRegister = INVALID_PREVIOUS_REGISTER_STATE;
max7456Brightness(0, 2);
// Re-enable MAX7456 (last function call disables it)
__spiBusTransactionBegin(busdev);
// Make sure the Max7456 is enabled
max7456Send(MAX7456ADD_VM0, videoSignalReg);
max7456Send(MAX7456ADD_HOS, hosRegValue);
max7456Send(MAX7456ADD_VOS, vosRegValue);
max7456SetRegisterVM1();
spiWriteReg(dev, MAX7456ADD_VM0, videoSignalReg);
spiWriteReg(dev, MAX7456ADD_HOS, hosRegValue);
spiWriteReg(dev, MAX7456ADD_VOS, vosRegValue);
max7456Send(MAX7456ADD_DMM, displayMemoryModeReg | CLEAR_DISPLAY);
__spiBusTransactionEnd(busdev);
max7456SetRegisterVM1();
// Clear shadow to force redraw all screen in non-dma mode.
max7456ClearShadowBuffer();
if (firstInit) {
max7456DrawScreenSlow();
firstInit = false;
@ -483,7 +341,6 @@ void max7456PreInit(const max7456Config_t *max7456Config)
max7456InitStatus_e max7456Init(const max7456Config_t *max7456Config, const vcdProfile_t *pVcdProfile, bool cpuOverclock)
{
max7456SpiClock = spiCalculateDivider(MAX7456_MAX_SPI_CLK_HZ);
max7456DeviceDetected = false;
deviceBackgroundType = DISPLAY_BACKGROUND_TRANSPARENT;
@ -494,69 +351,62 @@ max7456InitStatus_e max7456Init(const max7456Config_t *max7456Config, const vcdP
max7456HardwareReset();
if (!max7456Config->csTag || !max7456Config->spiDevice) {
if (!max7456Config->csTag || !spiSetBusInstance(dev, max7456Config->spiDevice, OWNER_OSD_CS)) {
return MAX7456_INIT_NOT_CONFIGURED;
}
busdev->busdev_u.spi.csnPin = IOGetByTag(max7456Config->csTag);
dev->busType_u.spi.csnPin = IOGetByTag(max7456Config->csTag);
if (!IOIsFreeOrPreinit(busdev->busdev_u.spi.csnPin)) {
if (!IOIsFreeOrPreinit(dev->busType_u.spi.csnPin)) {
return MAX7456_INIT_NOT_CONFIGURED;
}
IOInit(busdev->busdev_u.spi.csnPin, OWNER_OSD_CS, 0);
IOConfigGPIO(busdev->busdev_u.spi.csnPin, SPI_IO_CS_CFG);
IOHi(busdev->busdev_u.spi.csnPin);
spiBusSetInstance(busdev, spiInstanceByDevice(SPI_CFG_TO_DEV(max7456Config->spiDevice)));
IOInit(dev->busType_u.spi.csnPin, OWNER_OSD_CS, 0);
IOConfigGPIO(dev->busType_u.spi.csnPin, SPI_IO_CS_CFG);
IOHi(dev->busType_u.spi.csnPin);
// Detect MAX7456 existence and device type. Do this at half the speed for safety.
// Detect MAX7456 and compatible device by reading OSDM (OSD Insertion MUX) register.
// This register is not modified in this driver, therefore ensured to remain at its default value (0x1B).
spiSetDivisor(busdev->busdev_u.spi.instance, spiCalculateDivider(MAX7456_MAX_SPI_CLK_HZ) * 2);
spiSetClkDivisor(dev, spiCalculateDivider(MAX7456_INIT_MAX_SPI_CLK_HZ));
__spiBusTransactionBegin(busdev);
// Write 0xff to conclude any current SPI transaction the MAX7456 is expecting
spiWrite(dev, END_STRING);
uint8_t osdm = max7456Send(MAX7456ADD_OSDM|MAX7456ADD_READ, 0xff);
__spiBusTransactionEnd(busdev);
uint8_t osdm = spiReadRegMsk(dev, MAX7456ADD_OSDM);
if (osdm != 0x1B) {
IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_IPU);
IOConfigGPIO(dev->busType_u.spi.csnPin, IOCFG_IPU);
return MAX7456_INIT_NOT_FOUND;
}
// At this point, we can claim the ownership of the CS pin
max7456DeviceDetected = true;
IOInit(busdev->busdev_u.spi.csnPin, OWNER_OSD_CS, 0);
IOInit(dev->busType_u.spi.csnPin, OWNER_OSD_CS, 0);
// Detect device type by writing and reading CA[8] bit at CMAL[6].
// This is a bit for accessing second half of character glyph storage, supported only by AT variant.
__spiBusTransactionBegin(busdev);
spiWriteReg(dev, MAX7456ADD_CMAL, (1 << 6)); // CA[8] bit
max7456Send(MAX7456ADD_CMAL, (1 << 6)); // CA[8] bit
if (max7456Send(MAX7456ADD_CMAL|MAX7456ADD_READ, 0xff) & (1 << 6)) {
if (spiReadRegMsk(dev, MAX7456ADD_CMAL) & (1 << 6)) {
max7456DeviceType = MAX7456_DEVICE_TYPE_AT;
} else {
max7456DeviceType = MAX7456_DEVICE_TYPE_MAX;
}
__spiBusTransactionEnd(busdev);
#if defined(USE_OVERCLOCK)
// Determine SPI clock divisor based on config and the device type.
switch (max7456Config->clockConfig) {
case MAX7456_CLOCK_CONFIG_HALF:
max7456SpiClock = spiCalculateDivider(MAX7456_MAX_SPI_CLK_HZ) * 2;
max7456SpiClock = spiCalculateDivider(MAX7456_MAX_SPI_CLK_HZ / 2);
break;
case MAX7456_CLOCK_CONFIG_OC:
max7456SpiClock = (cpuOverclock && (max7456DeviceType == MAX7456_DEVICE_TYPE_MAX)) ? spiCalculateDivider(MAX7456_MAX_SPI_CLK_HZ) * 2 : spiCalculateDivider(MAX7456_MAX_SPI_CLK_HZ);
max7456SpiClock = (cpuOverclock && (max7456DeviceType == MAX7456_DEVICE_TYPE_MAX)) ? spiCalculateDivider(MAX7456_MAX_SPI_CLK_HZ / 2) : spiCalculateDivider(MAX7456_MAX_SPI_CLK_HZ);
break;
case MAX7456_CLOCK_CONFIG_FULL:
@ -572,26 +422,22 @@ max7456InitStatus_e max7456Init(const max7456Config_t *max7456Config, const vcdP
UNUSED(cpuOverclock);
#endif
#ifdef USE_SPI_TRANSACTION
spiBusTransactionInit(busdev, SPI_MODE3_POL_HIGH_EDGE_2ND, max7456SpiClock);
#else
spiBusSetDivisor(busdev, max7456SpiClock);
#endif
spiSetClkDivisor(dev, max7456SpiClock);
// force soft reset on Max7456
__spiBusTransactionBegin(busdev);
max7456Send(MAX7456ADD_VM0, MAX7456_RESET);
__spiBusTransactionEnd(busdev);
spiWriteReg(dev, MAX7456ADD_VM0, MAX7456_RESET);
// Wait for 100us before polling for completion of reset
delayMicroseconds(100);
// Wait for reset to complete
while ((spiReadRegMsk(dev, MAX7456ADD_VM0) & MAX7456_RESET) != 0x00);
// Setup values to write to registers
videoSignalCfg = pVcdProfile->video_system;
hosRegValue = 32 - pVcdProfile->h_offset;
vosRegValue = 16 - pVcdProfile->v_offset;
#ifdef MAX7456_DMA_CHANNEL_TX
dmaSetHandler(MAX7456_DMA_IRQ_HANDLER_ID, max7456_dma_irq_handler, NVIC_PRIO_MAX7456_DMA, 0);
#endif
// Real init will be made later when driver detect idle.
return MAX7456_INIT_OK;
}
@ -612,9 +458,7 @@ void max7456Invert(bool invert)
// redrawn with the proper invert state
max7456ClearShadowBuffer();
previousInvertRegister = displayMemoryModeReg;
__spiBusTransactionBegin(busdev);
max7456Send(MAX7456ADD_DMM, displayMemoryModeReg);
__spiBusTransactionEnd(busdev);
spiWriteReg(dev, MAX7456ADD_DMM, displayMemoryModeReg);
}
}
@ -630,11 +474,9 @@ void max7456Brightness(uint8_t black, uint8_t white)
if (reg != previousBlackWhiteRegister) {
previousBlackWhiteRegister = reg;
__spiBusTransactionBegin(busdev);
for (int i = MAX7456ADD_RB0; i <= MAX7456ADD_RB15; i++) {
max7456Send(i, reg);
spiWriteReg(dev, i, reg);
}
__spiBusTransactionEnd(busdev);
}
}
@ -692,11 +534,7 @@ bool max7456LayerCopy(displayPortLayer_e destLayer, displayPortLayer_e sourceLay
bool max7456DmaInProgress(void)
{
#ifdef MAX7456_DMA_CHANNEL_TX
return dmaTransactionInProgress;
#else
return false;
#endif
return spiIsBusy(dev);
}
bool max7456BuffersSynced(void)
@ -721,9 +559,11 @@ void max7456ReInitIfRequired(bool forceStallCheck)
bool stalled = false;
if (forceStallCheck || (lastStallCheckMs + MAX7456_STALL_CHECK_INTERVAL_MS < nowMs)) {
lastStallCheckMs = nowMs;
__spiBusTransactionBegin(busdev);
stalled = (max7456Send(MAX7456ADD_VM0|MAX7456ADD_READ, 0x00) != videoSignalReg);
__spiBusTransactionEnd(busdev);
// Write 0xff to conclude any current SPI transaction the MAX7456 is expecting
spiWrite(dev, END_STRING);
stalled = (spiReadRegMsk(dev, MAX7456ADD_VM0) != videoSignalReg);
}
if (stalled) {
@ -731,11 +571,12 @@ void max7456ReInitIfRequired(bool forceStallCheck)
} else if ((videoSignalCfg == VIDEO_SYSTEM_AUTO)
&& ((nowMs - lastSigCheckMs) > MAX7456_SIGNAL_CHECK_INTERVAL_MS)) {
// Write 0xff to conclude any current SPI transaction the MAX7456 is expecting
spiWrite(dev, END_STRING);
// Adjust output format based on the current input format.
__spiBusTransactionBegin(busdev);
const uint8_t videoSense = max7456Send(MAX7456ADD_STAT, 0x00);
__spiBusTransactionEnd(busdev);
const uint8_t videoSense = spiReadRegMsk(dev, MAX7456ADD_STAT);
DEBUG_SET(DEBUG_MAX7456_SIGNAL, DEBUG_MAX7456_SIGNAL_MODEREG, videoSignalReg & VIDEO_MODE_MASK);
DEBUG_SET(DEBUG_MAX7456_SIGNAL, DEBUG_MAX7456_SIGNAL_SENSE, videoSense & 0x7);
@ -767,6 +608,11 @@ void max7456ReInitIfRequired(bool forceStallCheck)
void max7456DrawScreen(void)
{
static uint16_t pos = 0;
// This routine doesn't block so need to use static data
static busSegment_t segments[] = {
{NULL, NULL, 0, true, NULL},
{NULL, NULL, 0, true, NULL},
};
if (!fontIsLoading) {
@ -795,13 +641,15 @@ void max7456DrawScreen(void)
}
if (buff_len) {
#ifdef MAX7456_DMA_CHANNEL_TX
max7456SendDma(spiBuff, NULL, buff_len);
#else
__spiBusTransactionBegin(busdev);
spiTransfer(busdev->busdev_u.spi.instance, spiBuff, NULL, buff_len);
__spiBusTransactionEnd(busdev);
#endif // MAX7456_DMA_CHANNEL_TX
segments[0].txData = spiBuff;
segments[0].len = buff_len;
// Ensure any prior DMA has completed
spiWaitClaim(dev);
spiSequence(dev, &segments[0]);
// Non-blocking, so transfer still in progress
}
}
}
@ -811,50 +659,49 @@ static void max7456DrawScreenSlow(void)
bool escapeCharFound = false;
uint8_t *buffer = getActiveLayerBuffer();
__spiBusTransactionBegin(busdev);
// Enable auto-increment mode and update every character in the active buffer.
// The "escape" character 0xFF must be skipped as it causes the MAX7456 to exit auto-increment mode.
max7456Send(MAX7456ADD_DMAH, 0);
max7456Send(MAX7456ADD_DMAL, 0);
max7456Send(MAX7456ADD_DMM, displayMemoryModeReg | 1);
uint8_t dma_regs[3];
dma_regs[0] = displayMemoryModeReg | 1;
dma_regs[1] = 0;
dma_regs[2] = 0;
spiWriteRegBuf(dev, MAX7456ADD_DMM, dma_regs, sizeof (dma_regs));
// The "escape" character 0xFF must be skipped as it causes the MAX7456 to exit auto-increment mode.
for (int xx = 0; xx < maxScreenSize; xx++) {
if (buffer[xx] == END_STRING) {
escapeCharFound = true;
max7456Send(MAX7456ADD_DMDI, ' '); // replace the 0xFF character with a blank in the first pass to avoid terminating auto-increment
spiWriteReg(dev, MAX7456ADD_DMDI, ' '); // replace the 0xFF character with a blank in the first pass to avoid terminating auto-increment
} else {
max7456Send(MAX7456ADD_DMDI, buffer[xx]);
spiWriteReg(dev, MAX7456ADD_DMDI, buffer[xx]);
}
shadowBuffer[xx] = buffer[xx];
}
max7456Send(MAX7456ADD_DMDI, END_STRING);
max7456Send(MAX7456ADD_DMM, displayMemoryModeReg);
spiWriteReg(dev, MAX7456ADD_DMDI, END_STRING);
// Turn off auto increment
spiWriteReg(dev, MAX7456ADD_DMM, displayMemoryModeReg);
// If we found any of the "escape" character 0xFF, then make a second pass
// to update them with direct addressing
if (escapeCharFound) {
dma_regs[2] = END_STRING;
for (int xx = 0; xx < maxScreenSize; xx++) {
if (buffer[xx] == END_STRING) {
max7456Send(MAX7456ADD_DMAH, xx >> 8);
max7456Send(MAX7456ADD_DMAL, xx & 0xFF);
max7456Send(MAX7456ADD_DMDI, END_STRING);
dma_regs[0] = xx >> 8;
dma_regs[1] = xx & 0xFF;
spiWriteRegBuf(dev, MAX7456ADD_DMAH, dma_regs, sizeof (dma_regs));
}
}
}
__spiBusTransactionEnd(busdev);
}
// should not be used when armed
void max7456RefreshAll(void)
{
#ifdef MAX7456_DMA_CHANNEL_TX
while (dmaTransactionInProgress);
#endif
max7456ReInitIfRequired(true);
max7456DrawScreenSlow();
}
@ -864,20 +711,19 @@ bool max7456WriteNvm(uint8_t char_address, const uint8_t *font_data)
if (!max7456DeviceDetected) {
return false;
}
#ifdef MAX7456_DMA_CHANNEL_TX
while (dmaTransactionInProgress);
#endif
__spiBusTransactionBegin(busdev);
// Block pending completion of any prior SPI access
spiWait(dev);
// disable display
fontIsLoading = true;
max7456Send(MAX7456ADD_VM0, 0);
spiWriteReg(dev, MAX7456ADD_VM0, 0);
max7456Send(MAX7456ADD_CMAH, char_address); // set start address high
spiWriteReg(dev, MAX7456ADD_CMAH, char_address); // set start address high
for (int x = 0; x < 54; x++) {
max7456Send(MAX7456ADD_CMAL, x); //set start address low
max7456Send(MAX7456ADD_CMDI, font_data[x]);
spiWriteReg(dev, MAX7456ADD_CMAL, x); //set start address low
spiWriteReg(dev, MAX7456ADD_CMDI, font_data[x]);
#ifdef LED0_TOGGLE
LED0_TOGGLE;
#else
@ -887,13 +733,12 @@ bool max7456WriteNvm(uint8_t char_address, const uint8_t *font_data)
// Transfer 54 bytes from shadow ram to NVM
max7456Send(MAX7456ADD_CMM, WRITE_NVR);
spiWriteReg(dev, MAX7456ADD_CMM, WRITE_NVR);
// Wait until bit 5 in the status register returns to 0 (12ms)
while ((max7456Send(MAX7456ADD_STAT, 0x00) & STAT_NVR_BUSY) != 0x00);
while ((spiReadRegMsk(dev, MAX7456ADD_STAT) & STAT_NVR_BUSY) != 0x00);
__spiBusTransactionEnd(busdev);
return true;
}
@ -910,11 +755,14 @@ void max7456HardwareReset(void)
IOInit(max7456ResetPin, OWNER_OSD, 0);
IOConfigGPIO(max7456ResetPin, IO_RESET_CFG);
// RESET
// RESET 50ms long pulse, followed by 100us pause
IOLo(max7456ResetPin);
delay(100);
delay(50);
IOHi(max7456ResetPin);
delayMicroseconds(100);
#else
// Allow device 50ms to powerup
delay(50);
#endif
}
@ -927,9 +775,7 @@ void max7456SetBackgroundType(displayPortBackground_e backgroundType)
{
deviceBackgroundType = backgroundType;
__spiBusTransactionBegin(busdev);
max7456SetRegisterVM1();
__spiBusTransactionEnd(busdev);
}
#endif // USE_MAX7456

1
src/main/drivers/nvic.h
View File

@ -74,6 +74,7 @@
#define NVIC_PRIO_MAG_DATA_READY NVIC_BUILD_PRIORITY(0x0f, 0x0f)
#define NVIC_PRIO_CALLBACK NVIC_BUILD_PRIORITY(0x0f, 0x0f)
#define NVIC_PRIO_MAX7456_DMA NVIC_BUILD_PRIORITY(3, 0)
#define NVIC_PRIO_SPI_DMA NVIC_BUILD_PRIORITY(0, 0)
#define NVIC_PRIO_SDIO_DMA NVIC_BUILD_PRIORITY(0, 0)
#ifdef USE_HAL_DRIVER

24
src/main/drivers/rx/rx_cc2500.c
View File

@ -48,10 +48,28 @@ void cc2500ReadFifo(uint8_t *dpbuffer, uint8_t len)
rxSpiReadCommandMulti(CC2500_3F_RXFIFO | CC2500_READ_BURST, NOP, dpbuffer, len);
}
void cc2500WriteCommand(uint8_t command, uint8_t data)
{
// Burst writes require an interbyte gap, see fig. 7, pg. 22 in https://www.ti.com/lit/ds/symlink/cc2500.pdf
// As such gaps can't be inserted if DMA is being used, inhibit DMA on this bus for the duration of this call
rxSpiDmaEnable(false);
rxSpiWriteCommand(command, data);
rxSpiDmaEnable(true);
}
void cc2500WriteCommandMulti(uint8_t command, const uint8_t *data, uint8_t length)
{
// Burst writes require an interbyte gap, see fig. 7, pg. 22 in https://www.ti.com/lit/ds/symlink/cc2500.pdf
// As such gaps can't be inserted if DMA is being used, inhibit DMA on this bus for the duration of this call
rxSpiDmaEnable(false);
rxSpiWriteCommandMulti(command, data, length);
rxSpiDmaEnable(true);
}
void cc2500WriteFifo(uint8_t *dpbuffer, uint8_t len)
{
cc2500Strobe(CC2500_SFTX); // 0x3B SFTX
rxSpiWriteCommandMulti(CC2500_3F_TXFIFO | CC2500_WRITE_BURST,
cc2500WriteCommandMulti(CC2500_3F_TXFIFO | CC2500_WRITE_BURST,
dpbuffer, len);
cc2500Strobe(CC2500_STX); // 0x35
}
@ -64,7 +82,7 @@ void cc2500ReadRegisterMulti(uint8_t address, uint8_t *data, uint8_t length)
void cc2500WriteRegisterMulti(uint8_t address, uint8_t *data,
uint8_t length)
{
rxSpiWriteCommandMulti(address, data, length);
cc2500WriteCommandMulti(address, data, length);
}
uint8_t cc2500ReadReg(uint8_t reg)
@ -76,7 +94,7 @@ void cc2500Strobe(uint8_t address) { rxSpiWriteByte(address); }
void cc2500WriteReg(uint8_t address, uint8_t data)
{
rxSpiWriteCommand(address, data);
cc2500WriteCommand(address, data);
}
void cc2500SetPower(uint8_t power)

56
src/main/drivers/rx/rx_spi.c
View File

@ -43,14 +43,13 @@
#include "rx_spi.h"
// 10 MHz max SPI frequency
#define RX_MAX_SPI_CLK_HZ 10000000
// 13.5 MHz max SPI frequency
#define RX_MAX_SPI_CLK_HZ 13500000
// 6.5 MHz max SPI frequency during startup
#define RX_STARTUP_MAX_SPI_CLK_HZ 6500000
#define ENABLE_RX() IOLo(busdev->busdev_u.spi.csnPin)
#define DISABLE_RX() IOHi(busdev->busdev_u.spi.csnPin)
static busDevice_t rxSpiDevice;
static busDevice_t *busdev = &rxSpiDevice;
static extDevice_t rxSpiDevice;
static extDevice_t *dev = &rxSpiDevice;
static IO_t extiPin = IO_NONE;
static extiCallbackRec_t rxSpiExtiCallbackRec;
@ -76,27 +75,32 @@ void rxSpiExtiHandler(extiCallbackRec_t* callback)
}
}
void rxSpiNormalSpeed()
{
spiSetClkDivisor(dev, spiCalculateDivider(RX_MAX_SPI_CLK_HZ));
}
void rxSpiStartupSpeed()
{
spiSetClkDivisor(dev, spiCalculateDivider(RX_STARTUP_MAX_SPI_CLK_HZ));
}
bool rxSpiDeviceInit(const rxSpiConfig_t *rxSpiConfig)
{
SPI_TypeDef *instance = spiInstanceByDevice(SPI_CFG_TO_DEV(rxSpiConfig->spibus));
if (!instance) {
if (!spiSetBusInstance(dev, rxSpiConfig->spibus, OWNER_RX_SPI_CS)) {
return false;
}
spiBusSetInstance(busdev, instance);
const IO_t rxCsPin = IOGetByTag(rxSpiConfig->csnTag);
IOInit(rxCsPin, OWNER_RX_SPI_CS, 0);
IOConfigGPIO(rxCsPin, SPI_IO_CS_CFG);
busdev->busdev_u.spi.csnPin = rxCsPin;
dev->busType_u.spi.csnPin = rxCsPin;
// Set the clock phase/polarity
spiSetClkPhasePolarity(dev, true);
rxSpiNormalSpeed();
IOHi(rxCsPin);
#ifdef USE_SPI_TRANSACTION
spiBusTransactionInit(busdev, SPI_MODE0_POL_LOW_EDGE_1ST, spiCalculateDivider(RX_MAX_SPI_CLK_HZ));
#else
spiBusSetDivisor(busdev, spiCalculateDivider(RX_MAX_SPI_CLK_HZ));
#endif
extiPin = IOGetByTag(rxSpiConfig->extiIoTag);
@ -119,37 +123,41 @@ void rxSpiExtiInit(ioConfig_t rxSpiExtiPinConfig, extiTrigger_t rxSpiExtiPinTrig
}
}
void rxSpiDmaEnable(bool enable)
{
spiDmaEnable(dev, enable);
}
uint8_t rxSpiTransferByte(uint8_t data)
{
return spiBusTransferByte(busdev, data);
return spiReadWrite(dev, data);
}
void rxSpiWriteByte(uint8_t data)
{
spiBusWriteByte(busdev, data);
spiWrite(dev, data);
}
void rxSpiWriteCommand(uint8_t command, uint8_t data)
{
spiBusWriteRegister(busdev, command, data);
spiWriteReg(dev, command, data);
}
void rxSpiWriteCommandMulti(uint8_t command, const uint8_t *data, uint8_t length)
{
spiBusWriteRegisterBuffer(busdev, command, data, length);
spiWriteRegBuf(dev, command, (uint8_t *)data, length);
}
uint8_t rxSpiReadCommand(uint8_t command, uint8_t data)
{
UNUSED(data);
return spiBusRawReadRegister(busdev, command);
return spiReadReg(dev, command);
}
void rxSpiReadCommandMulti(uint8_t command, uint8_t commandData, uint8_t *retData, uint8_t length)
{
UNUSED(commandData);
spiBusRawReadRegisterBuffer(busdev, command, retData, length);
spiReadRegBuf(dev, command, retData, length);
}
bool rxSpiExtiConfigured(void)

3
src/main/drivers/rx/rx_spi.h
View File

@ -32,6 +32,9 @@ struct rxSpiConfig_s;
void rxSpiDevicePreInit(const struct rxSpiConfig_s *rxSpiConfig);
bool rxSpiDeviceInit(const struct rxSpiConfig_s *rxSpiConfig);
void rxSpiNormalSpeed();
void rxSpiStartupSpeed();
void rxSpiDmaEnable(bool enable);
uint8_t rxSpiTransferByte(uint8_t data);
void rxSpiWriteByte(uint8_t data);
void rxSpiWriteCommand(uint8_t command, uint8_t data);

5
src/main/drivers/sdcard_impl.h
View File

@ -90,9 +90,8 @@ typedef struct sdcard_t {
IO_t cardDetectPin;
#ifdef USE_SDCARD_SPI
busDevice_t busdev;
bool useDMAForTx;
dmaChannelDescriptor_t * dma;
extDevice_t dev;
uint8_t idleCount;
#endif
#ifdef USE_SDCARD_SDIO

439
src/main/drivers/sdcard_spi.c
View File

@ -73,27 +73,18 @@ static bool sdcardSpi_isFunctional(void)
static void sdcard_select(void)
{
#ifdef USE_SPI_TRANSACTION
spiBusTransactionBegin(&sdcard.busdev);
#else
IOLo(sdcard.busdev.busdev_u.spi.csnPin);
#endif
IOLo(sdcard.dev.busType_u.spi.csnPin);
}
static void sdcard_deselect(void)
{
// As per the SD-card spec, give the card 8 dummy clocks so it can finish its operation
//spiBusTransferByte(&sdcard.busdev, 0xFF);
//spiReadWrite(&sdcard.dev, 0xFF);
while (spiBusIsBusBusy(&sdcard.busdev)) {
}
spiWait(&sdcard.dev);
delayMicroseconds(10);
#ifdef USE_SPI_TRANSACTION
spiBusTransactionEnd(&sdcard.busdev);
#else
IOHi(sdcard.busdev.busdev_u.spi.csnPin);
#endif
IOHi(sdcard.dev.busType_u.spi.csnPin);
}
/**
@ -110,11 +101,7 @@ static void sdcard_reset(void)
}
if (sdcard.state >= SDCARD_STATE_READY) {
#ifdef USE_SPI_TRANSACTION
spiBusTransactionInit(&sdcard.busdev, SDCARD_SPI_MODE, spiCalculateDivider(SDCARD_MAX_SPI_INIT_CLK_HZ));
#else
spiSetDivisor(sdcard.busdev.busdev_u.spi.instance, spiCalculateDivider(SDCARD_MAX_SPI_INIT_CLK_HZ));
#endif
spiSetClkDivisor(&sdcard.dev, spiCalculateDivider(SDCARD_MAX_SPI_INIT_CLK_HZ));
}
sdcard.failureCount++;
@ -126,6 +113,50 @@ static void sdcard_reset(void)
}
}
// Called in ISR context
// Wait until idle indicated by a read value of 0xff
busStatus_e sdcard_callbackIdle(uint32_t arg)
{
sdcard_t *sdcard = (sdcard_t *)arg;
extDevice_t *dev = &sdcard->dev;
uint8_t idleByte = dev->bus->curSegment->rxData[0];
if (idleByte == 0xff) {
return BUS_READY;
}
if (--sdcard->idleCount == 0) {
dev->bus->curSegment->rxData[0] = 0x00;
return BUS_ABORT;
}
return BUS_BUSY;
}
// Called in ISR context
// Wait until idle is no longer indicated by a read value of 0xff
busStatus_e sdcard_callbackNotIdle(uint32_t arg)
{
sdcard_t *sdcard = (sdcard_t *)arg;
extDevice_t *dev = &sdcard->dev;
uint8_t idleByte = dev->bus->curSegment->rxData[0];
if (idleByte != 0xff) {
return BUS_READY;
}
if (sdcard->idleCount-- == 0) {
return BUS_ABORT;
}
return BUS_BUSY;
}
/**
* The SD card spec requires 8 clock cycles to be sent by us on the bus after most commands so it can finish its
* processing of that command. The easiest way for us to do this is to just wait for the bus to become idle before
@ -133,15 +164,25 @@ static void sdcard_reset(void)
*/
static bool sdcard_waitForIdle(int maxBytesToWait)
{
while (maxBytesToWait > 0) {
uint8_t b = spiBusTransferByte(&sdcard.busdev, 0xFF);
if (b == 0xFF) {
return true;
}
maxBytesToWait--;
}
uint8_t idleByte;
return false;
// Note that this does not release the CS at the end of the transaction
busSegment_t segments[] = {
{NULL, &idleByte, sizeof (idleByte), false, sdcard_callbackIdle},
{NULL, NULL, 0, false, NULL},
};
sdcard.idleCount = maxBytesToWait;
// Ensure any prior DMA has completed before continuing
spiWaitClaim(&sdcard.dev);
spiSequence(&sdcard.dev, &segments[0]);
// Block pending completion of SPI access
spiWait(&sdcard.dev);
return (idleByte == 0xff);
}
/**
@ -151,15 +192,25 @@ static bool sdcard_waitForIdle(int maxBytesToWait)
*/
static uint8_t sdcard_waitForNonIdleByte(int maxDelay)
{
for (int i = 0; i < maxDelay + 1; i++) { // + 1 so we can wait for maxDelay '0xFF' bytes before reading a response byte afterwards
uint8_t response = spiBusTransferByte(&sdcard.busdev, 0xFF);
uint8_t idleByte;
if (response != 0xFF) {
return response;
}
}
// Note that this does not release the CS at the end of the transaction
busSegment_t segments[] = {
{NULL, &idleByte, sizeof (idleByte), false, sdcard_callbackNotIdle},
{NULL, NULL, 0, false, NULL},
};
return 0xFF;
sdcard.idleCount = maxDelay;
// Ensure any prior DMA has completed before continuing
spiWaitClaim(&sdcard.dev);
spiSequence(&sdcard.dev, &segments[0]);
// Block pending completion of SPI access
spiWait(&sdcard.dev);
return idleByte;
}
/**
@ -173,7 +224,7 @@ static uint8_t sdcard_waitForNonIdleByte(int maxDelay)
*/
static uint8_t sdcard_sendCommand(uint8_t commandCode, uint32_t commandArgument)
{
const uint8_t command[6] = {
uint8_t command[6] = {
0x40 | commandCode,
commandArgument >> 24,
commandArgument >> 16,
@ -183,17 +234,29 @@ static uint8_t sdcard_sendCommand(uint8_t commandCode, uint32_t commandArgument)
commands that require a CRC */
};
// Go ahead and send the command even if the card isn't idle if this is the reset command
uint8_t idleByte;
// Note that this does not release the CS at the end of the transaction
busSegment_t segments[] = {
{command, NULL, sizeof (command), false, NULL},
{NULL, &idleByte, sizeof (idleByte), false, sdcard_callbackNotIdle},
{NULL, NULL, 0, false, NULL},
};
if (!sdcard_waitForIdle(SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY) && commandCode != SDCARD_COMMAND_GO_IDLE_STATE)
return 0xFF;
spiBusRawTransfer(&sdcard.busdev, command, NULL, sizeof(command));
sdcard.idleCount = SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY;
/*
* The card can take up to SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY bytes to send the response, in the meantime
* it'll transmit 0xFF filler bytes.
*/
return sdcard_waitForNonIdleByte(SDCARD_MAXIMUM_BYTE_DELAY_FOR_CMD_REPLY);
// Ensure any prior DMA has completed before continuing
spiWaitClaim(&sdcard.dev);
spiSequence(&sdcard.dev, &segments[0]);
// Block pending completion of SPI access
spiWait(&sdcard.dev);
return idleByte;
}
static uint8_t sdcard_sendAppCommand(uint8_t commandCode, uint32_t commandArgument)
@ -223,7 +286,16 @@ static bool sdcard_validateInterfaceCondition(void)
// V1 cards don't support this command
sdcard.version = 1;
} else if (status == SDCARD_R1_STATUS_BIT_IDLE) {
spiBusRawTransfer(&sdcard.busdev, NULL, ifCondReply, sizeof(ifCondReply));
// Note that this does not release the CS at the end of the transaction
busSegment_t segments[] = {
{NULL, ifCondReply, sizeof (ifCondReply), false, NULL},
{NULL, NULL, 0, false, NULL},
};
spiSequence(&sdcard.dev, &segments[0]);
// Block pending completion of SPI access
spiWait(&sdcard.dev);
/*
* We don't bother to validate the SDCard's operating voltage range since the spec requires it to accept our
@ -247,7 +319,16 @@ static bool sdcard_readOCRRegister(uint32_t *result)
uint8_t response[4];
spiBusRawTransfer(&sdcard.busdev, NULL, response, sizeof(response));
// Note that this does not release the CS at the end of the transaction
busSegment_t segments[] = {
{NULL, response, sizeof (response), false, NULL},
{NULL, NULL, 0, false, NULL},
};
spiSequence(&sdcard.dev, &segments[0]);
// Block pending completion of SPI access
spiWait(&sdcard.dev);
if (status == 0) {
sdcard_deselect();
@ -285,30 +366,43 @@ static sdcardReceiveBlockStatus_e sdcard_receiveDataBlock(uint8_t *buffer, int c
return SDCARD_RECEIVE_ERROR;
}
spiBusRawTransfer(&sdcard.busdev, NULL, buffer, count);
// Note that this does not release the CS at the end of the transaction
busSegment_t segments[] = {
{NULL, buffer, count, false, NULL},
// Discard trailing CRC, we don't care
spiBusTransferByte(&sdcard.busdev, 0xFF);
spiBusTransferByte(&sdcard.busdev, 0xFF);
{NULL, NULL, 2, false, NULL},
{NULL, NULL, 0, false, NULL},
};
// Ensure any prior DMA has completed before continuing
spiWaitClaim(&sdcard.dev);
spiSequence(&sdcard.dev, &segments[0]);
// Block pending completion of SPI access
spiWait(&sdcard.dev);
return SDCARD_RECEIVE_SUCCESS;
}
static bool sdcard_sendDataBlockFinish(void)
{
#ifdef USE_HAL_DRIVER
// Drain anything left in the Rx FIFO (we didn't read it during the write)
// This is necessary here as when using msc there is timing issue
while (CHECK_SPI_RX_DATA_AVAILABLE(sdcard.busdev.busdev_u.spi.instance)) {
SPI_RX_DATA_REGISTER(sdcard.busdev.busdev_u.spi.instance);
}
#endif
uint16_t dummyCRC = 0;
uint8_t dataResponseToken;
// Note that this does not release the CS at the end of the transaction
busSegment_t segments[] = {
{(uint8_t *)&dummyCRC, NULL, sizeof (dummyCRC), false, NULL},
{NULL, &dataResponseToken, sizeof (dataResponseToken), false, NULL},
{NULL, NULL, 0, false, NULL},
};
// Send a dummy CRC
spiBusTransferByte(&sdcard.busdev, 0x00);
spiBusTransferByte(&sdcard.busdev, 0x00);
// Ensure any prior DMA has completed before continuing
spiWaitClaim(&sdcard.dev);
uint8_t dataResponseToken = spiBusTransferByte(&sdcard.busdev, 0xFF);
spiSequence(&sdcard.dev, &segments[0]);
// Block pending completion of SPI access
spiWait(&sdcard.dev);
/*
* Check if the card accepted the write (no CRC error / no address error)
@ -328,85 +422,30 @@ static bool sdcard_sendDataBlockFinish(void)
/**
* Begin sending a buffer of SDCARD_BLOCK_SIZE bytes to the SD card.
*/
static void sdcard_sendDataBlockBegin(const uint8_t *buffer, bool multiBlockWrite)
static void sdcard_sendDataBlockBegin(uint8_t *buffer, bool multiBlockWrite)
{
// Card wants 8 dummy clock cycles between the write command's response and a data block beginning:
spiBusTransferByte(&sdcard.busdev, 0xFF);
static uint8_t token;
spiBusTransferByte(&sdcard.busdev, multiBlockWrite ? SDCARD_MULTIPLE_BLOCK_WRITE_START_TOKEN : SDCARD_SINGLE_BLOCK_WRITE_START_TOKEN);
token = multiBlockWrite ? SDCARD_MULTIPLE_BLOCK_WRITE_START_TOKEN : SDCARD_SINGLE_BLOCK_WRITE_START_TOKEN;
if (sdcard.useDMAForTx) {
#if defined(USE_HAL_DRIVER)
LL_DMA_InitTypeDef init;
// Note that this does not release the CS at the end of the transaction
static busSegment_t segments[] = {
// Write a single 0xff
{NULL, NULL, 1, false, NULL},
{&token, NULL, sizeof (token), false, NULL},
{NULL, NULL, 0, false, NULL},
{NULL, NULL, 0, false, NULL},
};
LL_DMA_StructInit(&init);
segments[2].txData = buffer;
segments[2].len = spiUseDMA(&sdcard.dev) ? SDCARD_BLOCK_SIZE : SDCARD_NON_DMA_CHUNK_SIZE;
#if defined(STM32G4) || defined(STM32H7)
init.PeriphRequest = dmaGetChannel(sdcard.dmaChannel);
#else
init.Channel = dmaGetChannel(sdcard.dmaChannel);
#endif
init.Mode = LL_DMA_MODE_NORMAL;
init.Direction = LL_DMA_DIRECTION_MEMORY_TO_PERIPH;
// Ensure any prior DMA has completed before continuing
spiWaitClaim(&sdcard.dev);
init.PeriphOrM2MSrcAddress = (uint32_t)&SPI_RX_DATA_REGISTER(sdcard.busdev.busdev_u.spi.instance);
init.Priority = LL_DMA_PRIORITY_LOW;
init.PeriphOrM2MSrcIncMode = LL_DMA_PERIPH_NOINCREMENT;
init.PeriphOrM2MSrcDataSize = LL_DMA_PDATAALIGN_BYTE;
spiSequence(&sdcard.dev, &segments[0]);
init.MemoryOrM2MDstAddress = (uint32_t)buffer;
init.MemoryOrM2MDstIncMode = LL_DMA_MEMORY_INCREMENT;
init.MemoryOrM2MDstDataSize = LL_DMA_MDATAALIGN_BYTE;
init.NbData = SDCARD_BLOCK_SIZE;
LL_DMA_DeInit(sdcard.dma->dma, sdcard.dma->stream);
LL_DMA_Init(sdcard.dma->dma, sdcard.dma->stream, &init);
#if defined(STM32G4)
LL_DMA_EnableChannel(sdcard.dma->dma, sdcard.dma->stream);
#else
LL_DMA_EnableStream(sdcard.dma->dma, sdcard.dma->stream);
#endif
LL_SPI_EnableDMAReq_TX(sdcard.busdev.busdev_u.spi.instance);
#else
DMA_InitTypeDef init;
DMA_StructInit(&init);
#ifdef STM32F4
init.DMA_Channel = dmaGetChannel(sdcard.dmaChannel);
init.DMA_Memory0BaseAddr = (uint32_t) buffer;
init.DMA_DIR = DMA_DIR_MemoryToPeripheral;
#else
init.DMA_M2M = DMA_M2M_Disable;
init.DMA_MemoryBaseAddr = (uint32_t) buffer;
init.DMA_DIR = DMA_DIR_PeripheralDST;
#endif
init.DMA_PeripheralBaseAddr = (uint32_t)&SPI_RX_DATA_REGISTER(sdcard.busdev.busdev_u.spi.instance);
init.DMA_Priority = DMA_Priority_Low;
init.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
init.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
init.DMA_MemoryInc = DMA_MemoryInc_Enable;
init.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
init.DMA_BufferSize = SDCARD_BLOCK_SIZE;
init.DMA_Mode = DMA_Mode_Normal;
xDMA_DeInit(sdcard.dma->ref);
xDMA_Init(sdcard.dma->ref, &init);
xDMA_Cmd(sdcard.dma->ref, ENABLE);
SPI_I2S_DMACmd(sdcard.busdev.busdev_u.spi.instance, SPI_I2S_DMAReq_Tx, ENABLE);
#endif
} else {
// Send the first chunk now
spiBusRawTransfer(&sdcard.busdev, buffer, NULL, SDCARD_NON_DMA_CHUNK_SIZE);
}
// Don't block pending completion of SPI access
}
static bool sdcard_receiveCID(void)
@ -502,9 +541,7 @@ void sdcardSpi_preInit(const sdcardConfig_t *config)
*/
static void sdcardSpi_init(const sdcardConfig_t *config, const spiPinConfig_t *spiConfig)
{
#ifndef USE_DMA_SPEC
UNUSED(spiConfig);
#endif
sdcard.enabled = config->mode;
if (!sdcard.enabled) {
@ -512,32 +549,7 @@ static void sdcardSpi_init(const sdcardConfig_t *config, const spiPinConfig_t *s
return;
}
SPIDevice spiDevice = SPI_CFG_TO_DEV(config->device);
spiBusSetInstance(&sdcard.busdev, spiInstanceByDevice(spiDevice));
if (config->useDma) {
dmaIdentifier_e dmaIdentifier = DMA_NONE;
#ifdef USE_DMA_SPEC
const dmaChannelSpec_t *dmaChannelSpec = dmaGetChannelSpecByPeripheral(DMA_PERIPH_SPI_TX, config->device, spiConfig[spiDevice].txDmaopt);
if (dmaChannelSpec) {
dmaIdentifier = dmaGetIdentifier(dmaChannelSpec->ref);
sdcard.dmaChannel = dmaChannelSpec->channel; // XXX STM32F3 doesn't have this
}
#else
dmaIdentifier = config->dmaIdentifier;
#endif
if (dmaIdentifier) {
sdcard.dma = dmaGetDescriptorByIdentifier(dmaIdentifier);
dmaInit(dmaIdentifier, OWNER_SDCARD, 0);
sdcard.useDMAForTx = true;
} else {
sdcard.useDMAForTx = false;
}
}
spiSetBusInstance(&sdcard.dev, config->device, OWNER_SDCARD_CS);
IO_t chipSelectIO;
if (config->chipSelectTag) {
@ -547,32 +559,38 @@ static void sdcardSpi_init(const sdcardConfig_t *config, const spiPinConfig_t *s
} else {
chipSelectIO = IO_NONE;
}
sdcard.busdev.busdev_u.spi.csnPin = chipSelectIO;
sdcard.dev.busType_u.spi.csnPin = chipSelectIO;
// Set the clock phase/polarity
spiSetClkPhasePolarity(&sdcard.dev, true);
// Set the callback argument when calling back to this driver for DMA completion
sdcard.dev.callbackArg = (uint32_t)&sdcard;
// Max frequency is initially 400kHz
#ifdef USE_SPI_TRANSACTION
spiBusTransactionInit(&sdcard.busdev, SDCARD_SPI_MODE, spiCalculateDivider(SDCARD_MAX_SPI_INIT_CLK_HZ));
#else
spiSetDivisor(sdcard.busdev.busdev_u.spi.instance, spiCalculateDivider(SDCARD_MAX_SPI_INIT_CLK_HZ));
#endif
spiSetClkDivisor(&sdcard.dev, spiCalculateDivider(SDCARD_MAX_SPI_INIT_CLK_HZ));
// SDCard wants 1ms minimum delay after power is applied to it
delay(1000);
// Transmit at least 74 dummy clock cycles with CS high so the SD card can start up
IOHi(sdcard.busdev.busdev_u.spi.csnPin);
spiBusRawTransfer(&sdcard.busdev, NULL, NULL, SDCARD_INIT_NUM_DUMMY_BYTES);
IOHi(sdcard.dev.busType_u.spi.csnPin);
// Wait for that transmission to finish before we enable the SDCard, so it receives the required number of cycles:
int time = 100000;
while (spiBusIsBusBusy(&sdcard.busdev)) {
if (time-- == 0) {
sdcard.state = SDCARD_STATE_NOT_PRESENT;
sdcard.failureCount++;
return;
}
}
// Note that this does not release the CS at the end of the transaction
busSegment_t segments[] = {
// Write a single 0xff
{NULL, NULL, SDCARD_INIT_NUM_DUMMY_BYTES, false, NULL},
{NULL, NULL, 0, false, NULL},
};
// Ensure any prior DMA has completed before continuing
spiWaitClaim(&sdcard.dev);
spiSequence(&sdcard.dev, &segments[0]);
// Block pending completion of SPI access
spiWait(&sdcard.dev);
sdcard.operationStartTime = millis();
sdcard.state = SDCARD_STATE_RESET;
@ -610,12 +628,24 @@ static bool sdcard_isReady(void)
*/
static sdcardOperationStatus_e sdcard_endWriteBlocks(void)
{
uint8_t token = SDCARD_MULTIPLE_BLOCK_WRITE_STOP_TOKEN;
sdcard.multiWriteBlocksRemain = 0;
// Note that this does not release the CS at the end of the transaction
busSegment_t segments[] = {
// 8 dummy clocks to guarantee N_WR clocks between the last card response and this token
spiBusTransferByte(&sdcard.busdev, 0xFF);
{NULL, NULL, 1, false, NULL},
{&token, NULL, sizeof(token), false, NULL},
{NULL, NULL, 0, false, NULL},
};
spiBusTransferByte(&sdcard.busdev, SDCARD_MULTIPLE_BLOCK_WRITE_STOP_TOKEN);
// Ensure any prior DMA has completed before continuing
spiWaitClaim(&sdcard.dev);
spiSequence(&sdcard.dev, &segments[0]);
// Block pending completion of SPI access
spiWait(&sdcard.dev);
// Card may choose to raise a busy (non-0xFF) signal after at most N_BR (1 byte) delay
if (sdcard_waitForNonIdleByte(1) == 0xFF) {
@ -720,11 +750,7 @@ static bool sdcardSpi_poll(void)
// Now we're done with init and we can switch to the full speed clock (<25MHz)
#ifdef USE_SPI_TRANSACTION
spiBusTransactionInit(&sdcard.busdev, SDCARD_SPI_MODE, spiCalculateDivider(SDCARD_MAX_SPI_CLK_HZ));
#else
spiSetDivisor(sdcard.busdev.busdev_u.spi.instance, spiCalculateDivider(SDCARD_MAX_SPI_CLK_HZ));
#endif
spiSetClkDivisor(&sdcard.dev, spiCalculateDivider(SDCARD_MAX_SPI_CLK_HZ));
sdcard.multiWriteBlocksRemain = 0;
@ -734,54 +760,11 @@ static bool sdcardSpi_poll(void)
break;
case SDCARD_STATE_SENDING_WRITE:
// Have we finished sending the write yet?
sendComplete = false;
sendComplete = !spiIsBusy(&sdcard.dev);
#if defined(USE_HAL_DRIVER)
if (sdcard.useDMAForTx && DMA_GET_FLAG_STATUS(sdcard.dma, DMA_IT_TCIF)) {
//Clear both flags after transfer
DMA_CLEAR_FLAG(sdcard.dma, DMA_IT_TCIF);
DMA_CLEAR_FLAG(sdcard.dma, DMA_IT_HTIF);
// Drain anything left in the Rx FIFO (we didn't read it during the write)
while (CHECK_SPI_RX_DATA_AVAILABLE(sdcard.busdev.busdev_u.spi.instance)) {
SPI_RX_DATA_REGISTER(sdcard.busdev.busdev_u.spi.instance);
}
// Wait for the final bit to be transmitted
while (spiBusIsBusBusy(&sdcard.busdev)) {
}
LL_SPI_DisableDMAReq_TX(sdcard.busdev.busdev_u.spi.instance);
sendComplete = true;
}
#else
#ifdef STM32F4
if (sdcard.useDMAForTx && xDMA_GetFlagStatus(sdcard.dma->ref, sdcard.dma->completeFlag) == SET) {
xDMA_ClearFlag(sdcard.dma->ref, sdcard.dma->completeFlag);
#else
if (sdcard.useDMAForTx && DMA_GetFlagStatus(sdcard.dma->completeFlag) == SET) {
DMA_ClearFlag(sdcard.dma->completeFlag);
#endif
xDMA_Cmd(sdcard.dma->ref, DISABLE);
// Drain anything left in the Rx FIFO (we didn't read it during the write)
while (SPI_I2S_GetFlagStatus(sdcard.busdev.busdev_u.spi.instance, SPI_I2S_FLAG_RXNE) == SET) {
SPI_RX_DATA_REGISTER(sdcard.busdev.busdev_u.spi.instance);
}
// Wait for the final bit to be transmitted
while (spiBusIsBusBusy(&sdcard.busdev)) {
}
SPI_I2S_DMACmd(sdcard.busdev.busdev_u.spi.instance, SPI_I2S_DMAReq_Tx, DISABLE);
sendComplete = true;
}
#endif
if (!sdcard.useDMAForTx) {
if (!spiUseDMA(&sdcard.dev)) {
// Send another chunk
spiBusRawTransfer(&sdcard.busdev, sdcard.pendingOperation.buffer + SDCARD_NON_DMA_CHUNK_SIZE * sdcard.pendingOperation.chunkIndex, NULL, SDCARD_NON_DMA_CHUNK_SIZE);
spiReadWriteBuf(&sdcard.dev, sdcard.pendingOperation.buffer + SDCARD_NON_DMA_CHUNK_SIZE * sdcard.pendingOperation.chunkIndex, NULL, SDCARD_NON_DMA_CHUNK_SIZE);
sdcard.pendingOperation.chunkIndex++;

14
src/main/drivers/system.c
View File

@ -273,6 +273,20 @@ void initialiseMemorySections(void)
#endif
}
#ifdef STM32H7
void initialiseD2MemorySections(void)
{
/* Load DMA_DATA variable intializers into D2 RAM */
extern uint8_t _sdmaram_bss;
extern uint8_t _edmaram_bss;
extern uint8_t _sdmaram_data;
extern uint8_t _edmaram_data;
extern uint8_t _sdmaram_idata;
bzero(&_sdmaram_bss, (size_t) (&_edmaram_bss - &_sdmaram_bss));
memcpy(&_sdmaram_data, &_sdmaram_idata, (size_t) (&_edmaram_data - &_sdmaram_data));
}
#endif
static void unusedPinInit(IO_t io)
{
if (IOGetOwner(io) == OWNER_FREE) {

3
src/main/drivers/system.h
View File

@ -72,6 +72,9 @@ void systemCheckResetReason(void);
#endif
void initialiseMemorySections(void);
#ifdef STM32H7
void initialiseD2MemorySections(void);
#endif
void enableGPIOPowerUsageAndNoiseReductions(void);
// current crystal frequency - 8 or 12MHz

42
src/main/drivers/vtx_rtc6705.c
View File

@ -56,10 +56,10 @@
static IO_t vtxPowerPin = IO_NONE;
#endif
static busDevice_t *busdev = NULL;
static extDevice_t *dev = NULL;
#define DISABLE_RTC6705() IOHi(busdev->busdev_u.spi.csnPin)
#define ENABLE_RTC6705() IOLo(busdev->busdev_u.spi.csnPin)
#define DISABLE_RTC6705() IOHi(dev->busType_u.spi.csnPin)
#define ENABLE_RTC6705() IOLo(dev->busType_u.spi.csnPin)
#define DP_5G_MASK 0x7000 // b111000000000000
#define PA5G_BS_MASK 0x0E00 // b000111000000000
@ -94,7 +94,7 @@ static uint32_t reverse32(uint32_t in)
*/
bool rtc6705IOInit(const vtxIOConfig_t *vtxIOConfig)
{
static busDevice_t busInstance;
static extDevice_t devInstance;
IO_t csnPin = IOGetByTag(vtxIOConfig->csTag);
if (!csnPin) {
@ -110,20 +110,14 @@ bool rtc6705IOInit(const vtxIOConfig_t *vtxIOConfig)
IOConfigGPIO(vtxPowerPin, IOCFG_OUT_PP);
}
SPI_TypeDef *vtxSpiInstance = spiInstanceByDevice(SPI_CFG_TO_DEV(vtxIOConfig->spiDevice));
if (vtxSpiInstance) {
busdev = &busInstance;
busdev->busdev_u.spi.csnPin = csnPin;
IOInit(busdev->busdev_u.spi.csnPin, OWNER_VTX_CS, 0);
if (vtxIOConfig->csTag && spiSetBusInstance(dev, vtxIOConfig->spiDevice, OWNER_VTX_CS)) {
devInstance.busType_u.spi.csnPin = csnPin;
IOInit(devInstance.busType_u.spi.csnPin, OWNER_VTX_CS, 0);
DISABLE_RTC6705();
// GPIO bit is enabled so here so the output is not pulled low when the GPIO is set in output mode.
// Note: It's critical to ensure that incorrect signals are not sent to the VTX.
IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP);
busdev->bustype = BUSTYPE_SPI;
spiBusSetInstance(busdev, vtxSpiInstance);
IOConfigGPIO(devInstance.busType_u.spi.csnPin, IOCFG_OUT_PP);
return true;
#if defined(USE_VTX_RTC6705_SOFTSPI)
@ -142,18 +136,10 @@ bool rtc6705IOInit(const vtxIOConfig_t *vtxIOConfig)
*/
static void rtc6705Transfer(uint32_t command)
{
// Perform bitwise reverse of the command.
command = reverse32(command);
ENABLE_RTC6705();
spiTransferByte(busdev->busdev_u.spi.instance, (command >> 24) & 0xFF);
spiTransferByte(busdev->busdev_u.spi.instance, (command >> 16) & 0xFF);
spiTransferByte(busdev->busdev_u.spi.instance, (command >> 8) & 0xFF);
spiTransferByte(busdev->busdev_u.spi.instance, (command >> 0) & 0xFF);
delayMicroseconds(2);
DISABLE_RTC6705();
spiReadWriteBuf(dev, (uint8_t *)&command, NULL, sizeof (command));
delayMicroseconds(2);
}
@ -165,7 +151,7 @@ static void rtc6705Transfer(uint32_t command)
void rtc6705SetFrequency(uint16_t frequency)
{
#if defined(USE_VTX_RTC6705_SOFTSPI)
if (!busdev) {
if (!dev) {
rtc6705SoftSpiSetFrequency(frequency);
return;
@ -181,7 +167,7 @@ void rtc6705SetFrequency(uint16_t frequency)
val_hex |= (val_a << 5);
val_hex |= (val_n << 12);
spiSetDivisor(busdev->busdev_u.spi.instance, spiCalculateDivider(RTC6705_MAX_SPI_CLK_HZ));
spiSetClkDivisor(dev, spiCalculateDivider(RTC6705_MAX_SPI_CLK_HZ));
rtc6705Transfer(RTC6705_SET_HEAD);
delayMicroseconds(10);
@ -192,7 +178,7 @@ void rtc6705SetRFPower(uint8_t rf_power)
{
rf_power = constrain(rf_power, 1, 2);
#if defined(USE_VTX_RTC6705_SOFTSPI)
if (!busdev) {
if (!dev) {
rtc6705SoftSpiSetRFPower(rf_power);
return;
@ -204,7 +190,7 @@ void rtc6705SetRFPower(uint8_t rf_power)
const uint32_t data = rf_power > 1 ? PA_CONTROL_DEFAULT : (PA_CONTROL_DEFAULT | PD_Q5G_MASK) & (~(PA5G_PW_MASK | PA5G_BS_MASK));
val_hex |= data << 5; // 4 address bits and 1 rw bit.
spiSetDivisor(busdev->busdev_u.spi.instance, spiCalculateDivider(RTC6705_MAX_SPI_CLK_HZ));
spiSetClkDivisor(dev, spiCalculateDivider(RTC6705_MAX_SPI_CLK_HZ));
rtc6705Transfer(val_hex);
}

64
src/main/fc/init.c
View File

@ -208,40 +208,6 @@ static IO_t busSwitchResetPin = IO_NONE;
}
#endif
bool requiresSpiLeadingEdge(SPIDevice device)
{
#if defined(CONFIG_IN_SDCARD) || defined(CONFIG_IN_EXTERNAL_FLASH)
#if !defined(SDCARD_SPI_INSTANCE) && !defined(RX_SPI_INSTANCE)
UNUSED(device);
#endif
#if defined(SDCARD_SPI_INSTANCE)
if (device == spiDeviceByInstance(SDCARD_SPI_INSTANCE)) {
return true;
}
#endif
#if defined(RX_SPI_INSTANCE)
if (device == spiDeviceByInstance(RX_SPI_INSTANCE)) {
return true;
}
#endif
#else
#if !defined(USE_SDCARD) && !defined(USE_RX_SPI)
UNUSED(device);
#endif
#if defined(USE_SDCARD)
if (device == SPI_CFG_TO_DEV(sdcardConfig()->device)) {
return true;
}
#endif
#if defined(USE_RX_SPI)
if (device == SPI_CFG_TO_DEV(rxSpiConfig()->spibus)) {
return true;
}
#endif
#endif // CONFIG_IN_SDCARD || CONFIG_IN_EXTERNAL_FLASH
return false;
}
static void configureSPIAndQuadSPI(void)
{
@ -255,22 +221,22 @@ static void configureSPIAndQuadSPI(void)
spiPreinit();
#ifdef USE_SPI_DEVICE_1
spiInit(SPIDEV_1, requiresSpiLeadingEdge(SPIDEV_1));
spiInit(SPIDEV_1);
#endif
#ifdef USE_SPI_DEVICE_2
spiInit(SPIDEV_2, requiresSpiLeadingEdge(SPIDEV_2));
spiInit(SPIDEV_2);
#endif
#ifdef USE_SPI_DEVICE_3
spiInit(SPIDEV_3, requiresSpiLeadingEdge(SPIDEV_3));
spiInit(SPIDEV_3);
#endif
#ifdef USE_SPI_DEVICE_4
spiInit(SPIDEV_4, requiresSpiLeadingEdge(SPIDEV_4));
spiInit(SPIDEV_4);
#endif
#ifdef USE_SPI_DEVICE_5
spiInit(SPIDEV_5, requiresSpiLeadingEdge(SPIDEV_5));
spiInit(SPIDEV_5);
#endif
#ifdef USE_SPI_DEVICE_6
spiInit(SPIDEV_6, requiresSpiLeadingEdge(SPIDEV_6));
spiInit(SPIDEV_6);
#endif
#endif // USE_SPI
@ -318,6 +284,11 @@ void init(void)
detectHardwareRevision();
#endif
#if defined(USE_TARGET_CONFIG)
// Call once before the config is loaded for any target specific configuration required to support loading the config
targetConfiguration();
#endif
#ifdef USE_BRUSHED_ESC_AUTODETECT
// Opportunistically use the first motor pin of the default configuration for detection.
// We are doing this as with some boards, timing seems to be important, and the later detection will fail.
@ -362,10 +333,6 @@ void init(void)
pgResetAll();
#if defined(STM32H7) && defined(USE_SDCARD_SDIO) // H7 only for now, likely should be applied to F4/F7 too
sdioPinConfigure();
SDIO_GPIO_Init();
#endif
#ifdef USE_SDCARD_SPI
configureSPIAndQuadSPI();
initFlags |= SPI_AND_QSPI_INIT_ATTEMPTED;
@ -656,6 +623,10 @@ void init(void)
if (blackboxConfig()->device == BLACKBOX_DEVICE_FLASH) {
emfat_init_files();
}
#endif
// There's no more initialisation to be done, so enable DMA where possible for SPI
#ifdef USE_SPI
spiInitBusDMA();
#endif
if (mscStart() == 0) {
mscWaitForButton();
@ -1020,6 +991,11 @@ void init(void)
motorEnable();
#endif
#ifdef USE_SPI
// Attempt to enable DMA on all SPI busses
spiInitBusDMA();
#endif
swdPinsInit();
unusedPinsInit();

2
src/main/io/asyncfatfs/asyncfatfs.c
View File

@ -514,7 +514,7 @@ typedef struct afatfs_t {
} afatfs_t;
#ifdef STM32H7
static DMA_RW_AXI uint8_t afatfs_cache[AFATFS_SECTOR_SIZE * AFATFS_NUM_CACHE_SECTORS] __attribute__((aligned(32)));
static DMA_DATA_ZERO_INIT uint8_t afatfs_cache[AFATFS_SECTOR_SIZE * AFATFS_NUM_CACHE_SECTORS] __attribute__((aligned(32)));
#endif
static afatfs_t afatfs;

179
src/main/io/dashboard.c
View File

@ -81,7 +81,7 @@
#define DISPLAY_UPDATE_FREQUENCY (MICROSECONDS_IN_A_SECOND / 5)
#define PAGE_CYCLE_FREQUENCY (MICROSECONDS_IN_A_SECOND * 5)
static busDevice_t *bus;
static extDevice_t *dev;
static uint32_t nextDisplayUpdateAt = 0;
static bool dashboardPresent = false;
@ -128,12 +128,12 @@ static pageState_t pageState;
static void resetDisplay(void)
{
dashboardPresent = ug2864hsweg01InitI2C(bus);
dashboardPresent = ug2864hsweg01InitI2C(dev);
}
void LCDprint(uint8_t i)
{
i2c_OLED_send_char(bus, i);
i2c_OLED_send_char(dev, i);
}
static void padLineBuffer(void)
@ -182,8 +182,8 @@ static void fillScreenWithCharacters(void)
{
for (uint8_t row = 0; row < SCREEN_CHARACTER_ROW_COUNT; row++) {
for (uint8_t column = 0; column < SCREEN_CHARACTER_COLUMN_COUNT; column++) {
i2c_OLED_set_xy(bus, column, row);
i2c_OLED_send_char(bus, 'A' + column);
i2c_OLED_set_xy(dev, column, row);
i2c_OLED_send_char(dev, 'A' + column);
}
}
}
@ -193,22 +193,22 @@ static void fillScreenWithCharacters(void)
static void updateTicker(void)
{
static uint8_t tickerIndex = 0;
i2c_OLED_set_xy(bus, SCREEN_CHARACTER_COLUMN_COUNT - 1, 0);
i2c_OLED_send_char(bus, tickerCharacters[tickerIndex]);
i2c_OLED_set_xy(dev, SCREEN_CHARACTER_COLUMN_COUNT - 1, 0);
i2c_OLED_send_char(dev, tickerCharacters[tickerIndex]);
tickerIndex++;
tickerIndex = tickerIndex % TICKER_CHARACTER_COUNT;
}
static void updateRxStatus(void)
{
i2c_OLED_set_xy(bus, SCREEN_CHARACTER_COLUMN_COUNT - 2, 0);
i2c_OLED_set_xy(dev, SCREEN_CHARACTER_COLUMN_COUNT - 2, 0);
char rxStatus = '!';
if (rxIsReceivingSignal()) {
rxStatus = 'r';
} if (rxAreFlightChannelsValid()) {
rxStatus = 'R';
}
i2c_OLED_send_char(bus, rxStatus);
i2c_OLED_send_char(dev, rxStatus);
}
static void updateFailsafeStatus(void)
@ -237,19 +237,19 @@ static void updateFailsafeStatus(void)
failsafeIndicator = 'G';
break;
}
i2c_OLED_set_xy(bus, SCREEN_CHARACTER_COLUMN_COUNT - 3, 0);
i2c_OLED_send_char(bus, failsafeIndicator);
i2c_OLED_set_xy(dev, SCREEN_CHARACTER_COLUMN_COUNT - 3, 0);
i2c_OLED_send_char(dev, failsafeIndicator);
}
static void showTitle(void)
{
i2c_OLED_set_line(bus, 0);
i2c_OLED_send_string(bus, pageState.page->title);
i2c_OLED_set_line(dev, 0);
i2c_OLED_send_string(dev, pageState.page->title);
}
static void handlePageChange(void)
{
i2c_OLED_clear_display_quick(bus);
i2c_OLED_clear_display_quick(dev);
showTitle();
}
@ -265,7 +265,7 @@ static void drawRxChannel(uint8_t channelIndex, uint8_t width)
static void showRxPage(void)
{
for (int channelIndex = 0; channelIndex < rxRuntimeState.channelCount && channelIndex < RX_CHANNELS_PER_PAGE_COUNT; channelIndex += 2) {
i2c_OLED_set_line(bus, (channelIndex / 2) + PAGE_TITLE_LINE_COUNT);
i2c_OLED_set_line(dev, (channelIndex / 2) + PAGE_TITLE_LINE_COUNT);
drawRxChannel(channelIndex, HALF_SCREEN_CHARACTER_COLUMN_COUNT);
@ -286,11 +286,11 @@ static void showWelcomePage(void)
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
tfp_sprintf(lineBuffer, "v%s (%s)", FC_VERSION_STRING, shortGitRevision);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
i2c_OLED_set_line(dev, rowIndex++);
i2c_OLED_send_string(dev, lineBuffer);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, targetName);
i2c_OLED_set_line(dev, rowIndex++);
i2c_OLED_send_string(dev, targetName);
}
static void showArmedPage(void)
@ -302,8 +302,8 @@ static void showProfilePage(void)
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
tfp_sprintf(lineBuffer, "Profile: %d", getCurrentPidProfileIndex());
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
i2c_OLED_set_line(dev, rowIndex++);
i2c_OLED_send_string(dev, lineBuffer);
static const char* const axisTitles[3] = {"ROL", "PIT", "YAW"};
const pidProfile_t *pidProfile = currentPidProfile;
@ -315,8 +315,8 @@ static void showProfilePage(void)
pidProfile->pid[axis].D
);
padLineBuffer();
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
i2c_OLED_set_line(dev, rowIndex++);
i2c_OLED_send_string(dev, lineBuffer);
}
}
@ -326,15 +326,15 @@ static void showRateProfilePage(void)
const uint8_t currentRateProfileIndex = getCurrentControlRateProfileIndex();
tfp_sprintf(lineBuffer, "Rate profile: %d", currentRateProfileIndex);
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
i2c_OLED_set_line(dev, rowIndex++);
i2c_OLED_send_string(dev, lineBuffer);
const controlRateConfig_t *controlRateConfig = controlRateProfiles(currentRateProfileIndex);
tfp_sprintf(lineBuffer, " R P Y");
padLineBuffer();
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
i2c_OLED_set_line(dev, rowIndex++);
i2c_OLED_send_string(dev, lineBuffer);
tfp_sprintf(lineBuffer, "RcRate %3d %3d %3d",
controlRateConfig->rcRates[FD_ROLL],
@ -342,8 +342,8 @@ static void showRateProfilePage(void)
controlRateConfig->rcRates[FD_YAW]
);
padLineBuffer();
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
i2c_OLED_set_line(dev, rowIndex++);
i2c_OLED_send_string(dev, lineBuffer);
tfp_sprintf(lineBuffer, "Super %3d %3d %3d",
controlRateConfig->rates[FD_ROLL],
@ -351,8 +351,8 @@ static void showRateProfilePage(void)
controlRateConfig->rates[FD_YAW]
);
padLineBuffer();
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
i2c_OLED_set_line(dev, rowIndex++);
i2c_OLED_send_string(dev, lineBuffer);
tfp_sprintf(lineBuffer, "Expo %3d %3d %3d",
controlRateConfig->rcExpo[FD_ROLL],
@ -360,8 +360,8 @@ static void showRateProfilePage(void)
controlRateConfig->rcExpo[FD_YAW]
);
padLineBuffer();
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
i2c_OLED_set_line(dev, rowIndex++);
i2c_OLED_send_string(dev, lineBuffer);
}
#define SATELLITE_COUNT (sizeof(GPS_svinfo_cno) / sizeof(GPS_svinfo_cno[0]))
@ -385,64 +385,64 @@ static void showGpsPage(void)
gpsTicker = gpsTicker % TICKER_CHARACTER_COUNT;
}
i2c_OLED_set_xy(bus, 0, rowIndex);
i2c_OLED_send_char(bus, tickerCharacters[gpsTicker]);
i2c_OLED_set_xy(dev, 0, rowIndex);
i2c_OLED_send_char(dev, tickerCharacters[gpsTicker]);
i2c_OLED_set_xy(bus, MAX(0, (uint8_t)SATELLITE_GRAPH_LEFT_OFFSET), rowIndex++);
i2c_OLED_set_xy(dev, MAX(0, (uint8_t)SATELLITE_GRAPH_LEFT_OFFSET), rowIndex++);
uint32_t index;
for (index = 0; index < SATELLITE_COUNT && index < SCREEN_CHARACTER_COLUMN_COUNT; index++) {
uint8_t bargraphOffset = ((uint16_t) GPS_svinfo_cno[index] * VERTICAL_BARGRAPH_CHARACTER_COUNT) / (GPS_DBHZ_MAX - 1);
bargraphOffset = MIN(bargraphOffset, VERTICAL_BARGRAPH_CHARACTER_COUNT - 1);
i2c_OLED_send_char(bus, VERTICAL_BARGRAPH_ZERO_CHARACTER + bargraphOffset);
i2c_OLED_send_char(dev, VERTICAL_BARGRAPH_ZERO_CHARACTER + bargraphOffset);
}
char fixChar = STATE(GPS_FIX) ? 'Y' : 'N';
tfp_sprintf(lineBuffer, "Sats: %d Fix: %c", gpsSol.numSat, fixChar);
padLineBuffer();
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
i2c_OLED_set_line(dev, rowIndex++);
i2c_OLED_send_string(dev, lineBuffer);
tfp_sprintf(lineBuffer, "La/Lo: %d/%d", gpsSol.llh.lat / GPS_DEGREES_DIVIDER, gpsSol.llh.lon / GPS_DEGREES_DIVIDER);
padLineBuffer();
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
i2c_OLED_set_line(dev, rowIndex++);
i2c_OLED_send_string(dev, lineBuffer);
tfp_sprintf(lineBuffer, "Spd: %d", gpsSol.groundSpeed);
padHalfLineBuffer();
i2c_OLED_set_line(bus, rowIndex);
i2c_OLED_send_string(bus, lineBuffer);
i2c_OLED_set_line(dev, rowIndex);
i2c_OLED_send_string(dev, lineBuffer);
tfp_sprintf(lineBuffer, "GC: %d", gpsSol.groundCourse);
padHalfLineBuffer();
i2c_OLED_set_xy(bus, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
i2c_OLED_set_xy(dev, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
i2c_OLED_send_string(dev, lineBuffer);
tfp_sprintf(lineBuffer, "RX: %d", GPS_packetCount);
padHalfLineBuffer();
i2c_OLED_set_line(bus, rowIndex);
i2c_OLED_send_string(bus, lineBuffer);
i2c_OLED_set_line(dev, rowIndex);
i2c_OLED_send_string(dev, lineBuffer);
tfp_sprintf(lineBuffer, "ERRs: %d", gpsData.errors);
padHalfLineBuffer();
i2c_OLED_set_xy(bus, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
i2c_OLED_set_xy(dev, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
i2c_OLED_send_string(dev, lineBuffer);
tfp_sprintf(lineBuffer, "Dt: %d", gpsData.lastMessage - gpsData.lastLastMessage);
padHalfLineBuffer();
i2c_OLED_set_line(bus, rowIndex);
i2c_OLED_send_string(bus, lineBuffer);
i2c_OLED_set_line(dev, rowIndex);
i2c_OLED_send_string(dev, lineBuffer);
tfp_sprintf(lineBuffer, "TOs: %d", gpsData.timeouts);
padHalfLineBuffer();
i2c_OLED_set_xy(bus, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
i2c_OLED_set_xy(dev, HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
i2c_OLED_send_string(dev, lineBuffer);
strncpy(lineBuffer, gpsPacketLog, GPS_PACKET_LOG_ENTRY_COUNT);
padHalfLineBuffer();
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
i2c_OLED_set_line(dev, rowIndex++);
i2c_OLED_send_string(dev, lineBuffer);
}
#endif
@ -453,11 +453,11 @@ static void showBatteryPage(void)
if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE) {
tfp_sprintf(lineBuffer, "Volts: %d.%02d Cells: %d", getBatteryVoltage() / 100, getBatteryVoltage() % 100, getBatteryCellCount());
padLineBuffer();
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
i2c_OLED_set_line(dev, rowIndex++);
i2c_OLED_send_string(dev, lineBuffer);
uint8_t batteryPercentage = calculateBatteryPercentageRemaining();
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_set_line(dev, rowIndex++);
drawHorizonalPercentageBar(SCREEN_CHARACTER_COLUMN_COUNT, batteryPercentage);
}
@ -468,11 +468,11 @@ static void showBatteryPage(void)
// Amp: DDD.D mAh: DDDDD
tfp_sprintf(lineBuffer, "Amp: %d.%d mAh: %d", amperage / 100, (amperage % 100) / 10, getMAhDrawn());
padLineBuffer();
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
i2c_OLED_set_line(dev, rowIndex++);
i2c_OLED_send_string(dev, lineBuffer);
uint8_t capacityPercentage = calculateBatteryPercentageRemaining();
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_set_line(dev, rowIndex++);
drawHorizonalPercentageBar(SCREEN_CHARACTER_COLUMN_COUNT, capacityPercentage);
}
}
@ -482,38 +482,38 @@ static void showSensorsPage(void)
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
static const char *format = "%s %5d %5d %5d";
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, " X Y Z");
i2c_OLED_set_line(dev, rowIndex++);
i2c_OLED_send_string(dev, " X Y Z");
#if defined(USE_ACC)
if (sensors(SENSOR_ACC)) {
tfp_sprintf(lineBuffer, format, "ACC", lrintf(acc.accADC[X]), lrintf(acc.accADC[Y]), lrintf(acc.accADC[Z]));
padLineBuffer();
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
i2c_OLED_set_line(dev, rowIndex++);
i2c_OLED_send_string(dev, lineBuffer);
}
#endif
if (sensors(SENSOR_GYRO)) {
tfp_sprintf(lineBuffer, format, "GYR", lrintf(gyro.gyroADCf[X]), lrintf(gyro.gyroADCf[Y]), lrintf(gyro.gyroADCf[Z]));
padLineBuffer();
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
i2c_OLED_set_line(dev, rowIndex++);
i2c_OLED_send_string(dev, lineBuffer);
}
#ifdef USE_MAG
if (sensors(SENSOR_MAG)) {
tfp_sprintf(lineBuffer, format, "MAG", lrintf(mag.magADC[X]), lrintf(mag.magADC[Y]), lrintf(mag.magADC[Z]));
padLineBuffer();
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
i2c_OLED_set_line(dev, rowIndex++);
i2c_OLED_send_string(dev, lineBuffer);
}
#endif
tfp_sprintf(lineBuffer, format, "I&H", attitude.values.roll, attitude.values.pitch, DECIDEGREES_TO_DEGREES(attitude.values.yaw));
padLineBuffer();
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
i2c_OLED_set_line(dev, rowIndex++);
i2c_OLED_send_string(dev, lineBuffer);
/*
uint8_t length;
@ -526,8 +526,8 @@ static void showSensorsPage(void)
}
ftoa(EstG.A[Y], lineBuffer + length);
padLineBuffer();
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
i2c_OLED_set_line(dev, rowIndex++);
i2c_OLED_send_string(dev, lineBuffer);
ftoa(EstG.A[Z], lineBuffer);
length = strlen(lineBuffer);
@ -537,8 +537,8 @@ static void showSensorsPage(void)
}
ftoa(smallAngle, lineBuffer + length);
padLineBuffer();
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
i2c_OLED_set_line(dev, rowIndex++);
i2c_OLED_send_string(dev, lineBuffer);
*/
}
@ -549,8 +549,8 @@ static void showTasksPage(void)
uint8_t rowIndex = PAGE_TITLE_LINE_COUNT;
static const char *format = "%2d%6d%5d%4d%4d";
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, "Task max avg mx% av%");
i2c_OLED_set_line(dev, rowIndex++);
i2c_OLED_send_string(dev, "Task max avg mx% av%");
taskInfo_t taskInfo;
for (taskId_e taskId = 0; taskId < TASK_COUNT; ++taskId) {
getTaskInfo(taskId, &taskInfo);
@ -560,8 +560,8 @@ static void showTasksPage(void)
const int averageLoad = (taskInfo.averageExecutionTimeUs * taskFrequency + 5000) / 10000;
tfp_sprintf(lineBuffer, format, taskId, taskInfo.maxExecutionTimeUs, taskInfo.averageExecutionTimeUs, maxLoad, averageLoad);
padLineBuffer();
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
i2c_OLED_set_line(dev, rowIndex++);
i2c_OLED_send_string(dev, lineBuffer);
if (rowIndex > SCREEN_CHARACTER_ROW_COUNT) {
break;
}
@ -594,8 +594,8 @@ static void showBBPage(void)
}
padLineBuffer();
i2c_OLED_set_line(bus, rowIndex++);
i2c_OLED_send_string(bus, lineBuffer);
i2c_OLED_set_line(dev, rowIndex++);
i2c_OLED_send_string(dev, lineBuffer);
}
#endif
@ -606,8 +606,8 @@ static void showDebugPage(void)
for (int rowIndex = 0; rowIndex < 4; rowIndex++) {
tfp_sprintf(lineBuffer, "%d = %5d", rowIndex, debug[rowIndex]);
padLineBuffer();
i2c_OLED_set_line(bus, rowIndex + PAGE_TITLE_LINE_COUNT);
i2c_OLED_send_string(bus, lineBuffer);
i2c_OLED_set_line(dev, rowIndex + PAGE_TITLE_LINE_COUNT);
i2c_OLED_send_string(dev, lineBuffer);
}
}
#endif
@ -722,16 +722,21 @@ void dashboardUpdate(timeUs_t currentTimeUs)
void dashboardInit(void)
{
static extDevice_t dashBoardDev;
// TODO Use bus singleton
static busDevice_t dashBoardBus;
dashBoardBus.busdev_u.i2c.device = I2C_CFG_TO_DEV(dashboardConfig()->device);
dashBoardBus.busdev_u.i2c.address = dashboardConfig()->address;
bus = &dashBoardBus;
dashBoardDev.bus = &dashBoardBus;
dashBoardBus.busType_u.i2c.device = I2C_CFG_TO_DEV(dashboardConfig()->device);
dashBoardDev.busType_u.i2c.address = dashboardConfig()->address;
dev = &dashBoardDev;
delay(200);
resetDisplay();
delay(200);
displayPort = displayPortOledInit(bus);
displayPort = displayPortOledInit(dev);
#if defined(USE_CMS)
if (dashboardPresent) {
cmsDisplayPortRegister(displayPort);

4
src/main/io/flashfs.c
View File

@ -47,7 +47,7 @@ static const flashPartition_t *flashPartition = NULL;
static const flashGeometry_t *flashGeometry = NULL;
static uint32_t flashfsSize = 0;
static uint8_t flashWriteBuffer[FLASHFS_WRITE_BUFFER_SIZE];
static DMA_DATA_ZERO_INIT uint8_t flashWriteBuffer[FLASHFS_WRITE_BUFFER_SIZE];
/* The position of our head and tail in the circular flash write buffer.
*
@ -538,7 +538,7 @@ int flashfsIdentifyStartOfFreeSpace(void)
STATIC_ASSERT(FREE_BLOCK_SIZE >= FLASH_MAX_PAGE_SIZE, FREE_BLOCK_SIZE_too_small);
union {
STATIC_DMA_DATA_AUTO union {
uint8_t bytes[FREE_BLOCK_TEST_SIZE_BYTES];
uint32_t ints[FREE_BLOCK_TEST_SIZE_INTS];
} testBuffer;

2
src/main/msc/emfat_file.c
View File

@ -311,12 +311,12 @@ static void emfat_add_log(emfat_entry_t *entry, int number, uint32_t offset, uin
static int emfat_find_log(emfat_entry_t *entry, int maxCount, int flashfsUsedSpace)
{
static uint8_t buffer[HDR_BUF_SIZE];
int lastOffset = 0;
int currOffset = 0;
int buffOffset;
int hdrOffset;
int fileNumber = 0;
uint8_t buffer[HDR_BUF_SIZE];
int logCount = 0;
char *logHeader = "H Product:Blackbox";
int lenLogHeader = strlen(logHeader);

3
src/main/msp/msp.c
View File

@ -521,7 +521,8 @@ static void serializeDataflashReadReply(sbuf_t *dst, uint32_t address, const uin
#ifdef USE_HUFFMAN
// compress in 256-byte chunks
const uint16_t READ_BUFFER_SIZE = 256;
uint8_t readBuffer[READ_BUFFER_SIZE];
// This may be DMAable, so make it cache aligned
__attribute__ ((aligned(32))) uint8_t readBuffer[READ_BUFFER_SIZE];
huffmanState_t state = {
.bytesWritten = 0,

2
src/main/osd/osd.c
View File

@ -1056,12 +1056,10 @@ void osdUpdate(timeUs_t currentTimeUs)
showVisualBeeper = true;
}
#ifdef MAX7456_DMA_CHANNEL_TX
// don't touch buffers if DMA transaction is in progress
if (displayIsTransferInProgress(osdDisplayPort)) {
return;
}
#endif // MAX7456_DMA_CHANNEL_TX
#ifdef USE_SLOW_MSP_DISPLAYPORT_RATE_WHEN_UNARMED
static uint32_t idlecounter = 0;

6
src/main/pg/gyrodev.c
View File

@ -49,7 +49,7 @@ static void gyroResetCommonDeviceConfig(gyroDeviceConfig_t *devconf, ioTag_t ext
#ifdef USE_SPI_GYRO
static void gyroResetSpiDeviceConfig(gyroDeviceConfig_t *devconf, SPI_TypeDef *instance, ioTag_t csnTag, ioTag_t extiTag, uint8_t alignment, sensorAlignment_t customAlignment)
{
devconf->bustype = BUSTYPE_SPI;
devconf->busType = BUS_TYPE_SPI;
devconf->spiBus = SPI_DEV_TO_CFG(spiDeviceByInstance(instance));
devconf->csnTag = csnTag;
gyroResetCommonDeviceConfig(devconf, extiTag, alignment, customAlignment);
@ -59,7 +59,7 @@ static void gyroResetSpiDeviceConfig(gyroDeviceConfig_t *devconf, SPI_TypeDef *i
#if defined(USE_I2C_GYRO) && !defined(USE_MULTI_GYRO)
static void gyroResetI2cDeviceConfig(gyroDeviceConfig_t *devconf, I2CDevice i2cbus, ioTag_t extiTag, uint8_t alignment, sensorAlignment_t customAlignment)
{
devconf->bustype = BUSTYPE_I2C;
devconf->busType = BUS_TYPE_I2C;
devconf->i2cBus = I2C_DEV_TO_CFG(i2cbus);
devconf->i2cAddress = GYRO_I2C_ADDRESS;
gyroResetCommonDeviceConfig(devconf, extiTag, alignment, customAlignment);
@ -101,6 +101,6 @@ void pgResetFn_gyroDeviceConfig(gyroDeviceConfig_t *devconf)
// Special treatment for very rare F3 targets with variants having either I2C or SPI acc/gyro chip; mark it for run time detection.
#if defined(USE_SPI_GYRO) && defined(USE_I2C_GYRO)
devconf[0].bustype = BUSTYPE_GYRO_AUTO;
devconf[0].busType = BUS_TYPE_GYRO_AUTO;
#endif
}

2
src/main/pg/gyrodev.h
View File

@ -29,7 +29,7 @@
typedef struct gyroDeviceConfig_s {
int8_t index;
uint8_t bustype;
uint8_t busType;
uint8_t spiBus;
ioTag_t csnTag;
uint8_t i2cBus;

11
src/main/pg/sdcard.c
View File

@ -43,7 +43,6 @@ void pgResetFn_sdcardConfig(sdcardConfig_t *config)
// We can safely handle SPI and SDIO cases separately on custom targets, as these are exclusive per target.
// On generic targets, SPI has precedence over SDIO; SDIO must be post-flash configured.
config->useDma = false;
config->device = SPI_DEV_TO_CFG(SPIINVALID);
#ifdef CONFIG_IN_SDCARD
@ -68,15 +67,5 @@ void pgResetFn_sdcardConfig(sdcardConfig_t *config)
config->mode = SDCARD_MODE_SPI;
}
#endif
#ifndef USE_DMA_SPEC
#ifdef USE_SDCARD_SPI
#if defined(SDCARD_DMA_STREAM_TX_FULL)
config->dmaIdentifier = (uint8_t)dmaGetIdentifier(SDCARD_DMA_STREAM_TX_FULL);
#elif defined(SDCARD_DMA_CHANNEL_TX)
config->dmaIdentifier = (uint8_t)dmaGetIdentifier(SDCARD_DMA_CHANNEL_TX);
#endif
#endif
#endif // !USE_DMA_SPEC
}
#endif

5
src/main/pg/sdcard.h
View File

@ -30,15 +30,10 @@ typedef enum {
} sdcardMode_e;
typedef struct sdcardConfig_s {
uint8_t useDma;
int8_t device;
ioTag_t cardDetectTag;
ioTag_t chipSelectTag;
uint8_t cardDetectInverted;
#ifndef USE_DMA_SPEC
uint8_t dmaIdentifier;
uint8_t dmaChannel;
#endif
sdcardMode_e mode;
} sdcardConfig_t;

4
src/main/rx/cc2500_frsky_shared.c
View File

@ -155,6 +155,8 @@ const cc2500RegisterConfigElement_t cc2500FrskyXLbtV2Config[] =
};
static void initialise() {
rxSpiStartupSpeed();
cc2500Reset();
cc2500ApplyRegisterConfig(cc2500FrskyBaseConfig, sizeof(cc2500FrskyBaseConfig));
@ -197,6 +199,8 @@ static void initialise() {
calData[c][1] = cc2500ReadReg(CC2500_24_FSCAL2);
calData[c][2] = cc2500ReadReg(CC2500_25_FSCAL1);
}
rxSpiNormalSpeed();
}
void initialiseData(bool inBindState)

1
src/main/rx/rx.h
View File

@ -174,6 +174,7 @@ extern linkQualitySource_e linkQualitySource;
extern rxRuntimeState_t rxRuntimeState; //!!TODO remove this extern, only needed once for channelCount
void rxInit(void);
void rxProcessPending(bool state);
bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs);
bool rxIsReceivingSignal(void);
bool rxAreFlightChannelsValid(void);

4
src/main/rx/rx_spi_common.c
View File

@ -27,9 +27,11 @@
#include "drivers/io.h"
#include "drivers/time.h"
#include "rx/rx_spi_common.h"
#include "rx/rx_spi.h"
#include "rx_spi_common.h"
static IO_t ledPin;
static bool ledInversion = false;

2
src/main/sensors/acceleration_init.c
View File

@ -352,7 +352,7 @@ bool accInit(uint16_t accSampleRateHz)
{
memset(&acc, 0, sizeof(acc));
// copy over the common gyro mpu settings
acc.dev.bus = *gyroSensorBus();
acc.dev.gyro = gyroActiveDev();
acc.dev.mpuDetectionResult = *gyroMpuDetectionResult();
acc.dev.acc_high_fsr = accelerometerConfig()->acc_high_fsr;

45
src/main/sensors/barometer.c
View File

@ -43,6 +43,7 @@
#include "drivers/barometer/barometer_ms5611.h"
#include "drivers/barometer/barometer_lps.h"
#include "drivers/bus.h"
#include "drivers/bus_i2c_busdev.h"
#include "drivers/bus_spi.h"
#include "drivers/io.h"
#include "drivers/time.h"
@ -113,21 +114,21 @@ void pgResetFn_barometerConfig(barometerConfig_t *barometerConfig)
#endif
#if defined(DEFAULT_BARO_SPI_BMP388) || defined(DEFAULT_BARO_SPI_BMP280) || defined(DEFAULT_BARO_SPI_MS5611) || defined(DEFAULT_BARO_SPI_QMP6988) || defined(DEFAULT_BARO_SPI_LPS) || defined(DEFAULT_BARO_SPI_DPS310)
barometerConfig->baro_bustype = BUSTYPE_SPI;
barometerConfig->baro_busType = BUS_TYPE_SPI;
barometerConfig->baro_spi_device = SPI_DEV_TO_CFG(spiDeviceByInstance(BARO_SPI_INSTANCE));
barometerConfig->baro_spi_csn = IO_TAG(BARO_CS_PIN);
barometerConfig->baro_i2c_device = I2C_DEV_TO_CFG(I2CINVALID);
barometerConfig->baro_i2c_address = 0;
#elif defined(DEFAULT_BARO_MS5611) || defined(DEFAULT_BARO_BMP388) || defined(DEFAULT_BARO_BMP280) || defined(DEFAULT_BARO_BMP085) ||defined(DEFAULT_BARO_QMP6988) || defined(DEFAULT_BARO_DPS310)
// All I2C devices shares a default config with address = 0 (per device default)
barometerConfig->baro_bustype = BUSTYPE_I2C;
barometerConfig->baro_busType = BUS_TYPE_I2C;
barometerConfig->baro_i2c_device = I2C_DEV_TO_CFG(BARO_I2C_INSTANCE);
barometerConfig->baro_i2c_address = 0;
barometerConfig->baro_spi_device = SPI_DEV_TO_CFG(SPIINVALID);
barometerConfig->baro_spi_csn = IO_TAG_NONE;
#else
barometerConfig->baro_hardware = BARO_NONE;
barometerConfig->baro_bustype = BUSTYPE_NONE;
barometerConfig->baro_busType = BUS_TYPE_NONE;
barometerConfig->baro_i2c_device = I2C_DEV_TO_CFG(I2CINVALID);
barometerConfig->baro_i2c_address = 0;
barometerConfig->baro_spi_device = SPI_DEV_TO_CFG(SPIINVALID);
@ -154,14 +155,16 @@ static bool baroReady = false;
void baroPreInit(void)
{
#ifdef USE_SPI
if (barometerConfig()->baro_bustype == BUSTYPE_SPI) {
if (barometerConfig()->baro_busType == BUS_TYPE_SPI) {
spiPreinitRegister(barometerConfig()->baro_spi_csn, IOCFG_IPU, 1);
}
#endif
}
bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse)
bool baroDetect(baroDev_t *baroDev, baroSensor_e baroHardwareToUse)
{
extDevice_t *dev = &baroDev->dev;
// Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
baroSensor_e baroHardware = baroHardwareToUse;
@ -170,26 +173,22 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse)
UNUSED(dev);
#endif
switch (barometerConfig()->baro_bustype) {
switch (barometerConfig()->baro_busType) {
#ifdef USE_I2C
case BUSTYPE_I2C:
dev->busdev.bustype = BUSTYPE_I2C;
dev->busdev.busdev_u.i2c.device = I2C_CFG_TO_DEV(barometerConfig()->baro_i2c_device);
dev->busdev.busdev_u.i2c.address = barometerConfig()->baro_i2c_address;
case BUS_TYPE_I2C:
i2cBusSetInstance(dev, barometerConfig()->baro_i2c_device);
dev->busType_u.i2c.address = barometerConfig()->baro_i2c_address;
break;
#endif
#ifdef USE_SPI
case BUSTYPE_SPI:
case BUS_TYPE_SPI:
{
SPI_TypeDef *instance = spiInstanceByDevice(SPI_CFG_TO_DEV(barometerConfig()->baro_spi_device));
if (!instance) {
if (!spiSetBusInstance(dev, barometerConfig()->baro_spi_device, OWNER_BARO_CS)) {
return false;
}
dev->busdev.bustype = BUSTYPE_SPI;
spiBusSetInstance(&dev->busdev, instance);
dev->busdev.busdev_u.spi.csnPin = IOGetByTag(barometerConfig()->baro_spi_csn);
dev->busType_u.spi.csnPin = IOGetByTag(barometerConfig()->baro_spi_csn);
}
break;
#endif
@ -211,7 +210,7 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse)
static const bmp085Config_t *bmp085Config = &defaultBMP085Config;
if (bmp085Detect(bmp085Config, dev)) {
if (bmp085Detect(bmp085Config, baroDev)) {
baroHardware = BARO_BMP085;
break;
}
@ -221,7 +220,7 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse)
case BARO_MS5611:
#if defined(USE_BARO_MS5611) || defined(USE_BARO_SPI_MS5611)
if (ms5611Detect(dev)) {
if (ms5611Detect(baroDev)) {
baroHardware = BARO_MS5611;
break;
}
@ -230,7 +229,7 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse)
case BARO_LPS:
#if defined(USE_BARO_SPI_LPS)
if (lpsDetect(dev)) {
if (lpsDetect(baroDev)) {
baroHardware = BARO_LPS;
break;
}
@ -240,7 +239,7 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse)
case BARO_DPS310:
#if defined(USE_BARO_DPS310) || defined(USE_BARO_SPI_DPS310)
{
if (baroDPS310Detect(dev)) {
if (baroDPS310Detect(baroDev)) {
baroHardware = BARO_DPS310;
break;
}
@ -257,7 +256,7 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse)
static const bmp388Config_t *bmp388Config = &defaultBMP388Config;
if (bmp388Detect(bmp388Config, dev)) {
if (bmp388Detect(bmp388Config, baroDev)) {
baroHardware = BARO_BMP388;
break;
}
@ -267,7 +266,7 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse)
case BARO_BMP280:
#if defined(USE_BARO_BMP280) || defined(USE_BARO_SPI_BMP280)
if (bmp280Detect(dev)) {
if (bmp280Detect(baroDev)) {
baroHardware = BARO_BMP280;
break;
}
@ -276,7 +275,7 @@ bool baroDetect(baroDev_t *dev, baroSensor_e baroHardwareToUse)
case BARO_QMP6988:
#if defined(USE_BARO_QMP6988) || defined(USE_BARO_SPI_QMP6988)
if (qmp6988Detect(dev)) {
if (qmp6988Detect(baroDev)) {
baroHardware = BARO_QMP6988;
break;
}

2
src/main/sensors/barometer.h
View File

@ -38,7 +38,7 @@ typedef enum {
#define BARO_SAMPLE_COUNT_MAX 48
typedef struct barometerConfig_s {
uint8_t baro_bustype;
uint8_t baro_busType;
uint8_t baro_spi_device;
ioTag_t baro_spi_csn; // Also used as XCLR (positive logic) for BMP085
uint8_t baro_i2c_device;

85
src/main/sensors/compass.c
View File

@ -32,6 +32,7 @@
#include "drivers/bus.h"
#include "drivers/bus_i2c.h"
#include "drivers/bus_i2c_busdev.h"
#include "drivers/bus_spi.h"
#include "drivers/compass/compass.h"
#include "drivers/compass/compass_ak8975.h"
@ -63,7 +64,7 @@ static flightDynamicsTrims_t magZeroTempMin;
static flightDynamicsTrims_t magZeroTempMax;
magDev_t magDev;
mag_t mag; // mag access functions
mag_t mag;
PG_REGISTER_WITH_RESET_FN(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 3);
@ -80,26 +81,26 @@ void pgResetFn_compassConfig(compassConfig_t *compassConfig)
// 3. Slave I2C device on SPI gyro
#if defined(USE_SPI) && (defined(USE_MAG_SPI_HMC5883) || defined(USE_MAG_SPI_AK8963))
compassConfig->mag_bustype = BUSTYPE_SPI;
compassConfig->mag_bustype = BUS_TYPE_SPI;
compassConfig->mag_spi_device = SPI_DEV_TO_CFG(spiDeviceByInstance(MAG_SPI_INSTANCE));
compassConfig->mag_spi_csn = IO_TAG(MAG_CS_PIN);
compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(I2CINVALID);
compassConfig->mag_i2c_address = 0;
#elif defined(USE_MAG_HMC5883) || defined(USE_MAG_QMC5883) || defined(USE_MAG_AK8975) || (defined(USE_MAG_AK8963) && !(defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250)))
compassConfig->mag_bustype = BUSTYPE_I2C;
compassConfig->mag_bustype = BUS_TYPE_I2C;
compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(MAG_I2C_INSTANCE);
compassConfig->mag_i2c_address = 0;
compassConfig->mag_spi_device = SPI_DEV_TO_CFG(SPIINVALID);
compassConfig->mag_spi_csn = IO_TAG_NONE;
#elif defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))
compassConfig->mag_bustype = BUSTYPE_MPU_SLAVE;
compassConfig->mag_bustype = BUS_TYPE_MPU_SLAVE;
compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(I2CINVALID);
compassConfig->mag_i2c_address = 0;
compassConfig->mag_spi_device = SPI_DEV_TO_CFG(SPIINVALID);
compassConfig->mag_spi_csn = IO_TAG_NONE;
#else
compassConfig->mag_hardware = MAG_NONE;
compassConfig->mag_bustype = BUSTYPE_NONE;
compassConfig->mag_bustype = BUS_TYPE_NONE;
compassConfig->mag_i2c_device = I2C_DEV_TO_CFG(I2CINVALID);
compassConfig->mag_i2c_address = 0;
compassConfig->mag_spi_device = SPI_DEV_TO_CFG(SPIINVALID);
@ -114,56 +115,56 @@ static uint8_t magInit = 0;
void compassPreInit(void)
{
#ifdef USE_SPI
if (compassConfig()->mag_bustype == BUSTYPE_SPI) {
if (compassConfig()->mag_bustype == BUS_TYPE_SPI) {
spiPreinitRegister(compassConfig()->mag_spi_csn, IOCFG_IPU, 1);
}
#endif
}
#if !defined(SIMULATOR_BUILD)
bool compassDetect(magDev_t *dev, uint8_t *alignment)
bool compassDetect(magDev_t *magDev, uint8_t *alignment)
{
*alignment = ALIGN_DEFAULT; // may be overridden if target specifies MAG_*_ALIGN
magSensor_e magHardware = MAG_NONE;
busDevice_t *busdev = &dev->busdev;
extDevice_t *dev = &magDev->dev;
// Associate magnetometer bus with its device
dev->bus = &magDev->bus;
#ifdef USE_MAG_DATA_READY_SIGNAL
dev->magIntExtiTag = compassConfig()->interruptTag;
magDev->magIntExtiTag = compassConfig()->interruptTag;
#endif
switch (compassConfig()->mag_bustype) {
#ifdef USE_I2C
case BUSTYPE_I2C:
busdev->bustype = BUSTYPE_I2C;
busdev->busdev_u.i2c.device = I2C_CFG_TO_DEV(compassConfig()->mag_i2c_device);
busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address;
case BUS_TYPE_I2C:
i2cBusSetInstance(dev, compassConfig()->mag_i2c_device);
dev->busType_u.i2c.address = compassConfig()->mag_i2c_address;
break;
#endif
#ifdef USE_SPI
case BUSTYPE_SPI:
case BUS_TYPE_SPI:
{
SPI_TypeDef *instance = spiInstanceByDevice(SPI_CFG_TO_DEV(compassConfig()->mag_spi_device));
if (!instance) {
if (!spiSetBusInstance(dev, compassConfig()->mag_spi_device, OWNER_COMPASS_CS)) {
return false;
}
busdev->bustype = BUSTYPE_SPI;
spiBusSetInstance(busdev, instance);
busdev->busdev_u.spi.csnPin = IOGetByTag(compassConfig()->mag_spi_csn);
dev->busType_u.spi.csnPin = IOGetByTag(compassConfig()->mag_spi_csn);
}
break;
#endif
#if defined(USE_MAG_AK8963) && (defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250))
case BUSTYPE_MPU_SLAVE:
case BUS_TYPE_MPU_SLAVE:
{
if (gyroMpuDetectionResult()->sensor == MPU_9250_SPI) {
busdev->bustype = BUSTYPE_MPU_SLAVE;
busdev->busdev_u.mpuSlave.master = gyroSensorBus();
busdev->busdev_u.mpuSlave.address = compassConfig()->mag_i2c_address;
extDevice_t *masterDev = &gyroActiveDev()->dev;
dev->busType_u.i2c.address = compassConfig()->mag_i2c_address;
dev->bus->busType = BUS_TYPE_MPU_SLAVE;
dev->bus->busType_u.mpuSlave.master = masterDev;
} else {
return false;
}
@ -181,11 +182,11 @@ bool compassDetect(magDev_t *dev, uint8_t *alignment)
case MAG_HMC5883:
#if defined(USE_MAG_HMC5883) || defined(USE_MAG_SPI_HMC5883)
if (busdev->bustype == BUSTYPE_I2C) {
busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address;
if (dev->bus->busType == BUS_TYPE_I2C) {
dev->busType_u.i2c.address = compassConfig()->mag_i2c_address;
}
if (hmc5883lDetect(dev)) {
if (hmc5883lDetect(magDev)) {
#ifdef MAG_HMC5883_ALIGN
*alignment = MAG_HMC5883_ALIGN;
#endif
@ -197,11 +198,11 @@ bool compassDetect(magDev_t *dev, uint8_t *alignment)
case MAG_LIS3MDL:
#if defined(USE_MAG_LIS3MDL)
if (busdev->bustype == BUSTYPE_I2C) {
busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address;
if (dev->bus->busType == BUS_TYPE_I2C) {
dev->busType_u.i2c.address = compassConfig()->mag_i2c_address;
}
if (lis3mdlDetect(dev)) {
if (lis3mdlDetect(magDev)) {
#ifdef MAG_LIS3MDL_ALIGN
*alignment = MAG_LIS3MDL_ALIGN;
#endif
@ -213,11 +214,11 @@ bool compassDetect(magDev_t *dev, uint8_t *alignment)
case MAG_AK8975:
#ifdef USE_MAG_AK8975
if (busdev->bustype == BUSTYPE_I2C) {
busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address;
if (dev->bus->busType == BUS_TYPE_I2C) {
dev->busType_u.i2c.address = compassConfig()->mag_i2c_address;
}
if (ak8975Detect(dev)) {
if (ak8975Detect(magDev)) {
#ifdef MAG_AK8975_ALIGN
*alignment = MAG_AK8975_ALIGN;
#endif
@ -229,16 +230,16 @@ bool compassDetect(magDev_t *dev, uint8_t *alignment)
case MAG_AK8963:
#if defined(USE_MAG_AK8963) || defined(USE_MAG_SPI_AK8963)
if (busdev->bustype == BUSTYPE_I2C) {
busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address;
if (dev->bus->busType == BUS_TYPE_I2C) {
dev->busType_u.i2c.address = compassConfig()->mag_i2c_address;
}
if (gyroMpuDetectionResult()->sensor == MPU_9250_SPI) {
dev->busdev.bustype = BUSTYPE_MPU_SLAVE;
busdev->busdev_u.mpuSlave.address = compassConfig()->mag_i2c_address;
dev->busdev.busdev_u.mpuSlave.master = gyroSensorBus();
dev->bus->busType = BUS_TYPE_MPU_SLAVE;
dev->busType_u.mpuSlave.address = compassConfig()->mag_i2c_address;
dev->bus->busType_u.mpuSlave.master = &gyroActiveDev()->dev;
}
if (ak8963Detect(dev)) {
if (ak8963Detect(magDev)) {
#ifdef MAG_AK8963_ALIGN
*alignment = MAG_AK8963_ALIGN;
#endif
@ -250,11 +251,11 @@ bool compassDetect(magDev_t *dev, uint8_t *alignment)
case MAG_QMC5883:
#ifdef USE_MAG_QMC5883
if (busdev->bustype == BUSTYPE_I2C) {
busdev->busdev_u.i2c.address = compassConfig()->mag_i2c_address;
if (dev->bus->busType == BUS_TYPE_I2C) {
dev->busType_u.i2c.address = compassConfig()->mag_i2c_address;
}
if (qmc5883lDetect(dev)) {
if (qmc5883lDetect(magDev)) {
#ifdef MAG_QMC5883L_ALIGN
*alignment = MAG_QMC5883L_ALIGN;
#endif
@ -273,7 +274,7 @@ bool compassDetect(magDev_t *dev, uint8_t *alignment)
// Passthrough mode disables the gyro/acc part of the MPU, so we only want to detect this sensor if mag_hardware was explicitly set to MAG_MPU925X_AK8963
#ifdef USE_MAG_MPU925X_AK8963
if(compassConfig()->mag_hardware == MAG_MPU925X_AK8963){
if (mpu925Xak8963CompassDetect(dev)) {
if (mpu925Xak8963CompassDetect(magDev)) {
magHardware = MAG_MPU925X_AK8963;
} else {
return false;

13
src/main/sensors/gyro_init.c
View File

@ -708,9 +708,10 @@ void gyroSetTargetLooptime(uint8_t pidDenom)
}
}
const busDevice_t *gyroSensorBus(void)
gyroDev_t *gyroActiveDev(void)
{
return &ACTIVE_GYRO->gyroDev.bus;
return &ACTIVE_GYRO->gyroDev;
}
const mpuDetectionResult_t *gyroMpuDetectionResult(void)
@ -724,20 +725,20 @@ int16_t gyroRateDps(int axis)
}
#ifdef USE_GYRO_REGISTER_DUMP
static const busDevice_t *gyroSensorBusByDevice(uint8_t whichSensor)
static extDevice_t *gyroSensorDevByInstance(uint8_t whichSensor)
{
#ifdef USE_MULTI_GYRO
if (whichSensor == GYRO_CONFIG_USE_GYRO_2) {
return &gyro.gyroSensor2.gyroDev.bus;
return &gyro.gyroSensor2.gyroDev.dev;
}
#else
UNUSED(whichSensor);
#endif
return &gyro.gyroSensor1.gyroDev.bus;
return &gyro.gyroSensor1.gyroDev.dev;
}
uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg)
{
return mpuGyroReadRegister(gyroSensorBusByDevice(whichSensor), reg);
return mpuGyroReadRegister(gyroSensorDevByInstance(whichSensor), reg);
}
#endif // USE_GYRO_REGISTER_DUMP

2
src/main/sensors/gyro_init.h
View File

@ -29,7 +29,7 @@ bool gyroInit(void);
void gyroInitFilters(void);
void gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config);
gyroDetectionFlags_t getGyroDetectionFlags(void);
const busDevice_t *gyroSensorBus(void);
gyroDev_t *gyroActiveDev(void);
struct mpuDetectionResult_s;
const struct mpuDetectionResult_s *gyroMpuDetectionResult(void);
int16_t gyroRateDps(int axis);

0
src/main/startup/stm32g4xx_hal_conf.h Executable file → Normal file
View File

Some files were not shown because too many files have changed in this diff Show More