mirror of https://github.com/rusefi/rusefi-1.git
118 lines
3.3 KiB
C++
118 lines
3.3 KiB
C++
/*
|
|
* @file accelerometer.cpp
|
|
*
|
|
* stm32f4discovery has MEMS LIS302DL
|
|
* www.st.com/resource/en/datasheet/lis302dl.pdf
|
|
*
|
|
* SPI1
|
|
* LIS302DL_SPI_SCK PA5
|
|
* LIS302DL_SPI_MISO PA6
|
|
* LIS302DL_SPI_MOSI PA7
|
|
* LIS302DL_SPI_CS_PIN PE3
|
|
*
|
|
*
|
|
*
|
|
* @date May 19, 2016
|
|
* @author Andrey Belomutskiy, (c) 2012-2020
|
|
*/
|
|
|
|
#include "accelerometer.h"
|
|
#include "hardware.h"
|
|
|
|
EXTERN_ENGINE;
|
|
|
|
#if EFI_MEMS
|
|
#include "mpu_util.h"
|
|
#include "lis302dl.h"
|
|
#include "periodic_thread_controller.h"
|
|
|
|
static SPIDriver *driver;
|
|
|
|
/*
|
|
* SPI1 configuration structure.
|
|
* Speed 5.25MHz, CPHA=1, CPOL=1, 8bits frames, MSb transmitted first.
|
|
* The slave select line is the pin GPIOE_CS_SPI on the port GPIOE.
|
|
*/
|
|
static const SPIConfig accelerometerCfg = {
|
|
.spi_bus = NULL,
|
|
/* HW dependent part.*/
|
|
.ssport = GPIOE,
|
|
.sspad = GPIOE_PIN3,
|
|
.cr1 = SPI_CR1_BR_0 | SPI_CR1_BR_1 | SPI_CR1_CPOL | SPI_CR1_CPHA |
|
|
SPI_CR1_8BIT_MODE,
|
|
.cr2 = SPI_CR2_8BIT_MODE
|
|
};
|
|
#endif /* EFI_MEMS */
|
|
|
|
void configureAccelerometerPins(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
|
|
// engineConfiguration->LIS302DLCsPin = GPIOE_3; // we have a conflict with VVT output on Miata
|
|
// CONFIG(is_enabled_spi_1) = true; // we have a conflict with PA5 input pin
|
|
|
|
// stm32f4discovery defaults
|
|
CONFIG(spi1mosiPin) = GPIOA_7;
|
|
CONFIG(spi1misoPin) = GPIOA_6;
|
|
CONFIG(spi1sckPin) = GPIOA_5;
|
|
}
|
|
|
|
|
|
#if EFI_MEMS
|
|
|
|
static THD_WORKING_AREA(ivThreadStack, UTILITY_THREAD_STACK_SIZE);
|
|
|
|
class AccelController : public PeriodicController<UTILITY_THREAD_STACK_SIZE> {
|
|
public:
|
|
AccelController() : PeriodicController("Acc SPI") { }
|
|
private:
|
|
void PeriodicTask(efitick_t nowNt) override {
|
|
// has to be a thread since we want to use blocking method - blocking method only available in threads, not in interrupt handler
|
|
// todo: migrate to async SPI API?
|
|
engine->sensors.accelerometer.x = (int8_t)lis302dlReadRegister(driver, LIS302DL_OUTX);
|
|
engine->sensors.accelerometer.y = (int8_t)lis302dlReadRegister(driver, LIS302DL_OUTY);
|
|
chThdSleepMilliseconds(20);
|
|
}
|
|
};
|
|
|
|
static BenchController instance;
|
|
|
|
void initAccelerometer(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
|
|
if (engineConfiguration->LIS302DLCsPin == GPIO_UNASSIGNED)
|
|
return; // not used
|
|
|
|
if (!CONFIG(is_enabled_spi_1))
|
|
return; // temporary
|
|
#if HAL_USE_SPI
|
|
driver = getSpiDevice(engineConfiguration->accelerometerSpiDevice);
|
|
if (driver == NULL) {
|
|
// error already reported
|
|
return;
|
|
}
|
|
|
|
turnOnSpi(engineConfiguration->accelerometerSpiDevice);
|
|
spiStart(driver, &accelerometerCfg);
|
|
initSpiCs((SPIConfig *)driver->config, engineConfiguration->LIS302DLCsPin);
|
|
|
|
// memsCs.initPin("LIS302 CS", engineConfiguration->LIS302DLCsPin);
|
|
// memsCfg.ssport = getHwPort("mmc", CONFIG(sdCardCsPin));
|
|
// memsCfg.sspad = getHwPin("mmc", CONFIG(sdCardCsPin));
|
|
|
|
|
|
/* LIS302DL initialization.*/
|
|
lis302dlWriteRegister(driver, LIS302DL_CTRL_REG1, 0x47); // enable device, enable XYZ
|
|
lis302dlWriteRegister(driver, LIS302DL_CTRL_REG2, 0x00); // 4 wire mode
|
|
lis302dlWriteRegister(driver, LIS302DL_CTRL_REG3, 0x00);
|
|
|
|
chThdCreateStatic(ivThreadStack, sizeof(ivThreadStack), NORMALPRIO, (tfunc_t)(void*) ivThread, NULL);
|
|
#endif /* HAL_USE_SPI */
|
|
}
|
|
|
|
#endif /* EFI_MEMS */
|
|
|
|
|
|
float getLongitudinalAcceleration(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
|
|
return engine->sensors.accelerometer.x;
|
|
}
|
|
|
|
float getTransverseAcceleration(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
|
|
return engine->sensors.accelerometer.y;
|
|
}
|