rusEFI
The most advanced open source ECU
Loading...
Searching...
No Matches
accelerometer.cpp
Go to the documentation of this file.
1/*
2 * @file accelerometer.cpp
3 *
4 * stm32f4discovery has MEMS LIS302DL
5 * www.st.com/resource/en/datasheet/lis302dl.pdf
6 *
7 * SPI1
8 * LIS302DL_SPI_SCK PA5
9 * LIS302DL_SPI_MISO PA6
10 * LIS302DL_SPI_MOSI PA7
11 * LIS302DL_SPI_CS_PIN PE3
12 *
13 *
14 *
15 * @date May 19, 2016
16 * @author Andrey Belomutskiy, (c) 2012-2020
17 */
18
19#include "pch.h"
20
21#include "accelerometer.h"
22#include "hardware.h"
23
24#if EFI_ONBOARD_MEMS
25#include "mpu_util.h"
26#include "lis302dl.h"
28
29#if (EFI_ONBOARD_MEMS_LIS2DW12 == TRUE)
30#include "lis2dw12.h"
31#endif
32#if (EFI_ONBOARD_MEMS_LIS2DH12 == TRUE)
33#include "lsm303agr.h"
34#endif
35#if (EFI_ONBOARD_MEMS_LIS302DL == TRUE)
36#include "lis302dl.h"
37#endif
38#if (EFI_ONBOARD_MEMS_LIS3DSH == TRUE)
39#include "lis3dsh.h"
40#endif
41
42/*
43 * SPI1 configuration structure.
44 * Speed 5.25MHz, CPHA=1, CPOL=1, 8bits frames, MSb transmitted first.
45 */
46static SPIConfig accelerometerSpiCfg = {
47#if SPI_SUPPORTS_CIRCULAR == TRUE
48 .circular = false,
49#endif
50#ifdef _CHIBIOS_RT_CONF_VER_6_1_
51 .end_cb = NULL,
52#else
53 .slave = false,
54 .data_cb = NULL,
55 .error_cb = NULL,
56#endif
57 /* HW dependent part.*/
58 .ssport = NULL,
59 .sspad = 0,
60#ifdef STM32H7XX
61 .cfg1 = SPI_CFG1_8BIT_MODE | SPI_CFG1_MBR_0 | SPI_CFG1_MBR_1,
62 .cfg2 = SPI_CFG2_8BIT_MODE | SPI_CFG2_CPOL | SPI_CFG2_CPHA
63#else
64 .cr1 = SPI_CR1_BR_0 | SPI_CR1_BR_1 | SPI_CR1_CPOL | SPI_CR1_CPHA |
65 SPI_CR1_8BIT_MODE,
66 .cr2 = SPI_CR2_8BIT_MODE
67#endif
68};
69
71
72#if (EFI_ONBOARD_MEMS_LIS2DW12 == TRUE)
73
74static LIS2DW12Config lis2dw12cfg = {
75#if LIS2DW12_USE_SPI
76 .spip = NULL,
77 .spicfg = &accelerometerSpiCfg,
78#endif
79#ifdef LIS2DW12_USE_I2C
80 /* TODO: */
81#endif
82 .accsensitivity = NULL,
83 .accbias = NULL,
84 .accodr = LIS2DW12_ACC_ODR_25HZ,
85 .accoutputresolution = LIS2DW12_ACC_OR_HP,
86 .acclowpowermode = LIS2DW12_ACC_LP_MODE2,
87 .accbadwidthselect = LIS2DW12_ACC_BW_ODR4,
88 .accfullscale = LIS2DW12_ACC_FS_4G,
89};
90
91/* LIS302DL Driver: This object represent an LIS302DL instance */
92static LIS2DW12Driver LIS2DW12;
93
94#endif //EFI_ONBOARD_MEMS_LIS2DW12 == TRUE
95
96#if (EFI_ONBOARD_MEMS_LIS2DH12 == TRUE)
97
98static LSM303AGRConfig lis2dh12cfg = {
99#if LSM303AGR_USE_SPI
100 .spip = NULL,
101 .spicfg = &accelerometerSpiCfg,
102#endif
103 .accsensitivity = NULL,
104 .accbias = NULL,
105 .accfullscale = LSM303AGR_ACC_FS_4G,
106 .accodr = LSM303AGR_ACC_ODR_50Hz,
107#if LSM303AGR_USE_ADVANCED
108 .accmode = LSM303AGR_ACC_MODE_HRES,
109 .accblockdataupdate = LSM303AGR_ACC_BDU_CONT,
110 .accendianess = LSM303AGR_ACC_END_LITTLE,
111#endif
112 /* LIS2DW12 is Accelerometer only, ignore following */
113 .compsensitivity = NULL,
114 .compbias = NULL,
115 .compodr = LSM303AGR_COMP_ODR_50HZ,
116#if LSM303AGR_USE_ADVANCED
117 .compmode = LSM303AGR_COMP_MODE_NORM,
118 .complp = LSM303AGR_COMP_LPOW_EN
119#endif
120};
121
122/* LSM303AGR Driver: This object represent an LIS2DH12 instance */
123static LSM303AGRDriver LIS2DH12;
124
125#endif //EFI_ONBOARD_MEMS_LIS2DH12 == TRUE
126
127#if (EFI_ONBOARD_MEMS_LIS302DL == TRUE)
128static LIS302DLConfig lis302dlcfg ={
129#if LIS302DL_USE_SPI
130 .spip = NULL,
131 .spicfg = &accelerometerSpiCfg,
132#endif
133#if LIS302DL_USE_I2C
134 /* TODO: */
135#endif
136 .accsensitivity = NULL,
137 .accbias = NULL,
138 .accfullscale = LIS302DL_ACC_FS_8G,
139 .accodr = LIS302DL_ACC_ODR_100HZ,
140#if LIS302DL_USE_ADVANCED
141 .acchighpass = LIS302DL_ACC_HP_0,
142#endif
143};
144
145static LIS302DLDriver LIS302DL;
146
147#endif //EFI_ONBOARD_MEMS_LIS302DL == TRUE
148
149#if (EFI_ONBOARD_MEMS_LIS3DSH == TRUE)
150
151static LIS3DSHConfig lis3dshcfg ={
152#if LIS3DSH_USE_SPI
153 .spip = NULL,
154 .spicfg = &accelerometerSpiCfg,
155#endif
156 .accsensitivity = NULL,
157 .accbias = NULL,
158 .accfullscale = LIS3DSH_ACC_FS_4G,
159 .accodr = LIS3DSH_ACC_ODR_50HZ,
160#if LIS3DSH_USE_ADVANCED
161 .accantialiasing = LIS3DSH_ACC_BW_50HZ,
162 .accblockdataupdate = LIS3DSH_ACC_BDU_CONTINUOUS,
163#endif
164};
165
166static LIS3DSHDriver LIS3DSH;
167
168#endif //EFI_ONBOARD_MEMS_LIS3DSH == TRUE
169
173 ACCEL_LIS2DH12, // Same as LSM303
174 ACCEL_LIS302DL, // STM32F4DISCOVERY (old?)
175 ACCEL_LIS3DSH, // STM32F4DISCOVERY
176};
177
179
180class AccelController : public PeriodicController<UTILITY_THREAD_STACK_SIZE> {
181public:
182 AccelController() : PeriodicController("Acc SPI") { }
183private:
184 void PeriodicTask(efitick_t nowNt) override {
185 UNUSED(nowNt);
186
187 msg_t ret = MSG_RESET;
188 float acccooked[3];
189
190 #if (EFI_ONBOARD_MEMS_LIS2DW12 == TRUE)
191 if (AccelType == ACCEL_LIS2DW12) {
192 ret = lis2dw12AccelerometerReadCooked(&LIS2DW12, acccooked);
193 }
194 #endif
195 #if (EFI_ONBOARD_MEMS_LIS2DH12 == TRUE)
196 if (AccelType == ACCEL_LIS2DH12) {
197 ret = lsm303agrAccelerometerReadCooked(&LIS2DH12, acccooked);
198 }
199 #endif
200 #if (EFI_ONBOARD_MEMS_LIS302DL == TRUE)
201 if (AccelType == ACCEL_LIS302DL) {
202 ret = lis302dlAccelerometerReadCooked(&LIS302DL, acccooked);
203 }
204 #endif
205 #if (EFI_ONBOARD_MEMS_LIS3DSH == TRUE)
206 if (AccelType == ACCEL_LIS3DSH) {
207 ret = lis3dshAccelerometerReadCooked(&LIS3DSH, acccooked);
208 }
209 #endif
210
211 if ((engineConfiguration->useSpiImu) && (ret == MSG_OK)) {
212 /* milli-G to G */
213 engine->sensors.accelerometer.lat = acccooked[0] / 1000.0;
214 engine->sensors.accelerometer.lon = acccooked[1] / 1000.0;
215 engine->sensors.accelerometer.vert = acccooked[2] / 1000.0;
216 }
217 }
218};
219
220static AccelController instance;
221
223#if HAL_USE_SPI
224 static SPIDriver *bus;
225 msg_t ret = MSG_RESET;
226
228 return; // not used
229
231 if (bus == nullptr) {
232 // error already reported
233 return;
234 }
235
236 /* so far only Hellen boards share SPI device for SD card and accelerometer
237 * thus need to make sure CS pin is in a well known proper state */
240 }
243
244 /* Try to detect any of enabled accels */
245 /* Hope all device drivers know how to detect correct chip */
246#if (EFI_ONBOARD_MEMS_LIS2DW12 == TRUE)
247 if (ret != MSG_OK) {
248 lis2dw12cfg.spip = bus;
249
250 /* LIS2DW12 Object Initialization.*/
251 lis2dw12ObjectInit(&LIS2DW12);
252
253 /* Activates the LIS2DW12 driver.*/
254 ret = lis2dw12Start(&LIS2DW12, &lis2dw12cfg);
255 if (ret == MSG_OK) {
257 }
258 }
259#endif //EFI_ONBOARD_MEMS_LIS2DW12 == TRUE
260#if (EFI_ONBOARD_MEMS_LIS2DH12 == TRUE)
261 if (ret != MSG_OK) {
262 lis2dh12cfg.spip = bus;
263
264 /* LIS2DH12 Object Initialization.*/
265 lsm303agrObjectInit(&LIS2DH12);
266
267 /* Activates the LIS2DH12 driver.*/
268 ret = lsm303agrStart(&LIS2DH12, &lis2dh12cfg);
269 if (ret == MSG_OK) {
271 }
272 }
273#endif //EFI_ONBOARD_MEMS_LIS2DH12 == TRUE
274#if (EFI_ONBOARD_MEMS_LIS302DL == TRUE)
275 if (ret != MSG_OK) {
276 lis302dlcfg.spip = bus;
277
278 /* LIS302DL Object Initialization.*/
279 lis302dlObjectInit(&LIS302DL);
280
281 /* Activates the LIS302DL driver.*/
282 ret = lis302dlStart(&LIS302DL, &lis302dlcfg);
283 if (ret == MSG_OK) {
285 }
286 }
287#endif //EFI_ONBOARD_MEMS_LIS302DL == TRUE
288#if (EFI_ONBOARD_MEMS_LIS3DSH == TRUE)
289 if (ret != MSG_OK) {
290 lis3dshcfg.spip = bus;
291
292 /* LIS3DSH Object Initialization.*/
293 lis3dshObjectInit(&LIS3DSH);
294
295 /* Activates the LIS3DSH driver.*/
296 ret = lis3dshStart(&LIS3DSH, &lis3dshcfg);
297 if (ret == MSG_OK) {
299 }
300 }
301#endif //EFI_ONBOARD_MEMS_LIS3DSH == TRUE
302
303 if (ret == MSG_OK) {
304 /* 50 Hz */
305 instance.setPeriod(20 /*ms*/);
306 instance.start();
307 efiPrintf("accelerometer init OK");
308 } else {
309 efiPrintf("accelerometer init failed %d", (int)ret);
310 }
311#endif /* HAL_USE_SPI */
312}
313
314#endif /* EFI_ONBOARD_MEMS */
AccelType_t
@ ACCEL_LIS2DH12
@ ACCEL_UNK
@ ACCEL_LIS3DSH
@ ACCEL_LIS302DL
@ ACCEL_LIS2DW12
static LIS2DW12Driver LIS2DW12
void initAccelerometer()
static LIS3DSHConfig lis3dshcfg
OutputPin accelerometerChipSelect
static LIS2DW12Config lis2dw12cfg
static AccelController instance
static LIS3DSHDriver LIS3DSH
static LIS302DLDriver LIS302DL
static SPIConfig accelerometerSpiCfg
static LSM303AGRDriver LIS2DH12
static AccelType_t AccelType
static LIS302DLConfig lis302dlcfg
static LSM303AGRConfig lis2dh12cfg
SensorsState sensors
Definition engine.h:353
Single output pin reference and state.
Definition efi_output.h:49
void initPin(const char *msg, brain_pin_e brainPin, pin_output_mode_e outputMode, bool forceInitWithFatalError=false)
Definition efi_gpio.cpp:711
bool isInitialized() const
Definition efi_gpio.cpp:559
Base class for a controller that needs to run periodically to perform work.
virtual void PeriodicTask(efitick_t nowNt)=0
Called periodically. Override this method to do work for your controller.
ioportid_t getHwPort(const char *msg, brain_pin_e brainPin)
ioportmask_t getHwPin(const char *msg, brain_pin_e brainPin)
static EngineAccessor engine
Definition engine.h:413
static constexpr engine_configuration_s * engineConfiguration
SPIDriver * getSpiDevice(spi_device_e spiDevice)
Definition hardware.cpp:149
UNUSED(samplingTimeSeconds)
bool isBrainPinValid(brain_pin_e brainPin)
Accelerometer accelerometer