From 7b5bc3c6b06fb62f8eb48ab65d362192e38fe4dc Mon Sep 17 00:00:00 2001 From: Myers-Ty Date: Thu, 5 Dec 2024 20:42:24 -0500 Subject: [PATCH] Clang formatting --- Core/Src/monitor.c | 406 +++++++++++++++++++++------------------------ Core/Src/msb.c | 207 +++++++++++------------ 2 files changed, 291 insertions(+), 322 deletions(-) diff --git a/Core/Src/monitor.c b/Core/Src/monitor.c index 7b9bb76..ed10477 100644 --- a/Core/Src/monitor.c +++ b/Core/Src/monitor.c @@ -14,294 +14,274 @@ extern device_loc_t device_loc; -uint16_t convert_can(uint16_t original_value, device_loc_t mode) -{ - switch (mode) { - case DEVICE_FRONT_LEFT: - return original_value; - case DEVICE_FRONT_RIGHT: - return original_value + 0x20; - case DEVICE_BACK_LEFT: - return original_value + 0x40; - case DEVICE_BACK_RIGHT: - return original_value + 0x60; - default: - return original_value; - } +uint16_t convert_can(uint16_t original_value, device_loc_t mode) { + switch (mode) { + case DEVICE_FRONT_LEFT: + return original_value; + case DEVICE_FRONT_RIGHT: + return original_value + 0x20; + case DEVICE_BACK_LEFT: + return original_value + 0x40; + case DEVICE_BACK_RIGHT: + return original_value + 0x60; + default: + return original_value; + } } #ifdef SENSOR_TEMP osThreadId_t temp_monitor_handle; const osThreadAttr_t temp_monitor_attributes = { - .name = "TempMonitor", - .stack_size = 64 * 16, - .priority = (osPriority_t)osPriorityHigh1, + .name = "TempMonitor", + .stack_size = 64 * 16, + .priority = (osPriority_t)osPriorityHigh1, }; -void vTempMonitor(void *pv_params) -{ - can_msg_t temp_sensor_msg = { .id = convert_can(CANID_TEMP_SENSOR, - device_loc), - .len = 4, - .data = { 0 } }; +void vTempMonitor(void *pv_params) { + can_msg_t temp_sensor_msg = { + .id = convert_can(CANID_TEMP_SENSOR, device_loc), .len = 4, .data = {0}}; - struct __attribute__((__packed__)) { - uint16_t temp; - uint16_t humidity; - } temp_sensor_data; + struct __attribute__((__packed__)) { + uint16_t temp; + uint16_t humidity; + } temp_sensor_data; - uint16_t temp_dat = 0; - uint16_t humidity_dat = 0; + uint16_t temp_dat = 0; + uint16_t humidity_dat = 0; - for (;;) { - if (central_temp_measure(&temp_dat, &humidity_dat)) { - printf("Failed to get temp\r\n"); - } + for (;;) { + if (central_temp_measure(&temp_dat, &humidity_dat)) { + printf("Failed to get temp\r\n"); + } - temp_sensor_data.temp = temp_dat; - temp_sensor_data.humidity = humidity_dat; + temp_sensor_data.temp = temp_dat; + temp_sensor_data.humidity = humidity_dat; #ifdef LOG_VERBOSE - printf("Board Temperature:\t%d\r\n", temp_sensor_data.temp); - printf("Board Humidity:\t%d\r\n", temp_sensor_data.humidity); + printf("Board Temperature:\t%d\r\n", temp_sensor_data.temp); + printf("Board Humidity:\t%d\r\n", temp_sensor_data.humidity); #endif - endian_swap(&temp_sensor_data.temp, - sizeof(temp_sensor_data.temp)); - endian_swap(&temp_sensor_data.humidity, - sizeof(temp_sensor_data.humidity)); - - memcpy(temp_sensor_msg.data, &temp_sensor_data, - temp_sensor_msg.len); - /* Send CAN message */ - if (queue_can_msg(temp_sensor_msg)) { - printf("Failed to send CAN message\r\n"); - } - - /* Yield to other tasks */ - osDelay(DELAY_TEMP_SENSOR_REFRESH); - } + endian_swap(&temp_sensor_data.temp, sizeof(temp_sensor_data.temp)); + endian_swap(&temp_sensor_data.humidity, sizeof(temp_sensor_data.humidity)); + + memcpy(temp_sensor_msg.data, &temp_sensor_data, temp_sensor_msg.len); + /* Send CAN message */ + if (queue_can_msg(temp_sensor_msg)) { + printf("Failed to send CAN message\r\n"); + } + + /* Yield to other tasks */ + osDelay(DELAY_TEMP_SENSOR_REFRESH); + } } #endif #ifdef SENSOR_IMU osThreadId_t imu_monitor_handle; const osThreadAttr_t imu_monitor_attributes = { - .name = "IMUMonitor", - .stack_size = 128 * 8, - .priority = (osPriority_t)osPriorityHigh, + .name = "IMUMonitor", + .stack_size = 128 * 8, + .priority = (osPriority_t)osPriorityHigh, }; -void vIMUMonitor(void *pv_params) -{ - //const uint8_t num_samples = 10; - can_msg_t imu_accel_msg = { .id = convert_can(CANID_IMU_ACCEL, - device_loc), - .len = 6, - .data = { 0 } }; - can_msg_t imu_gyro_msg = { .id = convert_can(CANID_IMU_GYRO, - device_loc), - .len = 6, - .data = { 0 } }; - - struct __attribute__((__packed__)) { - int16_t accel_x; - int16_t accel_y; - int16_t accel_z; - } accel_data; - - struct __attribute__((__packed__)) { - int16_t gyro_x; - int16_t gyro_y; - int16_t gyro_z; - } gyro_data; - - struct __attribute__((__packed__)) { - float_t temp; - } temperature_data; - - stmdev_ctx_t ctx; - stmdev_ctx_t aux_ctx; - //int16_t temperature_data_temp; - - lsm6dso_md_t imu_md_temp; - lsm6dso_data_t imu_data_temp; - - /* Add parameters for formatting data */ - imu_md_temp.ui.gy.fs = LSM6DSO_500dps; - imu_md_temp.ui.gy.odr = LSM6DSO_GY_UI_52Hz_LP; - imu_md_temp.ui.xl.fs = LSM6DSO_XL_UI_2g; - imu_md_temp.ui.xl.odr = LSM6DSO_XL_UI_52Hz_LP; - - for (;;) { - /* Take measurement */ - if (imu_data_get(&ctx, &aux_ctx, &imu_md_temp, &imu_data_temp)) { - printf("Failed to get IMU data \r\n"); - } - - /* Run values through LPF of sample size */ - accel_data.accel_x = imu_data_temp.ui.xl.mg[0]; - accel_data.accel_y = imu_data_temp.ui.xl.mg[1]; - accel_data.accel_z = imu_data_temp.ui.xl.mg[2]; - gyro_data.gyro_x = imu_data_temp.ui.gy.mdps[0]; - gyro_data.gyro_y = imu_data_temp.ui.gy.mdps[1]; - gyro_data.gyro_z = imu_data_temp.ui.gy.mdps[2]; - temperature_data.temp = imu_data_temp.ui.heat.deg_c; +void vIMUMonitor(void *pv_params) { + // const uint8_t num_samples = 10; + can_msg_t imu_accel_msg = { + .id = convert_can(CANID_IMU_ACCEL, device_loc), .len = 6, .data = {0}}; + can_msg_t imu_gyro_msg = { + .id = convert_can(CANID_IMU_GYRO, device_loc), .len = 6, .data = {0}}; + + struct __attribute__((__packed__)) { + int16_t accel_x; + int16_t accel_y; + int16_t accel_z; + } accel_data; + + struct __attribute__((__packed__)) { + int16_t gyro_x; + int16_t gyro_y; + int16_t gyro_z; + } gyro_data; + + struct __attribute__((__packed__)) { + float_t temp; + } temperature_data; + + stmdev_ctx_t ctx; + stmdev_ctx_t aux_ctx; + // int16_t temperature_data_temp; + + lsm6dso_md_t imu_md_temp; + lsm6dso_data_t imu_data_temp; + + /* Add parameters for formatting data */ + imu_md_temp.ui.gy.fs = LSM6DSO_500dps; + imu_md_temp.ui.gy.odr = LSM6DSO_GY_UI_52Hz_LP; + imu_md_temp.ui.xl.fs = LSM6DSO_XL_UI_2g; + imu_md_temp.ui.xl.odr = LSM6DSO_XL_UI_52Hz_LP; + + for (;;) { + /* Take measurement */ + if (imu_data_get(&ctx, &aux_ctx, &imu_md_temp, &imu_data_temp)) { + printf("Failed to get IMU data \r\n"); + } + + /* Run values through LPF of sample size */ + accel_data.accel_x = imu_data_temp.ui.xl.mg[0]; + accel_data.accel_y = imu_data_temp.ui.xl.mg[1]; + accel_data.accel_z = imu_data_temp.ui.xl.mg[2]; + gyro_data.gyro_x = imu_data_temp.ui.gy.mdps[0]; + gyro_data.gyro_y = imu_data_temp.ui.gy.mdps[1]; + gyro_data.gyro_z = imu_data_temp.ui.gy.mdps[2]; + temperature_data.temp = imu_data_temp.ui.heat.deg_c; #ifdef LOG_VERBOSE - printf("IMU Accel x: %d y: %d z: %d \r\n", accel_data.accel_x, - accel_data.accel_y, accel_data.accel_z); - printf("IMU Gyro x: %d y: %d z: %d \r\n", gyro_data.gyro_x, - gyro_data.gyro_y, gyro_data.gyro_z); - printf("IMU Temp: %3.2f °C \r\n", temperature_data.temp); + printf("IMU Accel x: %d y: %d z: %d \r\n", accel_data.accel_x, + accel_data.accel_y, accel_data.accel_z); + printf("IMU Gyro x: %d y: %d z: %d \r\n", gyro_data.gyro_x, + gyro_data.gyro_y, gyro_data.gyro_z); + printf("IMU Temp: %3.2f °C \r\n", temperature_data.temp); #endif - /* convert to big endian */ - endian_swap(&accel_data.accel_x, sizeof(accel_data.accel_x)); - endian_swap(&accel_data.accel_y, sizeof(accel_data.accel_y)); - endian_swap(&accel_data.accel_z, sizeof(accel_data.accel_z)); - endian_swap(&gyro_data.gyro_x, sizeof(gyro_data.gyro_x)); - endian_swap(&gyro_data.gyro_y, sizeof(gyro_data.gyro_y)); - endian_swap(&gyro_data.gyro_z, sizeof(gyro_data.gyro_z)); - - /* Send CAN message */ - memcpy(imu_accel_msg.data, &accel_data, imu_accel_msg.len); - if (queue_can_msg(imu_accel_msg)) { - printf("Failed to send CAN message\r\n"); - } - - memcpy(imu_gyro_msg.data, &gyro_data, imu_gyro_msg.len); - if (queue_can_msg(imu_gyro_msg)) { - printf("Failed to send CAN message\r\n"); - } - - /* Yield to other tasks */ - osDelay(DELAY_IMU_REFRESH); - } + /* convert to big endian */ + endian_swap(&accel_data.accel_x, sizeof(accel_data.accel_x)); + endian_swap(&accel_data.accel_y, sizeof(accel_data.accel_y)); + endian_swap(&accel_data.accel_z, sizeof(accel_data.accel_z)); + endian_swap(&gyro_data.gyro_x, sizeof(gyro_data.gyro_x)); + endian_swap(&gyro_data.gyro_y, sizeof(gyro_data.gyro_y)); + endian_swap(&gyro_data.gyro_z, sizeof(gyro_data.gyro_z)); + + /* Send CAN message */ + memcpy(imu_accel_msg.data, &accel_data, imu_accel_msg.len); + if (queue_can_msg(imu_accel_msg)) { + printf("Failed to send CAN message\r\n"); + } + + memcpy(imu_gyro_msg.data, &gyro_data, imu_gyro_msg.len); + if (queue_can_msg(imu_gyro_msg)) { + printf("Failed to send CAN message\r\n"); + } + + /* Yield to other tasks */ + osDelay(DELAY_IMU_REFRESH); + } } #endif #ifdef SENSOR_TOF osThreadId_t tof_monitor_handle; const osThreadAttr_t tof_monitor_attributes = { - .name = "TOFMonitor", - .stack_size = 128 * 8, - .priority = (osPriority_t)osPriorityHigh, + .name = "TOFMonitor", + .stack_size = 128 * 8, + .priority = (osPriority_t)osPriorityHigh, }; -void vTOFMonitor(void *pv_params) -{ - can_msg_t range_msg = { .id = convert_can(CANID_TOF, device_loc), - .len = 4, - .data = { 0 } }; +void vTOFMonitor(void *pv_params) { + can_msg_t range_msg = { + .id = convert_can(CANID_TOF, device_loc), .len = 4, .data = {0}}; - int32_t range; + int32_t range; - for (;;) { - if (distance_read(&range)) { - printf("failed to read distance!\r\n"); - continue; - } + for (;;) { + if (distance_read(&range)) { + printf("failed to read distance!\r\n"); + continue; + } #ifdef LOG_VERBOSE - printf("Range is: %ld\r\n", range); + printf("Range is: %ld\r\n", range); #endif - endian_swap(&range, sizeof(range)); + endian_swap(&range, sizeof(range)); - memcpy(range_msg.data, &range, range_msg.len); - /* Send CAN message */ - if (queue_can_msg(range_msg)) { - printf("Failed to send CAN message\r\n"); - } + memcpy(range_msg.data, &range, range_msg.len); + /* Send CAN message */ + if (queue_can_msg(range_msg)) { + printf("Failed to send CAN message\r\n"); + } - osDelay(DELAY_TOF_REFRESH); - } + osDelay(DELAY_TOF_REFRESH); + } } #endif #ifdef SENSOR_SHOCKPOT osThreadId_t shockpot_monitor_handle; const osThreadAttr_t shockpot_monitor_attributes = { - .name = "ShockpotMonitor", - .stack_size = 64 * 8, - .priority = (osPriority_t)osPriorityHigh1, + .name = "ShockpotMonitor", + .stack_size = 64 * 8, + .priority = (osPriority_t)osPriorityHigh1, }; -void vShockpotMonitor(void *pv_params) -{ - can_msg_t shockpot_msg = { .id = convert_can(CANID_SHOCK_SENSE, - device_loc), - .len = 4, - .data = { 0 } }; +void vShockpotMonitor(void *pv_params) { + can_msg_t shockpot_msg = { + .id = convert_can(CANID_SHOCK_SENSE, device_loc), .len = 4, .data = {0}}; - uint32_t shock_value = 0; + uint32_t shock_value = 0; - for (;;) { - shockpot_read(shock_value); + for (;;) { + shockpot_read(shock_value); #ifdef LOG_VERBOSE - printf("Shock value:\t%ld\r\n", shock_value); + printf("Shock value:\t%ld\r\n", shock_value); #endif - endian_swap(&shock_value, sizeof(shock_value)); + endian_swap(&shock_value, sizeof(shock_value)); - memcpy(shockpot_msg.data, &shock_value, shockpot_msg.len); - /* Send CAN message */ - if (queue_can_msg(shockpot_msg)) { - printf("Failed to send CAN message\r\n"); - } + memcpy(shockpot_msg.data, &shock_value, shockpot_msg.len); + /* Send CAN message */ + if (queue_can_msg(shockpot_msg)) { + printf("Failed to send CAN message\r\n"); + } - /* Yield to other tasks */ - osDelay(DELAY_SHOCKPOT_REFRESH); - } + /* Yield to other tasks */ + osDelay(DELAY_SHOCKPOT_REFRESH); + } } #endif #ifdef SENSOR_STRAIN osThreadId_t strain_monitor_handle; const osThreadAttr_t strain_monitor_attributes = { - .name = "StrainMonitor", - .stack_size = 64 * 8, - .priority = (osPriority_t)osPriorityHigh1, + .name = "StrainMonitor", + .stack_size = 64 * 8, + .priority = (osPriority_t)osPriorityHigh1, }; -void vStrainMonitor(void *pv_params) -{ - can_msg_t strain_msg = { .id = convert_can(CANID_STRAIN_SENSE, - device_loc), - .len = 8, - .data = { 0 } }; +void vStrainMonitor(void *pv_params) { + can_msg_t strain_msg = { + .id = convert_can(CANID_STRAIN_SENSE, device_loc), .len = 8, .data = {0}}; - struct __attribute__((__packed__)) { - uint32_t strain1; - uint32_t strain2; - } strain_data; + struct __attribute__((__packed__)) { + uint32_t strain1; + uint32_t strain2; + } strain_data; - uint32_t strain1_dat = 0; - uint32_t strain2_dat = 0; - for (;;) { - strain1_read(strain1_dat); - strain2_read(strain2_dat); + uint32_t strain1_dat = 0; + uint32_t strain2_dat = 0; + for (;;) { + strain1_read(strain1_dat); + strain2_read(strain2_dat); #ifdef LOG_VERBOSE - printf("Strain 1: %ld 2: %ld \r\n", strain1_dat, strain2_dat); + printf("Strain 1: %ld 2: %ld \r\n", strain1_dat, strain2_dat); #endif - strain_data.strain1 = strain1_dat; - strain_data.strain2 = strain2_dat; + strain_data.strain1 = strain1_dat; + strain_data.strain2 = strain2_dat; - endian_swap(&strain_data.strain1, sizeof(strain_data.strain1)); - endian_swap(&strain_data.strain2, sizeof(strain_data.strain2)); + endian_swap(&strain_data.strain1, sizeof(strain_data.strain1)); + endian_swap(&strain_data.strain2, sizeof(strain_data.strain2)); - memcpy(strain_msg.data, &strain_data, strain_msg.len); - /* Send CAN message */ - if (queue_can_msg(strain_msg)) { - printf("Failed to send CAN message"); - } + memcpy(strain_msg.data, &strain_data, strain_msg.len); + /* Send CAN message */ + if (queue_can_msg(strain_msg)) { + printf("Failed to send CAN message"); + } - /* Yield to other tasks */ - osDelay(DELAY_SHOCKPOT_REFRESH); - } + /* Yield to other tasks */ + osDelay(DELAY_SHOCKPOT_REFRESH); + } } #endif \ No newline at end of file diff --git a/Core/Src/msb.c b/Core/Src/msb.c index 28423c7..545cc49 100644 --- a/Core/Src/msb.c +++ b/Core/Src/msb.c @@ -1,11 +1,11 @@ #include "msb.h" -#include "main.h" #include "lsm6dso.h" #include "lsm6dso_reg.h" +#include "main.h" #include +#include #include #include -#include static osMutexAttr_t msb_i2c_mutex_attr; @@ -17,17 +17,15 @@ osMutexId_t i2c_mutex; // reads imu reg -int32_t lsm6dso_read_reg(stmdev_ctx_t *ctx, uint8_t reg, uint8_t *data, uint16_t len) -{ - return HAL_I2C_Mem_Read(&hi2c3, LSM6DSO_I2C_ADD_L, reg, - I2C_MEMADD_SIZE_8BIT, data, len, - HAL_MAX_DELAY); +int32_t lsm6dso_read_reg(stmdev_ctx_t *ctx, uint8_t reg, uint8_t *data, + uint16_t len) { + return HAL_I2C_Mem_Read(&hi2c3, LSM6DSO_I2C_ADD_L, reg, I2C_MEMADD_SIZE_8BIT, + data, len, HAL_MAX_DELAY); } -int32_t lsm6dso_write_reg(stmdev_ctx_t *ctx, uint8_t reg, uint8_t *data, uint16_t len) -{ - return HAL_I2C_Mem_Write(&hi2c3, LSM6DSO_I2C_ADD_L, reg, - I2C_MEMADD_SIZE_8BIT, data, len, - HAL_MAX_DELAY); +int32_t lsm6dso_write_reg(stmdev_ctx_t *ctx, uint8_t reg, uint8_t *data, + uint16_t len) { + return HAL_I2C_Mem_Write(&hi2c3, LSM6DSO_I2C_ADD_L, reg, I2C_MEMADD_SIZE_8BIT, + data, len, HAL_MAX_DELAY); } #ifdef SENSOR_TEMP sht30_t temp_sensor; @@ -45,157 +43,148 @@ VL6180xDev_t tof; uint32_t adc1_buf[3]; #endif -int8_t msb_init() -{ +int8_t msb_init() { #ifdef SENSOR_TEMP - /* Initialize the Onboard Temperature Sensor */ - temp_sensor = (sht30_t){ - .i2c_handle = &hi2c3, - }; - assert(!sht30_init(&temp_sensor)); /* This is always connected */ + /* Initialize the Onboard Temperature Sensor */ + temp_sensor = (sht30_t){ + .i2c_handle = &hi2c3, + }; + assert(!sht30_init(&temp_sensor)); /* This is always connected */ #endif #ifdef SENSOR_IMU - /* Initialize the IMU */ - assert(!LSM6DSO_Init(&imu)); /* This is always connected */ + /* Initialize the IMU */ + assert(!LSM6DSO_Init(&imu)); /* This is always connected */ - /* Setup IMU Accelerometer */ - LSM6DSO_ACC_Enable(&imu); + /* Setup IMU Accelerometer */ + LSM6DSO_ACC_Enable(&imu); - /* Setup IMU Gyroscope */ - LSM6DSO_GYRO_Enable(&imu); + /* Setup IMU Gyroscope */ + LSM6DSO_GYRO_Enable(&imu); - LSM6DSO_FIFO_Set_Mode(&imu, 0); - LSM6DSO_ACC_Disable_Inactivity_Detection(&imu); + LSM6DSO_FIFO_Set_Mode(&imu, 0); + LSM6DSO_ACC_Disable_Inactivity_Detection(&imu); #endif #ifdef SENSOR_TOF - /* Initialize the ToF sensor */ - struct MyDev_t tof_get = { - .i2c_bus_num = 0x29 << 1, - .i2c_handle = &hi2c3, - }; - tof = &tof_get; - assert(tof); - osDelay(1); - assert(!VL6180x_WaitDeviceBooted(tof)); - assert(!VL6180x_InitData(tof)); - assert(!VL6180x_Prepare(tof)); + /* Initialize the ToF sensor */ + struct MyDev_t tof_get = { + .i2c_bus_num = 0x29 << 1, + .i2c_handle = &hi2c3, + }; + tof = &tof_get; + assert(tof); + osDelay(1); + assert(!VL6180x_WaitDeviceBooted(tof)); + assert(!VL6180x_InitData(tof)); + assert(!VL6180x_Prepare(tof)); #endif #if defined SENSOR_SHOCKPOT || defined SENSOR_STRAIN - assert(!HAL_ADC_Start_DMA(&hadc1, adc1_buf, - sizeof(adc1_buf) / sizeof(uint32_t))); + assert(!HAL_ADC_Start_DMA(&hadc1, adc1_buf, + sizeof(adc1_buf) / sizeof(uint32_t))); #endif - /* Create Mutexes */ - i2c_mutex = osMutexNew(&msb_i2c_mutex_attr); - assert(i2c_mutex); + /* Create Mutexes */ + i2c_mutex = osMutexNew(&msb_i2c_mutex_attr); + assert(i2c_mutex); - return 0; + return 0; } #ifdef SENSOR_TEMP /// @brief Measure the temperature and humidity of central MSB SHT30 /// @param out /// @return error code -int8_t central_temp_measure(uint16_t *temp, uint16_t *humidity) -{ - osStatus_t mut_stat = osMutexAcquire(i2c_mutex, osWaitForever); - if (mut_stat) - return mut_stat; +int8_t central_temp_measure(uint16_t *temp, uint16_t *humidity) { + osStatus_t mut_stat = osMutexAcquire(i2c_mutex, osWaitForever); + if (mut_stat) + return mut_stat; - HAL_StatusTypeDef hal_stat = sht30_get_temp_humid(&temp_sensor); - if (hal_stat) - return hal_stat; + HAL_StatusTypeDef hal_stat = sht30_get_temp_humid(&temp_sensor); + if (hal_stat) + return hal_stat; - *temp = temp_sensor.temp; - *humidity = temp_sensor.humidity; + *temp = temp_sensor.temp; + *humidity = temp_sensor.humidity; - osMutexRelease(i2c_mutex); + osMutexRelease(i2c_mutex); - return 0; + return 0; } #endif #if defined SENSOR_SHOCKPOT || defined SENSOR_STRAIN -void adc1_read(uint32_t result_buf[3]) -{ - memcpy(result_buf, adc1_buf, sizeof(adc1_buf)); +void adc1_read(uint32_t result_buf[3]) { + memcpy(result_buf, adc1_buf, sizeof(adc1_buf)); } #endif #ifdef SENSOR_SHOCKPOT -void shockpot_read(uint32_t shockpot_sense) -{ - memcpy((uint32_t *)shockpot_sense, adc1_buf, sizeof(shockpot_sense)); +void shockpot_read(uint32_t shockpot_sense) { + memcpy((uint32_t *)shockpot_sense, adc1_buf, sizeof(shockpot_sense)); } #endif #ifdef SENSOR_STRAIN -void strain1_read(uint32_t strain1) -{ - memcpy((uint32_t *)strain1, adc1_buf + 1, sizeof(strain1)); +void strain1_read(uint32_t strain1) { + memcpy((uint32_t *)strain1, adc1_buf + 1, sizeof(strain1)); } -void strain2_read(uint32_t strain2) -{ - memcpy((uint32_t *)strain2, adc1_buf + 2, sizeof(strain2)); +void strain2_read(uint32_t strain2) { + memcpy((uint32_t *)strain2, adc1_buf + 2, sizeof(strain2)); } #endif #ifdef SENSOR_TOF VL6180x_RangeData_t *range; -int8_t distance_read(int32_t *range_mm) -{ - osStatus_t mut_stat = osMutexAcquire(i2c_mutex, osWaitForever); - if (mut_stat) - return mut_stat; - - VL6180x_RangePollMeasurement(tof, range); - if (range->errorStatus) { - printf("Error in range %s\r\n", - VL6180x_RangeGetStatusErrString(range->errorStatus)); - return range->errorStatus; - } - - memcpy(range_mm, &range->range_mm, sizeof(range->range_mm)); - - osMutexRelease(i2c_mutex); - return 0; +int8_t distance_read(int32_t *range_mm) { + osStatus_t mut_stat = osMutexAcquire(i2c_mutex, osWaitForever); + if (mut_stat) + return mut_stat; + + VL6180x_RangePollMeasurement(tof, range); + if (range->errorStatus) { + printf("Error in range %s\r\n", + VL6180x_RangeGetStatusErrString(range->errorStatus)); + return range->errorStatus; + } + + memcpy(range_mm, &range->range_mm, sizeof(range->range_mm)); + + osMutexRelease(i2c_mutex); + return 0; } #endif -int8_t debug1_write(bool status) -{ - HAL_GPIO_WritePin(Debug_LED_1_GPIO_Port, Debug_LED_1_Pin, status); - return 0; +int8_t debug1_write(bool status) { + HAL_GPIO_WritePin(Debug_LED_1_GPIO_Port, Debug_LED_1_Pin, status); + return 0; } -int8_t debug2_write(bool status) -{ - HAL_GPIO_WritePin(Debug_LED_2_GPIO_Port, Debug_LED_2_Pin, status); - return 0; +int8_t debug2_write(bool status) { + HAL_GPIO_WritePin(Debug_LED_2_GPIO_Port, Debug_LED_2_Pin, status); + return 0; } -int8_t vcc5_en_write(bool status) -{ - HAL_GPIO_WritePin(VCC5_En_GPIO_Port, VCC5_En_Pin, status); - return 0; +int8_t vcc5_en_write(bool status) { + HAL_GPIO_WritePin(VCC5_En_GPIO_Port, VCC5_En_Pin, status); + return 0; } #ifdef SENSOR_IMU -int32_t imu_data_get(stmdev_ctx_t *ctx, stmdev_ctx_t *aux_ctx, lsm6dso_md_t *imu_md_temp, lsm6dso_data_t *imu_data_temp) -{ - osStatus_t mut_stat = osMutexAcquire(i2c_mutex, osWaitForever); - if (mut_stat) - return mut_stat; - HAL_StatusTypeDef hal_stat = lsm6dso_data_get(&ctx, &aux_ctx, &imu_md_temp, &imu_data_temp); - if (hal_stat) - return hal_stat; - - //memcpy(gyro, imu.gyro, 3); - - osMutexRelease(i2c_mutex); +int32_t imu_data_get(stmdev_ctx_t *ctx, stmdev_ctx_t *aux_ctx, + lsm6dso_md_t *imu_md_temp, lsm6dso_data_t *imu_data_temp) { + osStatus_t mut_stat = osMutexAcquire(i2c_mutex, osWaitForever); + if (mut_stat) + return mut_stat; + HAL_StatusTypeDef hal_stat = + lsm6dso_data_get(&ctx, &aux_ctx, &imu_md_temp, &imu_data_temp); + if (hal_stat) + return hal_stat; + + // memcpy(gyro, imu.gyro, 3); + + osMutexRelease(i2c_mutex); } #endif \ No newline at end of file