Cherry-picked #3321 (Preset CS lines for SPI devices initial high)
This commit is contained in:
parent
1f262d4446
commit
0c803f23d2
5
Makefile
5
Makefile
|
@ -680,6 +680,7 @@ COMMON_SRC = \
|
|||
drivers/bus_i2c_config.c \
|
||||
drivers/bus_i2c_soft.c \
|
||||
drivers/bus_spi.c \
|
||||
drivers/bus_spi_config.c \
|
||||
drivers/bus_spi_pinconfig.c \
|
||||
drivers/bus_spi_soft.c \
|
||||
drivers/buttons.c \
|
||||
|
@ -899,6 +900,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
|
|||
|
||||
SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
|
||||
drivers/bus_i2c_config.c \
|
||||
drivers/bus_spi_config.c \
|
||||
drivers/bus_spi_pinconfig.c \
|
||||
drivers/serial_escserial.c \
|
||||
drivers/serial_pinconfig.c \
|
||||
|
@ -1025,9 +1027,10 @@ F7EXCLUDES = drivers/bus_spi.c \
|
|||
|
||||
SITLEXCLUDES = \
|
||||
drivers/adc.c \
|
||||
drivers/bus_spi.c \
|
||||
drivers/bus_i2c.c \
|
||||
drivers/bus_i2c_config.c \
|
||||
drivers/bus_spi.c \
|
||||
drivers/bus_spi_config.c \
|
||||
drivers/bus_spi_pinconfig.c \
|
||||
drivers/dma.c \
|
||||
drivers/pwm_output.c \
|
||||
|
|
|
@ -103,6 +103,7 @@ bool bmi160Detect(const busDevice_t *bus)
|
|||
return true;
|
||||
IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
|
||||
IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
|
||||
IOHi(bus->spi.csnPin);
|
||||
|
||||
spiSetDivisor(BMI160_SPI_INSTANCE, BMI160_SPI_DIVISOR);
|
||||
|
||||
|
|
|
@ -69,6 +69,7 @@ static void icm20689SpiInit(const busDevice_t *bus)
|
|||
|
||||
IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
|
||||
IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
|
||||
IOHi(bus->spi.csnPin);
|
||||
|
||||
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD);
|
||||
|
||||
|
|
|
@ -155,6 +155,7 @@ bool mpu6000SpiDetect(const busDevice_t *bus)
|
|||
|
||||
IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
|
||||
IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
|
||||
IOHi(bus->spi.csnPin);
|
||||
|
||||
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
|
||||
|
||||
|
|
|
@ -77,6 +77,7 @@ static void mpu6500SpiInit(const busDevice_t *bus)
|
|||
|
||||
IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
|
||||
IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
|
||||
IOHi(bus->spi.csnPin);
|
||||
|
||||
spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST);
|
||||
|
||||
|
|
|
@ -21,8 +21,6 @@
|
|||
|
||||
#include <platform.h>
|
||||
|
||||
#include "build/debug.h"
|
||||
|
||||
#include "drivers/bus_spi.h"
|
||||
#include "drivers/bus_spi_impl.h"
|
||||
#include "drivers/exti.h"
|
||||
|
|
|
@ -79,6 +79,7 @@ typedef enum SPIDevice {
|
|||
#define SPIDEV_COUNT 4
|
||||
#endif
|
||||
|
||||
void spiPreInitCs(ioTag_t iotag);
|
||||
bool spiInit(SPIDevice device);
|
||||
void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor);
|
||||
uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t in);
|
||||
|
|
|
@ -0,0 +1,37 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* Cleanflight is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include <platform.h>
|
||||
|
||||
#include "drivers/bus_spi.h"
|
||||
#include "drivers/io.h"
|
||||
|
||||
// Bring a pin for possible CS line to pull-up state in preparation for
|
||||
// sequential initialization by relevant drivers.
|
||||
// Note that the pin is set to input for safety at this point.
|
||||
|
||||
void spiPreInitCs(ioTag_t iotag)
|
||||
{
|
||||
IO_t io = IOGetByTag(iotag);
|
||||
if (io) {
|
||||
IOInit(io, OWNER_SPI_PREINIT, 0);
|
||||
IOConfigGPIO(io, IOCFG_IPU);
|
||||
}
|
||||
}
|
|
@ -397,6 +397,7 @@ void max7456Init(const vcdProfile_t *pVcdProfile)
|
|||
#endif
|
||||
IOInit(max7456CsPin, OWNER_OSD_CS, 0);
|
||||
IOConfigGPIO(max7456CsPin, SPI_IO_CS_CFG);
|
||||
IOHi(max7456CsPin);
|
||||
|
||||
spiSetDivisor(MAX7456_SPI_INSTANCE, SPI_CLOCK_STANDARD);
|
||||
// force soft reset on Max7456
|
||||
|
|
|
@ -61,6 +61,7 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = {
|
|||
"LED_STRIP",
|
||||
"TRANSPONDER",
|
||||
"VTX",
|
||||
"COMPASS_CS"
|
||||
"COMPASS_CS",
|
||||
"SPI_PREINIT",
|
||||
};
|
||||
|
||||
|
|
|
@ -62,6 +62,7 @@ typedef enum {
|
|||
OWNER_TRANSPONDER,
|
||||
OWNER_VTX,
|
||||
OWNER_COMPASS_CS,
|
||||
OWNER_SPI_PREINIT,
|
||||
OWNER_TOTAL_COUNT
|
||||
} resourceOwner_e;
|
||||
|
||||
|
|
|
@ -184,6 +184,61 @@ static IO_t busSwitchResetPin = IO_NONE;
|
|||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_SPI
|
||||
// Pre-initialize all CS pins to input with pull-up.
|
||||
// It's sad that we can't do this with an initialized array,
|
||||
// since we will be taking care of configurable CS pins shortly.
|
||||
|
||||
void spiPreInit(void)
|
||||
{
|
||||
#ifdef USE_ACC_SPI_MPU6000
|
||||
spiPreInitCs(IO_TAG(MPU6000_CS_PIN));
|
||||
#endif
|
||||
#ifdef USE_ACC_SPI_MPU6500
|
||||
spiPreInitCs(IO_TAG(MPU6500_CS_PIN));
|
||||
#endif
|
||||
#ifdef USE_GYRO_SPI_MPU9250
|
||||
spiPreInitCs(IO_TAG(MPU9250_CS_PIN));
|
||||
#endif
|
||||
#ifdef USE_GYRO_SPI_ICM20689
|
||||
spiPreInitCs(IO_TAG(ICM20689_CS_PIN));
|
||||
#endif
|
||||
#ifdef USE_ACCGYRO_BMI160
|
||||
spiPreInitCs(IO_TAG(BMI160_CS_PIN));
|
||||
#endif
|
||||
#ifdef USE_GYRO_L3GD20
|
||||
spiPreInitCs(IO_TAG(L3GD20_CS_PIN));
|
||||
#endif
|
||||
#ifdef USE_MAX7456
|
||||
spiPreInitCs(IO_TAG(MAX7456_SPI_CS_PIN));
|
||||
#endif
|
||||
#ifdef USE_SDCARD
|
||||
spiPreInitCs(IO_TAG(SDCARD_SPI_CS_PIN));
|
||||
#endif
|
||||
#ifdef USE_BARO_SPI_BMP280
|
||||
spiPreInitCs(IO_TAG(BMP280_CS_PIN));
|
||||
#endif
|
||||
#ifdef USE_BARO_SPI_MS5611
|
||||
spiPreInitCs(IO_TAG(MS5611_CS_PIN));
|
||||
#endif
|
||||
#ifdef USE_MAG_SPI_HMC5883
|
||||
spiPreInitCs(IO_TAG(HMC5883_CS_PIN));
|
||||
#endif
|
||||
#ifdef USE_MAG_SPI_AK8963
|
||||
spiPreInitCs(IO_TAG(AK8963_CS_PIN));
|
||||
#endif
|
||||
#ifdef RTC6705_CS_PIN // XXX VTX_RTC6705? Should use USE_ format.
|
||||
spiPreInitCs(IO_TAG(RTC6705_CS_PIN));
|
||||
#endif
|
||||
#ifdef M25P16_CS_PIN // XXX Should use USE_ format.
|
||||
spiPreInitCs(IO_TAG(M25P16_CS_PIN));
|
||||
#endif
|
||||
#if defined(USE_RX_SPI) && !defined(USE_RX_SOFTSPI)
|
||||
spiPreInitCs(IO_TAG(RX_NSS_PIN));
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
void init(void)
|
||||
{
|
||||
#ifdef USE_HAL_DRIVER
|
||||
|
@ -351,6 +406,9 @@ void init(void)
|
|||
#ifdef USE_SPI
|
||||
spiPinConfigure();
|
||||
|
||||
// Initialize CS lines and keep them high
|
||||
spiPreInit();
|
||||
|
||||
#ifdef USE_SPI_DEVICE_1
|
||||
spiInit(SPIDEV_1);
|
||||
#endif
|
||||
|
|
|
@ -24,13 +24,14 @@
|
|||
#include "drivers/bus_spi.h"
|
||||
#include "io/serial.h"
|
||||
|
||||
extern void spiPreInit(void); // XXX In fc/fc_init.c
|
||||
|
||||
void targetBusInit(void)
|
||||
{
|
||||
#ifdef USE_SPI
|
||||
#if defined(USE_SPI) && defined(USE_SPI_DEVICE_1)
|
||||
spiPinConfigure();
|
||||
#ifdef USE_SPI_DEVICE_1
|
||||
spiPreInit();
|
||||
spiInit(SPIDEV_1);
|
||||
#endif
|
||||
#endif
|
||||
|
||||
if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) {
|
||||
|
|
|
@ -25,10 +25,13 @@
|
|||
#include "io/serial.h"
|
||||
#include "hardware_revision.h"
|
||||
|
||||
extern void spiPreInit(void); // XXX In fc/fc_init.c
|
||||
|
||||
void targetBusInit(void)
|
||||
{
|
||||
#ifdef USE_SPI
|
||||
spiPinConfigure();
|
||||
spiPreInit();
|
||||
#ifdef USE_SPI_DEVICE_2
|
||||
spiInit(SPIDEV_2);
|
||||
#endif
|
||||
|
|
Loading…
Reference in New Issue