Skip to content

Commit 416b271

Browse files
dnakhoodadnakhooda
committed
stuff
1 parent cc9a378 commit 416b271

File tree

3 files changed

+137
-92
lines changed

3 files changed

+137
-92
lines changed

‎CMakeLists.txt‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -65,8 +65,8 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
6565
"./Drivers/Embedded-Base/middleware/src/c_utils.c"
6666

6767
# Peripheral Drivers
68-
"../Drivers/Embedded-Base/general/src/lsm6dso_reg.c"
69-
"../Drivers/Embedded-Base/general/src/lsm6dso.c"
68+
"../Drivers/Embedded-Base/general/include/lsm6dsv_reg.h"
69+
"../Drivers/Embedded-Base/general/src/lsm6dsv_reg.c"
7070
"./Drivers/Embedded-Base/general/src/as3935.c"
7171
"../Drivers/Embedded-Base/general/src/lis2mdl_reg.c"
7272

‎Core/Src/u_sensors.c‎

Lines changed: 134 additions & 89 deletions
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,7 @@
44

55
#include "c_utils.h"
66
#include "as3935.h"
7-
#include "lsm6dso.h"
7+
#include "lsm6dsv_reg.h"
88
#include "lis2mdl_reg.h"
99
#include "u_can.h"
1010
#include "u_tx_debug.h"
@@ -14,153 +14,210 @@ extern SPI_HandleTypeDef hspi1; // imu
1414
extern SPI_HandleTypeDef hspi2; // lightning sensor
1515
extern SPI_HandleTypeDef hspi3; // magnetometer
1616

17-
static LSM6DSO_Object_t imu;
17+
static stmdev_ctx_t imu;
1818
static as3935_t *as3935 = NULL;
1919
static stmdev_ctx_t *lis2mdl_ctx = NULL;
2020

21+
static int16_t _float_to_int16(float value) {
22+
if (value > 32767.0f) {
23+
return 32767;
24+
}
25+
26+
if (value < -32768.0f) {
27+
return -32768;
28+
}
29+
30+
return (int16_t) value;
31+
}
2132

2233
/**
2334
* IMU
2435
*/
2536

