193 lines
4.6 KiB
C++
193 lines
4.6 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 "pch.h"
|
|
|
|
#include "accelerometer.h"
|
|
#include "hardware.h"
|
|
|
|
#if EFI_ONBOARD_MEMS
|
|
#include "mpu_util.h"
|
|
#include "lis302dl.h"
|
|
#include "periodic_thread_controller.h"
|
|
|
|
#if (EFI_ONBOARD_MEMS_LIS2DW12 == TRUE)
|
|
#include "lis2dw12.h"
|
|
#endif
|
|
#if (EFI_ONBOARD_MEMS_LIS2DH12 == TRUE)
|
|
#include "lsm303agr.h"
|
|
#endif
|
|
|
|
/*
|
|
* SPI1 configuration structure.
|
|
* Speed 5.25MHz, CPHA=1, CPOL=1, 8bits frames, MSb transmitted first.
|
|
*/
|
|
static SPIConfig accelerometerSpiCfg = {
|
|
#if SPI_SUPPORTS_CIRCULAR == TRUE
|
|
.circular = FALSE,
|
|
#endif
|
|
.end_cb = NULL,
|
|
/* HW dependent part.*/
|
|
.ssport = NULL,
|
|
.sspad = 0,
|
|
.cr1 = SPI_CR1_BR_0 | SPI_CR1_BR_1 | SPI_CR1_CPOL | SPI_CR1_CPHA |
|
|
SPI_CR1_8BIT_MODE,
|
|
.cr2 = SPI_CR2_8BIT_MODE
|
|
};
|
|
|
|
|
|
#if 0
|
|
static OutputPin chipSelect;
|
|
#endif
|
|
|
|
#if (EFI_ONBOARD_MEMS_LIS2DW12 == TRUE)
|
|
|
|
static LIS2DW12Config lis2dw12cfg = {
|
|
#if LIS2DW12_USE_SPI
|
|
.spip = NULL,
|
|
.spicfg = &accelerometerSpiCfg,
|
|
#endif
|
|
#ifdef LIS2DW12_USE_I2C
|
|
/* TODO: */
|
|
#endif
|
|
.accsensitivity = NULL,
|
|
.accbias = NULL,
|
|
.accoutputdatarate = LIS2DW12_ACC_ODR_25HZ,
|
|
.accoutputresolution = LIS2DW12_ACC_OR_HP,
|
|
.acclowpowermode = LIS2DW12_ACC_LP_MODE2,
|
|
.accbadwidthselect = LIS2DW12_ACC_BW_ODR4,
|
|
.accfullscale = LIS2DW12_ACC_FS_4G,
|
|
};
|
|
|
|
/* LIS302DL Driver: This object represent an LIS302DL instance */
|
|
static LIS2DW12Driver LIS2DW12;
|
|
|
|
#endif //EFI_ONBOARD_MEMS_LIS2DW12 == TRUE
|
|
|
|
#if (EFI_ONBOARD_MEMS_LIS2DH12 == TRUE)
|
|
|
|
static LSM303AGRConfig lis2dh12cfg = {
|
|
#if LSM303AGR_USE_SPI
|
|
.spip = NULL,
|
|
.spicfg = &accelerometerSpiCfg,
|
|
#endif
|
|
.accsensitivity = NULL,
|
|
.accbias = NULL,
|
|
.accfullscale = LSM303AGR_ACC_FS_4G,
|
|
.accoutdatarate = LSM303AGR_ACC_ODR_50Hz,
|
|
#if LSM303AGR_USE_ADVANCED
|
|
.accmode = LSM303AGR_ACC_MODE_HRES,
|
|
.accblockdataupdate = LSM303AGR_ACC_BDU_CONT,
|
|
.accendianess = LSM303AGR_ACC_END_LITTLE,
|
|
#endif
|
|
/* LIS2DW12 is Accelerometer only, ignore following */
|
|
.compsensitivity = NULL,
|
|
.compbias = NULL,
|
|
.compoutputdatarate = LSM303AGR_COMP_ODR_50HZ,
|
|
#if LSM303AGR_USE_ADVANCED
|
|
.compmode = LSM303AGR_COMP_MODE_NORM,
|
|
.complp = LSM303AGR_COMP_LPOW_EN
|
|
#endif
|
|
};
|
|
|
|
/* LSM303AGR Driver: This object represent an LIS2DH12 instance */
|
|
static LSM303AGRDriver LIS2DH12;
|
|
|
|
#endif //EFI_ONBOARD_MEMS_LIS2DH12 == TRUE
|
|
|
|
class AccelController : public PeriodicController<UTILITY_THREAD_STACK_SIZE> {
|
|
public:
|
|
AccelController() : PeriodicController("Acc SPI") { }
|
|
private:
|
|
void PeriodicTask(efitick_t nowNt) override {
|
|
float acccooked[3];
|
|
|
|
#if (EFI_ONBOARD_MEMS_LIS2DW12 == TRUE)
|
|
lis2dw12AccelerometerReadCooked(&LIS2DW12, acccooked);
|
|
#endif
|
|
#if (EFI_ONBOARD_MEMS_LIS2DH12 == TRUE)
|
|
lsm303agrAccelerometerReadCooked(&LIS2DH12, acccooked);
|
|
#endif
|
|
|
|
if (engineConfiguration->useSpiImu) {
|
|
/* milli-G to G */
|
|
engine->sensors.accelerometer.lat = acccooked[0] / 1000.0;
|
|
engine->sensors.accelerometer.lon = acccooked[1] / 1000.0;
|
|
engine->sensors.accelerometer.vert = acccooked[2] / 1000.0;
|
|
}
|
|
}
|
|
};
|
|
|
|
static AccelController instance;
|
|
|
|
void initAccelerometer() {
|
|
static SPIDriver *bus;
|
|
msg_t ret = MSG_RESET;
|
|
|
|
#if HAL_USE_SPI
|
|
if (!isBrainPinValid(engineConfiguration->accelerometerCsPin))
|
|
return; // not used
|
|
|
|
bus = getSpiDevice(engineConfiguration->accelerometerSpiDevice);
|
|
if (bus == nullptr) {
|
|
// error already reported
|
|
return;
|
|
}
|
|
|
|
/* Commented until we have configureHellenMegaAccCS2Pin() */
|
|
#if 0
|
|
chipSelect.initPin("SPI Acc", engineConfiguration->accelerometerCsPin);
|
|
#endif
|
|
accelerometerSpiCfg.ssport = getHwPort("SPI Acc", engineConfiguration->accelerometerCsPin);
|
|
accelerometerSpiCfg.sspad = getHwPin("SPI Acc", engineConfiguration->accelerometerCsPin);
|
|
|
|
#if (EFI_ONBOARD_MEMS_LIS2DW12 == TRUE)
|
|
lis2dw12cfg.spip = bus;
|
|
|
|
/* LIS302DL Object Initialization.*/
|
|
lis2dw12ObjectInit(&LIS2DW12);
|
|
|
|
/* Activates the LIS302DL driver.*/
|
|
ret = lis2dw12Start(&LIS2DW12, &lis2dw12cfg);
|
|
#elif (EFI_ONBOARD_MEMS_LIS2DH12 == TRUE)
|
|
lis2dh12cfg.spip = bus;
|
|
|
|
/* LIS302DL Object Initialization.*/
|
|
lsm303agrObjectInit(&LIS2DH12);
|
|
|
|
/* Activates the LIS302DL driver.*/
|
|
ret = lsm303agrStart(&LIS2DH12, &lis2dh12cfg);
|
|
#else
|
|
fail("no MEMS type");
|
|
#endif /* EFI_ONBOARD_MEMS_LIS2DW12 == TRUE */
|
|
|
|
/* TODO: add support for LIS302 on discovery board */
|
|
|
|
if (ret == MSG_OK) {
|
|
/* 50 Hz */
|
|
instance.setPeriod(20 /*ms*/);
|
|
instance.start();
|
|
efiPrintf("accelerometer init OK");
|
|
} else {
|
|
efiPrintf("accelerometer init failed %d", ret);
|
|
}
|
|
#endif /* HAL_USE_SPI */
|
|
}
|
|
|
|
#endif /* EFI_ONBOARD_MEMS */
|