From b7b7fb54d351cbe5a595a6ea845014c8c17d0104 Mon Sep 17 00:00:00 2001 From: Jack Rubacha Date: Fri, 6 Jun 2025 19:36:41 -0400 Subject: [PATCH 1/5] switch to high level driver to avoid scaling errors --- Core/Inc/msb.h | 4 ++-- Core/Src/monitor.c | 51 +++++++++++++++++--------------------------- Core/Src/msb.c | 53 +++++++++++++++++++++++++++------------------- 3 files changed, 52 insertions(+), 56 deletions(-) diff --git a/Core/Inc/msb.h b/Core/Inc/msb.h index 6aa4fb8..2da6e3c 100644 --- a/Core/Inc/msb.h +++ b/Core/Inc/msb.h @@ -34,8 +34,8 @@ int8_t debug2_write(bool status); int8_t vcc5_en_write(bool status); -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); +int32_t imu_data_get_accel(LSM6DSO_Axes_t *axes); +int32_t imu_data_get_gyro(LSM6DSO_Axes_t *axes); #ifdef SENSOR_SHOCKPOT void shockpot_read(uint32_t *shockpot_sense); diff --git a/Core/Src/monitor.c b/Core/Src/monitor.c index 0e140ee..557d410 100644 --- a/Core/Src/monitor.c +++ b/Core/Src/monitor.c @@ -116,17 +116,6 @@ void vIMUMonitor(void *pv_params) 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; - can_msg_t imu_orientation_msg = { .id = convert_can(CANID_IMU_ORIENTATION, device_loc), .len = 6, @@ -142,40 +131,38 @@ void vIMUMonitor(void *pv_params) int16_t yaw; } orientation_data; - /* 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; + LSM6DSO_Axes_t axes_accel; + LSM6DSO_Axes_t axes_gyro; for (;;) { /* Take measurement */ - if (imu_data_get(&ctx, &aux_ctx, &imu_md_temp, - &imu_data_temp)) { - printf("Failed to get IMU data \r\n"); + if (imu_data_get_accel(&axes_accel)) { + printf("Failed to get accel data \r\n"); + } + if (imu_data_get_accel(&axes_gyro)) { + printf("Failed to get gyro 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; + accel_data.accel_x = axes_accel.x; + accel_data.accel_y = axes_accel.y; + accel_data.accel_z = axes_accel.z; + gyro_data.gyro_x = axes_gyro.x; + gyro_data.gyro_y = axes_gyro.y; + gyro_data.gyro_z = axes_gyro.z; // Acc (Convert mg to g) - mFXInput.acc[0] = (float)(imu_data_temp.ui.xl.mg[0] / 1000.0f); - mFXInput.acc[1] = (float)(imu_data_temp.ui.xl.mg[1] / 1000.0f); - mFXInput.acc[2] = (float)(imu_data_temp.ui.xl.mg[2] / 1000.0f); + mFXInput.acc[0] = (float)(axes_accel.x / 1000.0f); + mFXInput.acc[1] = (float)(axes_accel.y / 1000.0f); + mFXInput.acc[2] = (float)(axes_accel.z / 1000.0f); // Gyro (Convert mdps to dps) mFXInput.gyro[0] = - (float)(imu_data_temp.ui.gy.mdps[0] * 0.001f); + (float)(axes_gyro.x * 0.001f); mFXInput.gyro[1] = - (float)(imu_data_temp.ui.gy.mdps[1] * 0.001f); + (float)(axes_gyro.y * 0.001f); mFXInput.gyro[2] = - (float)(imu_data_temp.ui.gy.mdps[2] * 0.001f); + (float)(axes_gyro.z * 0.001f); // Magnetometer mFXInput.mag[0] = 0.0f; diff --git a/Core/Src/msb.c b/Core/Src/msb.c index 14b5017..02b114a 100644 --- a/Core/Src/msb.c +++ b/Core/Src/msb.c @@ -16,20 +16,19 @@ extern device_loc_t device_loc; osMutexId_t i2c_mutex; -// reads imu reg - +// overriden functions int32_t lsm6dso_read_reg(stmdev_ctx_t *ctx, uint8_t reg, uint8_t *data, - uint16_t len) + uint16_t len) { return HAL_I2C_Mem_Read(&hi2c3, LSM6DSO_I2C_ADD_L, reg, - I2C_MEMADD_SIZE_8BIT, data, len, HAL_MAX_DELAY); + 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) + uint16_t len) { return HAL_I2C_Mem_Write(&hi2c3, LSM6DSO_I2C_ADD_L, reg, - I2C_MEMADD_SIZE_8BIT, data, len, - HAL_MAX_DELAY); + I2C_MEMADD_SIZE_8BIT, data, len, + HAL_MAX_DELAY); } #ifdef SENSOR_TEMP sht30_t temp_sensor; @@ -65,11 +64,14 @@ int8_t msb_init() assert(!LSM6DSO_Init(&imu)); /* This is always connected */ /* Setup IMU Accelerometer - default 104Hz */ + LSM6DSO_ACC_SetOutputDataRate_With_Mode(&imu, 104.0f, LSM6DSO_ACC_HIGH_PERFORMANCE_MODE); LSM6DSO_ACC_Enable(&imu); + // 4=div100 + LSM6DSO_ACC_Set_Filter_Mode(&imu, 0, 4); /* Setup IMU Gyroscope */ + LSM6DSO_GYRO_SetOutputDataRate_With_Mode(&imu, 104.0f, LSM6DSO_GYRO_HIGH_PERFORMANCE_MODE); LSM6DSO_GYRO_Enable(&imu); - - LSM6DSO_ACC_Set_Filter_Mode(&imu, 0, 4); + // LSM6DSO_GYRO_Set_Filter_Mode(&imu, 0, 3); LSM6DSO_FIFO_Set_Mode(&imu, 0); LSM6DSO_ACC_Disable_Inactivity_Detection(&imu); @@ -94,7 +96,7 @@ int8_t msb_init() #if defined SENSOR_SHOCKPOT || defined SENSOR_STRAIN assert(!HAL_ADC_Start_DMA(&hadc1, adc1_buf, - sizeof(adc1_buf) / sizeof(uint32_t))); + sizeof(adc1_buf) / sizeof(uint32_t))); #endif /* Create Mutexes */ @@ -162,9 +164,10 @@ int8_t distance_read(int32_t *range_mm) return mut_stat; VL6180x_RangePollMeasurement(tof, range); - if (range->errorStatus) { + if (range->errorStatus) + { printf("Error in range %s\r\n", - VL6180x_RangeGetStatusErrString(range->errorStatus)); + VL6180x_RangeGetStatusErrString(range->errorStatus)); return range->errorStatus; } @@ -194,23 +197,29 @@ int8_t vcc5_en_write(bool status) } #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) +int32_t imu_data_get_accel(LSM6DSO_Axes_t *axes) { 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); - osMutexRelease(i2c_mutex); - if (hal_stat) - return hal_stat; - return 0; + HAL_StatusTypeDef hal_stat = LSM6DSO_ACC_GetAxes(&imu, axes); + osMutexRelease(i2c_mutex); + return hal_stat; +} +int32_t imu_data_get_gyro(LSM6DSO_Axes_t *axes) +{ + osStatus_t mut_stat = osMutexAcquire(i2c_mutex, osWaitForever); + if (mut_stat) + return mut_stat; + HAL_StatusTypeDef hal_stat = LSM6DSO_GYRO_GetAxes(&imu, axes); + osMutexRelease(i2c_mutex); + return hal_stat; } void motion_fx_init(void) { - if (STATE_SIZE < MotionFX_GetStateSize()) { + if (STATE_SIZE < MotionFX_GetStateSize()) + { printf("Not enough memory allocated for MotionFX!!"); return; } @@ -245,7 +254,7 @@ void motion_fx_init(void) } void process_motion_fx(MFX_input_t *data_in, MFX_output_t *data_out, - float delta_time) + float delta_time) { MotionFX_propagate(mFXState, data_out, data_in, &delta_time); From d95bb5d54dd6b6549d6c5f9087e24a8443832192 Mon Sep 17 00:00:00 2001 From: Jack Rubacha Date: Sun, 8 Jun 2025 02:43:07 -0400 Subject: [PATCH 2/5] fix --- Core/Src/monitor.c | 9 ++++++++- Core/Src/msb.c | 4 ++-- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/Core/Src/monitor.c b/Core/Src/monitor.c index 557d410..2d09ae1 100644 --- a/Core/Src/monitor.c +++ b/Core/Src/monitor.c @@ -139,7 +139,7 @@ void vIMUMonitor(void *pv_params) if (imu_data_get_accel(&axes_accel)) { printf("Failed to get accel data \r\n"); } - if (imu_data_get_accel(&axes_gyro)) { + if (imu_data_get_gyro(&axes_gyro)) { printf("Failed to get gyro data \r\n"); } @@ -185,6 +185,13 @@ void vIMUMonitor(void *pv_params) orientation_data.roll); printf("IMU Temp: %3.2f °C \r\n", temperature_data.temp); #endif +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 Orientation Yaw: %d Pitch: %d Roll: %d \r\n", + // orientation_data.yaw, orientation_data.pitch, + // orientation_data.roll); /* convert to big endian */ endian_swap(&accel_data.accel_x, sizeof(accel_data.accel_x)); diff --git a/Core/Src/msb.c b/Core/Src/msb.c index 02b114a..f60dbd5 100644 --- a/Core/Src/msb.c +++ b/Core/Src/msb.c @@ -64,10 +64,10 @@ int8_t msb_init() assert(!LSM6DSO_Init(&imu)); /* This is always connected */ /* Setup IMU Accelerometer - default 104Hz */ - LSM6DSO_ACC_SetOutputDataRate_With_Mode(&imu, 104.0f, LSM6DSO_ACC_HIGH_PERFORMANCE_MODE); + LSM6DSO_ACC_SetOutputDataRate_With_Mode(&imu, 833.0f, LSM6DSO_ACC_HIGH_PERFORMANCE_MODE); LSM6DSO_ACC_Enable(&imu); // 4=div100 - LSM6DSO_ACC_Set_Filter_Mode(&imu, 0, 4); + //LSM6DSO_ACC_Set_Filter_Mode(&imu, 0, 4); /* Setup IMU Gyroscope */ LSM6DSO_GYRO_SetOutputDataRate_With_Mode(&imu, 104.0f, LSM6DSO_GYRO_HIGH_PERFORMANCE_MODE); LSM6DSO_GYRO_Enable(&imu); From 70c21b17981b0a83e29119045a1dd921d072a1ca Mon Sep 17 00:00:00 2001 From: Jack Rubacha Date: Tue, 5 Aug 2025 21:54:14 -0400 Subject: [PATCH 3/5] updates from test day --- Core/Inc/msb_conf.h | 2 +- Core/Src/main.c | 2 +- Core/Src/monitor.c | 4 ++-- Core/Src/msb.c | 2 +- MSB-FW.ioc | 2 +- Makefile | 2 +- 6 files changed, 7 insertions(+), 7 deletions(-) diff --git a/Core/Inc/msb_conf.h b/Core/Inc/msb_conf.h index fd650a3..9cca922 100644 --- a/Core/Inc/msb_conf.h +++ b/Core/Inc/msb_conf.h @@ -33,7 +33,7 @@ // on central #define SENSOR_TEMP // SHT30 -#define SENSOR_SHOCKPOT // ADC1 +//#define SENSOR_SHOCKPOT // ADC1 //#define SENSOR_STRAIN // ADC1 //#define SENSOR_TOF // VL6180X diff --git a/Core/Src/main.c b/Core/Src/main.c index 5ea446e..ffa1713 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -431,7 +431,7 @@ static void MX_CAN1_Init(void) hcan1.Init.TimeTriggeredMode = DISABLE; hcan1.Init.AutoBusOff = ENABLE; hcan1.Init.AutoWakeUp = DISABLE; - hcan1.Init.AutoRetransmission = ENABLE; + hcan1.Init.AutoRetransmission = DISABLE; hcan1.Init.ReceiveFifoLocked = DISABLE; hcan1.Init.TransmitFifoPriority = DISABLE; if (HAL_CAN_Init(&hcan1) != HAL_OK) diff --git a/Core/Src/monitor.c b/Core/Src/monitor.c index 2d09ae1..1b6f33d 100644 --- a/Core/Src/monitor.c +++ b/Core/Src/monitor.c @@ -185,8 +185,8 @@ void vIMUMonitor(void *pv_params) orientation_data.roll); printf("IMU Temp: %3.2f °C \r\n", temperature_data.temp); #endif -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 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 Orientation Yaw: %d Pitch: %d Roll: %d \r\n", diff --git a/Core/Src/msb.c b/Core/Src/msb.c index f60dbd5..d20fdeb 100644 --- a/Core/Src/msb.c +++ b/Core/Src/msb.c @@ -67,7 +67,7 @@ int8_t msb_init() LSM6DSO_ACC_SetOutputDataRate_With_Mode(&imu, 833.0f, LSM6DSO_ACC_HIGH_PERFORMANCE_MODE); LSM6DSO_ACC_Enable(&imu); // 4=div100 - //LSM6DSO_ACC_Set_Filter_Mode(&imu, 0, 4); + LSM6DSO_ACC_Set_Filter_Mode(&imu, 0, 3); /* Setup IMU Gyroscope */ LSM6DSO_GYRO_SetOutputDataRate_With_Mode(&imu, 104.0f, LSM6DSO_GYRO_HIGH_PERFORMANCE_MODE); LSM6DSO_GYRO_Enable(&imu); diff --git a/MSB-FW.ioc b/MSB-FW.ioc index 672b4cf..e0d8def 100644 --- a/MSB-FW.ioc +++ b/MSB-FW.ioc @@ -27,7 +27,7 @@ CAN1.CalculateBaudRate=500000 CAN1.CalculateTimeBit=2000 CAN1.CalculateTimeQuantum=142.85714285714286 CAN1.IPParameters=CalculateTimeQuantum,CalculateTimeBit,CalculateBaudRate,Prescaler,BS1,BS2,ABOM,NART -CAN1.NART=ENABLE +CAN1.NART=DISABLE CAN1.Prescaler=6 Dma.ADC1.0.Direction=DMA_PERIPH_TO_MEMORY Dma.ADC1.0.FIFOMode=DMA_FIFOMODE_DISABLE diff --git a/Makefile b/Makefile index 5abf5fd..57d7c4b 100644 --- a/Makefile +++ b/Makefile @@ -1,5 +1,5 @@ ########################################################################################################################## -# File automatically-generated by tool: [projectgenerator] version: [4.5.0-RC5] date: [Fri May 30 20:32:44 EDT 2025] +# File automatically-generated by tool: [projectgenerator] version: [4.5.0-RC5] date: [Sat Jul 26 14:06:59 EDT 2025] ########################################################################################################################## # ------------------------------------------------ From 18727c26767dc371a0de6da6440fa9591f32be3b Mon Sep 17 00:00:00 2001 From: Jack Rubacha Date: Tue, 5 Aug 2025 21:57:09 -0400 Subject: [PATCH 4/5] fmt msb --- Core/Src/msb.c | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/Core/Src/msb.c b/Core/Src/msb.c index d20fdeb..bb39a07 100644 --- a/Core/Src/msb.c +++ b/Core/Src/msb.c @@ -18,17 +18,17 @@ osMutexId_t i2c_mutex; // overriden functions int32_t lsm6dso_read_reg(stmdev_ctx_t *ctx, uint8_t reg, uint8_t *data, - uint16_t len) + uint16_t len) { return HAL_I2C_Mem_Read(&hi2c3, LSM6DSO_I2C_ADD_L, reg, - I2C_MEMADD_SIZE_8BIT, data, len, HAL_MAX_DELAY); + 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) + uint16_t len) { return HAL_I2C_Mem_Write(&hi2c3, LSM6DSO_I2C_ADD_L, reg, - I2C_MEMADD_SIZE_8BIT, data, len, - HAL_MAX_DELAY); + I2C_MEMADD_SIZE_8BIT, data, len, + HAL_MAX_DELAY); } #ifdef SENSOR_TEMP sht30_t temp_sensor; @@ -64,12 +64,14 @@ int8_t msb_init() assert(!LSM6DSO_Init(&imu)); /* This is always connected */ /* Setup IMU Accelerometer - default 104Hz */ - LSM6DSO_ACC_SetOutputDataRate_With_Mode(&imu, 833.0f, LSM6DSO_ACC_HIGH_PERFORMANCE_MODE); + LSM6DSO_ACC_SetOutputDataRate_With_Mode( + &imu, 833.0f, LSM6DSO_ACC_HIGH_PERFORMANCE_MODE); LSM6DSO_ACC_Enable(&imu); // 4=div100 LSM6DSO_ACC_Set_Filter_Mode(&imu, 0, 3); /* Setup IMU Gyroscope */ - LSM6DSO_GYRO_SetOutputDataRate_With_Mode(&imu, 104.0f, LSM6DSO_GYRO_HIGH_PERFORMANCE_MODE); + LSM6DSO_GYRO_SetOutputDataRate_With_Mode( + &imu, 104.0f, LSM6DSO_GYRO_HIGH_PERFORMANCE_MODE); LSM6DSO_GYRO_Enable(&imu); // LSM6DSO_GYRO_Set_Filter_Mode(&imu, 0, 3); @@ -96,7 +98,7 @@ int8_t msb_init() #if defined SENSOR_SHOCKPOT || defined SENSOR_STRAIN assert(!HAL_ADC_Start_DMA(&hadc1, adc1_buf, - sizeof(adc1_buf) / sizeof(uint32_t))); + sizeof(adc1_buf) / sizeof(uint32_t))); #endif /* Create Mutexes */ @@ -164,10 +166,9 @@ int8_t distance_read(int32_t *range_mm) return mut_stat; VL6180x_RangePollMeasurement(tof, range); - if (range->errorStatus) - { + if (range->errorStatus) { printf("Error in range %s\r\n", - VL6180x_RangeGetStatusErrString(range->errorStatus)); + VL6180x_RangeGetStatusErrString(range->errorStatus)); return range->errorStatus; } @@ -203,7 +204,7 @@ int32_t imu_data_get_accel(LSM6DSO_Axes_t *axes) if (mut_stat) return mut_stat; HAL_StatusTypeDef hal_stat = LSM6DSO_ACC_GetAxes(&imu, axes); - osMutexRelease(i2c_mutex); + osMutexRelease(i2c_mutex); return hal_stat; } int32_t imu_data_get_gyro(LSM6DSO_Axes_t *axes) @@ -212,14 +213,13 @@ int32_t imu_data_get_gyro(LSM6DSO_Axes_t *axes) if (mut_stat) return mut_stat; HAL_StatusTypeDef hal_stat = LSM6DSO_GYRO_GetAxes(&imu, axes); - osMutexRelease(i2c_mutex); + osMutexRelease(i2c_mutex); return hal_stat; } void motion_fx_init(void) { - if (STATE_SIZE < MotionFX_GetStateSize()) - { + if (STATE_SIZE < MotionFX_GetStateSize()) { printf("Not enough memory allocated for MotionFX!!"); return; } @@ -254,7 +254,7 @@ void motion_fx_init(void) } void process_motion_fx(MFX_input_t *data_in, MFX_output_t *data_out, - float delta_time) + float delta_time) { MotionFX_propagate(mFXState, data_out, data_in, &delta_time); From a0de8a268fd089cd843bd45646ff9857158c1ac2 Mon Sep 17 00:00:00 2001 From: Jack Rubacha Date: Tue, 5 Aug 2025 21:57:43 -0400 Subject: [PATCH 5/5] fmt --- Core/Inc/msb_conf.h | 2 +- Core/Src/can_handler.c | 2 +- Core/Src/monitor.c | 18 +++++++----------- 3 files changed, 9 insertions(+), 13 deletions(-) diff --git a/Core/Inc/msb_conf.h b/Core/Inc/msb_conf.h index 9cca922..e2ac8fc 100644 --- a/Core/Inc/msb_conf.h +++ b/Core/Inc/msb_conf.h @@ -32,7 +32,7 @@ #define CAN_ENABLE // on central -#define SENSOR_TEMP // SHT30 +#define SENSOR_TEMP // SHT30 //#define SENSOR_SHOCKPOT // ADC1 //#define SENSOR_STRAIN // ADC1 //#define SENSOR_TOF // VL6180X diff --git a/Core/Src/can_handler.c b/Core/Src/can_handler.c index c086e19..7e3a4f6 100644 --- a/Core/Src/can_handler.c +++ b/Core/Src/can_handler.c @@ -62,7 +62,7 @@ void vCanDispatch(void *pv_params) &msg_from_queue, NULL, osWaitForever)) { #ifdef CAN_ENABLE -while (HAL_CAN_GetTxMailboxesFreeLevel(&hcan1) == 0) { + while (HAL_CAN_GetTxMailboxesFreeLevel(&hcan1) == 0) { osDelay(1); } diff --git a/Core/Src/monitor.c b/Core/Src/monitor.c index 1b6f33d..7898238 100644 --- a/Core/Src/monitor.c +++ b/Core/Src/monitor.c @@ -157,12 +157,9 @@ void vIMUMonitor(void *pv_params) mFXInput.acc[2] = (float)(axes_accel.z / 1000.0f); // Gyro (Convert mdps to dps) - mFXInput.gyro[0] = - (float)(axes_gyro.x * 0.001f); - mFXInput.gyro[1] = - (float)(axes_gyro.y * 0.001f); - mFXInput.gyro[2] = - (float)(axes_gyro.z * 0.001f); + mFXInput.gyro[0] = (float)(axes_gyro.x * 0.001f); + mFXInput.gyro[1] = (float)(axes_gyro.y * 0.001f); + mFXInput.gyro[2] = (float)(axes_gyro.z * 0.001f); // Magnetometer mFXInput.mag[0] = 0.0f; @@ -185,8 +182,8 @@ void vIMUMonitor(void *pv_params) orientation_data.roll); printf("IMU Temp: %3.2f °C \r\n", temperature_data.temp); #endif -// 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 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 Orientation Yaw: %d Pitch: %d Roll: %d \r\n", @@ -291,7 +288,6 @@ void vShockpotMonitor(void *pv_params) uint16_t raw; } shockpot_data; - for (;;) { shockpot_read(&shock_value); @@ -301,8 +297,8 @@ void vShockpotMonitor(void *pv_params) //printf("Shock value:\t%ld\r\n", shock_value); // convert to inches, get percent and multiply by 50 mm (stroke length) then convert to inches - float in = (shock_value / 4095.0) * 54.44 * (1/25.4); - + float in = (shock_value / 4095.0) * 54.44 * (1 / 25.4); + shockpot_data.in = in; endian_swap(&shockpot_data.in, sizeof(shockpot_data.in)); shockpot_data.raw = shock_value;