26-
static int32_t _lsm6dso_read(uint16_t device_address, uint16_t register_address, uint8_t *data, uint16_t length) {
37+
typedef struct {
38+
float x;
39+
float y;
40+
float z;
41+
} LSM6DSV_Axes_t;
42+
43+
static int32_t _lsm6dsv_read(void *handle, uint8_t register_address, uint8_t *data, uint16_t length) {
2744
/* For SPI reads, set MSB = 1 for read operation. */
2845
uint8_t spi_reg = (uint8_t)(register_address | 0x80);
2946
HAL_StatusTypeDef status;
47+
SPI_HandleTypeDef *spi_handle = (SPI_HandleTypeDef *) handle;
3048

3149
/* Send the register address we're trying to read from. */
32-
status = HAL_SPI_Transmit(&hspi1, &spi_reg, sizeof(spi_reg), HAL_MAX_DELAY);
50+
status = HAL_SPI_Transmit(spi_handle, &spi_reg, sizeof(spi_reg), HAL_MAX_DELAY);
3351
if(status != HAL_OK) {
3452
PRINTLN_INFO("ERROR: Failed to send register address to lsm6dso over SPI (Status: %d/%s).", status, hal_status_toString(status));
35-
return LSM6DSO_ERROR;
53+
return -1;
3654
}
3755

3856
/* Receive the data. */
39-
status = HAL_SPI_Receive(&hspi1, data, length, HAL_MAX_DELAY);
57+
status = HAL_SPI_Receive(spi_handle, data, length, HAL_MAX_DELAY);
4058
if(status != HAL_OK) {
4159
PRINTLN_INFO("ERROR: Failed to read from the lsm6dso over SPI (Status: %d/%s).", status, hal_status_toString(status));
42-
return LSM6DSO_ERROR;
60+
return -1;
4361
}
4462

45-
return LSM6DSO_OK;
63+
return 0;
4664
}
4765

48-
static int32_t _lsm6dso_write(uint16_t device_address, uint16_t register_address, uint8_t *data, uint16_t length) {
66+
static int32_t _lsm6dsv_write(void *handle, uint8_t register_address, uint8_t *data, uint16_t length) {
4967
/* For SPI writes, clear MSB = 0 for write operation. */
5068
uint8_t spi_reg = (uint8_t)(register_address & 0x7F);
5169
HAL_StatusTypeDef status;
70+
SPI_HandleTypeDef *spi_handle = (SPI_HandleTypeDef *) handle;
5271

5372
/* Send register address. */
54-
status = HAL_SPI_Transmit(&hspi1, &spi_reg, sizeof(spi_reg), HAL_MAX_DELAY);
73+
status = HAL_SPI_Transmit(spi_handle, &spi_reg, sizeof(spi_reg), HAL_MAX_DELAY);
5574
if(status != HAL_OK) {
5675
PRINTLN_INFO("ERROR: Failed to send register address to lsm6dso over SPI (Status: %d/%s).", status, hal_status_toString(status));
57-
return LSM6DSO_ERROR;
76+
return -1;
5877
}
5978

6079
/* Send data. */
61-
status = HAL_SPI_Transmit(&hspi1, data, length, HAL_MAX_DELAY);
80+
status = HAL_SPI_Transmit(spi_handle, data, length, HAL_MAX_DELAY);
6281
if(status != HAL_OK) {
6382
PRINTLN_INFO("ERROR: Failed to write to the lsm6dso over SPI (Status: %d/%s).", status, hal_status_toString(status));
64-
return LSM6DSO_ERROR;
83+
return -1;
6584
}
6685

67-
return LSM6DSO_OK;
68-
}
69-
70-
int32_t _get_tick(void) {
71-
return (int32_t)HAL_GetTick();
86+
return 0;
7287
}
7388

7489
void _delay(uint32_t delay) {
7590
return HAL_Delay(delay);
7691
}
7792

78-
uint16_t imu_getAccelerometerData(LSM6DSO_Axes_t *axes) {
79-
int status = LSM6DSO_ACC_GetAxes(&imu, axes);
80-
if(status != LSM6DSO_OK) {
81-
PRINTLN_INFO("ERROR: Failed to call LSM6DSO_ACC_GetAxes() (Status: %d).", status);
93+
uint16_t imu_getAccelerometerData(LSM6DSV_Axes_t *axes) {
94+
int16_t buf[3];
95+
96+
int status = lsm6dsv_acceleration_raw_get(&imu, buf);
97+
98+
if(status != 0) {
99+
PRINTLN_INFO("ERROR: Failed to call lsm6dsv_acceleration_raw_get() (Status: %d).", status);
82100
return U_ERROR;
83101
}
102+
103+
axes->x = lsm6dsv_from_fs2_to_mg(buf[0]);
104+
axes->y = lsm6dsv_from_fs2_to_mg(buf[1]);
105+
axes->z = lsm6dsv_from_fs2_to_mg(buf[2]);
106+
84107
return U_SUCCESS;
85108
}
86109

87-
uint16_t imu_getGyroscopeData(LSM6DSO_Axes_t *axes) {
88-
int status = LSM6DSO_GYRO_GetAxes(&imu, axes);
89-
if(status != LSM6DSO_OK) {
90-
PRINTLN_INFO("ERROR: Failed to call LSM6DSO_GYRO_GetAxes() (Status: %d).", status);
110+
uint16_t imu_getGyroscopeData(LSM6DSV_Axes_t *axes) {
111+
int16_t buf[3];
112+
113+
int status = lsm6dsv_angular_rate_raw_get(&imu, buf);
114+
115+
if(status != 0) {
116+
PRINTLN_INFO("ERROR: Failed to call lsm6dso_angular_rate_raw_get() (Status: %d).", status);
91117
return U_ERROR;
92118
}
119+
120+
axes->x = lsm6dsv_from_fs250_to_mdps(buf[0]);
121+
axes->y = lsm6dsv_from_fs250_to_mdps(buf[1]);
122+
axes->z = lsm6dsv_from_fs250_to_mdps(buf[2]);
123+
93124
return U_SUCCESS;
94125
}
95126

96-
/**
97-
* @brief initializes imu for reading
98-
* @return returns tx status for errors
99-
*/
100127
uint16_t init_imu() {
128+
uint8_t whoami;
101129

102-
LSM6DSO_IO_t io_config = {
103-
.BusType = LSM6DSO_SPI_4WIRES_BUS,
104-
.WriteReg = _lsm6dso_write,
105-
.ReadReg = _lsm6dso_read,
106-
.GetTick = _get_tick,
107-
.Delay = _delay
108-
};
109-
110-
int status = LSM6DSO_RegisterBusIO(&imu, &io_config);
111-
if(status != LSM6DSO_OK) {
112-
PRINTLN_INFO("ERROR: Failed to call LSM6DSO_RegisterBusIO (Status: %d).", status);
130+
imu.read_reg = _lsm6dsv_read;
131+
imu.read_reg = _lsm6dsv_write;
132+
imu.mdelay = _delay;
133+
imu.handle = &hspi1;
134+
135+
int status = lis2mdl_device_id_get(&imu, &whoami);
136+
if(status != 0) {
137+
PRINTLN_ERROR("ERROR: Failed to call lis2mdl_device_id_get (Status: %d).", status);
138+
return U_ERROR;
139+
}
140+
141+
if (whoami != LIS2MDL_ID) {
142+
PRINTLN_ERROR("ERROR: Failed whoami (Status: %d).", status);
143+
return U_ERROR;
144+
}
145+
146+
status = lsm6dsv_sw_reset(&imu);
147+
if (status != 0) {
148+
PRINTLN_ERROR("ERROR: failed lsm6dsv_sw_reset (Status: %d).", status);
149+
return U_ERROR;
150+
}
151+
152+
/** TODO: just picked one that sounds good. double check this */
153+
status = lsm6dsv_xl_mode_set(&imu, LSM6DSV_XL_HIGH_PERFORMANCE_MD);
154+
if (status != 0) {
155+
PRINTLN_ERROR("ERROR: failed lsm6dsv_xl_mode_set (Status: %d).", status);
156+
return U_ERROR;
157+
}
158+
159+
/** TODO: just picked one that sounds good. double check this */
160+
status = lsm6dsv_xl_data_rate_set(&imu, LSM6DSV_EIS_960Hz);
161+
if (status != 0) {
162+
PRINTLN_ERROR("ERROR: failed lsm6dsv_xl_data_rate_set (Status: %d).", status);
163+
return U_ERROR;
164+
}
165+
166+
/** TODO: just picked one that sounds good. double check this */
167+
status = lsm6dsv_xl_data_rate_set(&imu, LSM6DSV_EIS_960Hz);
168+
if (status != 0) {
169+
PRINTLN_ERROR("ERROR: failed lsm6dsv_xl_data_rate_set (Status: %d).", status);
113170
return U_ERROR;
114171
}
115172

116-
/* Initialize IMU. */
117-
status = LSM6DSO_Init(&imu);
118-
if(status != LSM6DSO_OK) {
119-
PRINTLN_INFO("ERROR: Failed to run LSM6DS0_Init() (Status: %d).", status);
173+
/** TODO: just picked one that sounds good. double check this */
174+
status = lsm6dsv_xl_full_scale_set(&imu, LSM6DSV_OIS_2g);
175+
if (status != 0) {
176+
PRINTLN_ERROR("ERROR: failed lsm6dsv_xl_full_scale_set (Status: %d).", status);
120177
return U_ERROR;
121178
}
122179

123-
/* Setup IMU Accelerometer - default 104Hz */
124-
status = LSM6DSO_ACC_SetOutputDataRate_With_Mode(&imu, 833.0f, LSM6DSO_ACC_HIGH_PERFORMANCE_MODE);
125-
if(status != LSM6DSO_OK) {
126-
PRINTLN_INFO("ERROR: Failed to run LSM6DSO_ACC_SetOutputDataRate_With_Mode (Status: %d).", status);
180+
/** TODO: just picked one that sounds good. double check this */
181+
status = lsm6dsv_filt_xl_lp2_set(&imu, 1);
182+
if (status != 0) {
183+
PRINTLN_ERROR("ERROR: failed lsm6dsv_filt_xl_lp2_set (Status: %d).", status);
127184
return U_ERROR;
128185
}
129186

130-
/* Enable Accelerometer. */
131-
status = LSM6DSO_ACC_Enable(&imu);
132-
if(status != LSM6DSO_OK) {
133-
PRINTLN_INFO("ERROR: Failed to run LSM6DSO_ACC_Enable() (Status: %d).", status);
187+
/** TODO: just picked one that sounds good. double check this */
188+
status = lsm6dsv_filt_xl_lp2_bandwidth_set(&imu, LSM6DSV_OIS_XL_LP_ULTRA_LIGHT);
189+
if (status != 0) {
190+
PRINTLN_ERROR("ERROR: failed lsm6dsv_filt_xl_lp2_bandwidth_set (Status: %d).", status);
134191
return U_ERROR;
135192
}
136-
137-
/* Set Accelerometer Filter Mode. */
138-
status = LSM6DSO_ACC_Set_Filter_Mode(&imu, 0, 3); // 3 = 'LSM6DSO_LP_ODR_DIV_45'
139-
if(status != LSM6DSO_OK) {
140-
PRINTLN_INFO("ERROR: Failed to run LSM6DSO_ACC_Set_Filter_Mode() (Status: %d).", status);
193+
194+
/** TODO: just picked one that sounds good. double check this */
195+
status = lsm6dsv_gy_mode_set(&imu, LSM6DSV_GY_HIGH_PERFORMANCE_MD);
196+
if (status != 0) {
197+
PRINTLN_ERROR("ERROR: failed lsm6dsv_gy_mode_set (Status: %d).", status);
141198
return U_ERROR;
142199
}
143200

144-
/* Setup IMU Gyroscope */
145-
status = LSM6DSO_GYRO_SetOutputDataRate_With_Mode(&imu, 104.0f, LSM6DSO_GYRO_HIGH_PERFORMANCE_MODE);
146-
if(status != LSM6DSO_OK) {
147-
PRINTLN_INFO("ERROR: Failed to run LSM6DSO_GYRO_SetOutputDataRate_With_Mode() (Status: %d).", status);
201+
/** TODO: just picked one that sounds good. double check this */
202+
status = lsm6dsv_gy_data_rate_set(&imu, LSM6DSV_ODR_AT_120Hz);
203+
if (status != 0) {
204+
PRINTLN_ERROR("ERROR: failed lsm6dsv_gy_data_rate_set (Status: %d).", status);
148205
return U_ERROR;
149206
}
150207

151-
/* Enable IMU Gyroscope. */
152-
status = LSM6DSO_GYRO_Enable(&imu);
153-
if(status != LSM6DSO_OK) {
154-
PRINTLN_INFO("ERROR: Failed to run LSM6DSO_GYRO_Enable() (Status: %d).", status);
208+
/** TODO: just picked one that sounds good. double check this */
209+
status = lsm6dsv_gy_full_scale_set(&imu, LSM6DSV_250dps);
210+
if (status != 0) {
211+
PRINTLN_ERROR("ERROR: failed lsm6dsv_gy_full_scale_set (Status: %d).", status);
155212
return U_ERROR;
156213
}
157214

158215
return U_SUCCESS;
159216
}
160217

161218
uint16_t read_imu() {
162-
LSM6DSO_Axes_t accel_axes;
163-
LSM6DSO_Axes_t gyro_axes;
219+
LSM6DSV_Axes_t accel_axes;
220+
LSM6DSV_Axes_t gyro_axes;
164221

165222
if (imu_getAccelerometerData(&accel_axes) != U_SUCCESS) {
166223
return U_ERROR;
@@ -176,19 +233,19 @@ uint16_t read_imu() {
176233
int16_t accel_z;
177234
} accel_data;
178235

179-
accel_data.accel_x = accel_axes.x;
180-
accel_data.accel_y = accel_axes.y;
181-
accel_data.accel_z = accel_axes.z;
236+
accel_data.accel_x = _float_to_int16(accel_axes.x * 1000);
237+
accel_data.accel_y = _float_to_int16(accel_axes.y * 1000);
238+
accel_data.accel_z = _float_to_int16(accel_axes.z * 1000);
182239

183240
struct __attribute__((__packed__)) {
184241
int16_t gyro_x;
185242
int16_t gyro_y;
186243
int16_t gyro_z;
187244
} gyro_data;
188245

189-
gyro_data.gyro_x = gyro_axes.x;
190-
gyro_data.gyro_y = gyro_axes.y;
191-
gyro_data.gyro_z = gyro_axes.z;
246+
gyro_data.gyro_x = _float_to_int16(gyro_axes.x * 1000);
247+
gyro_data.gyro_y = _float_to_int16(gyro_axes.y * 1000);
248+
gyro_data.gyro_z = _float_to_int16(gyro_axes.z * 1000);
192249

193250
can_msg_t imu_accel_msg = { .id = 0xAA,
194251
.len = 6,
@@ -331,18 +388,6 @@ uint16_t init_magnetometer() {
331388
return U_SUCCESS;
332389
}
333390

334-
static int16_t float_to_int16_saturate(float value) {
335-
if (value > 32767.0f) {
336-
return 32767;
337-
}
338-
339-
if (value < -32768.0f) {
340-
return -32768;
341-
}
342-
343-
return (int16_t) value;
344-
}
345-
346391
uint16_t read_magnetometer() {
347392
if (lis2mdl_ctx == NULL) {
348393
return U_ERROR;
@@ -364,9 +409,9 @@ uint16_t read_magnetometer() {
364409
int16_t axes_3;
365410
} axes_data;
366411

367-
axes_data.axes_1 = float_to_int16_saturate(lis2mdl_from_lsb_to_mgauss(raw_axes[0]) * 1000.0f);
368-
axes_data.axes_2 = float_to_int16_saturate(lis2mdl_from_lsb_to_mgauss(raw_axes[1]) * 1000.0f);
369-
axes_data.axes_3 = float_to_int16_saturate(lis2mdl_from_lsb_to_mgauss(raw_axes[2]) * 1000.0f);
412+
axes_data.axes_1 = _float_to_int16(lis2mdl_from_lsb_to_mgauss(raw_axes[0]) * 1000.0f);
413+
axes_data.axes_2 = _float_to_int16(lis2mdl_from_lsb_to_mgauss(raw_axes[1]) * 1000.0f);
414+
axes_data.axes_3 = _float_to_int16(lis2mdl_from_lsb_to_mgauss(raw_axes[2]) * 1000.0f);
370415

371416
can_msg_t message = { .id = 0xAD, .len = 6, .data = { 0 } };
372417

‎Drivers/Embedded-Base‎

0 commit comments

Comments
 (0)