Here's the code, but I don't think the problem has anything to do with it, since it works fine as long as the encoder isn't connected.
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file : main.c
* @brief : Main program body
******************************************************************************
* @attention
*
* Copyright (c) 2022 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include <movingQueue.h>
#include <string.h>
#include <stdio.h>
#include <sys/time.h>
#include <math.h>
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
/* USER CODE END PTD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
I2C_HandleTypeDef hi2c1;
UART_HandleTypeDef huart2;
/* USER CODE BEGIN PV */
static const uint8_t BNO055_ADDR = 0x28 << 1; // leftshift by 1 bit, since this is a 7 bit address
static const uint8_t OPR_MODE_NDOF = 0x0C;
static const uint8_t REG_UNIT_SEL = 0x3B;
static const uint8_t REG_CONFIG_OPERATION_MODE = 0x3D;
static const uint8_t REG_CALIB_STAT = 0x35;
static const uint8_t REG_LINEAR_ACCEL_X = 0x28; // 0x28 for LSB, 0x29 for MSB
static const uint8_t REG_LINEAR_ACCEL_Y = 0x2A; // 0x2A for LSB, 0x2B for MSB
static const uint8_t REG_LINEAR_ACCEL_Z = 0x2C; // 0x2C for LSB, 0x2D for MSB
static const uint8_t REG_MAG_X = 0x0E; // 0x0E for LSB, 0x0F for MSB
static const uint8_t REG_MAG_Y = 0x10; // 0x10 for LSB, 0x11 for MSB
static const uint8_t REG_MAG_Z = 0x12; // 0x12 for LSB, 0x13 for MSB
static const uint8_t REG_GRV_X = 0x2E;
static const uint8_t REG_QUAT_X = 0x22;
static const uint8_t REG_QUAT_Y = 0x24;
static const uint8_t REG_QUAT_Z = 0x26;
static const uint8_t REG_QUAT_W = 0x20;
static const uint8_t REG_TEMP = 0x34;
static const uint8_t MAG_DIV_UT = 16.0; // get microT
static const uint8_t GRV_DIV_MSQ = 100.0; // get m/s^2
static const uint8_t L_ACCEL_DIV_MSQ = 100.0; // get m/s^2
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_USART2_UART_Init(void);
static void MX_I2C1_Init(void);
/* USER CODE BEGIN PFP */
/* USER CODE END PFP */
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
double read_linear_accel();
/* USER CODE END 0 */
/**
* @brief The application entry point.
* @retval int
*/
int main(void)
{
/* USER CODE BEGIN 1 */
HAL_StatusTypeDef ret; // saves the return values of the i2c transmitting functions
uint8_t buf[16]; // buffer, where return data from i2c is temporarily saved in
uint8_t reg[16]; // here are the registers, that I want to read from/write to saved in temporarily
uint32_t ticks;
ticks = HAL_GetTick();
int drift_is_calibrated = 0;
double l_accel_x_sum = 0; // sum of acceleration values during calibration
double drift_calib_value_count = 0; // counter of stored values during calibration
double l_speed_x = 0;
double l_pos_x = 0;
double accel_calib_value = 0;
double accel_calib_sum = 0;
double accel_calib_count = 0;
double x_pos_five_sec = 0;
double x_pos_start = 0;
int progress_count = 3;
/* USER CODE END 1 */
/* MCU Configuration--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* USER CODE BEGIN Init */
/* USER CODE END Init */
/* Configure the system clock */
SystemClock_Config();
/* USER CODE BEGIN SysInit */
/* USER CODE END SysInit */
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_USART2_UART_Init();
MX_I2C1_Init();
/* USER CODE BEGIN 2 */
//----------------- configure the BNO055 --------------------
reg[0] = REG_CONFIG_OPERATION_MODE; // tell the bno055, that we want to change the operation mode
reg[1] = OPR_MODE_NDOF; // set mode to NDOF
ret = HAL_I2C_Master_Transmit(&hi2c1, BNO055_ADDR, reg, 2, HAL_MAX_DELAY);
// check, if transmission was fine
if (ret != HAL_OK) {
strcpy((char*)buf, "Error Rx\r\n");
HAL_UART_Transmit(&huart2, (const uint8_t*) buf, strlen((char*)buf), HAL_MAX_DELAY);
} else {
strcpy((char*)buf, "Config OK\r\n");
HAL_UART_Transmit(&huart2, (const uint8_t*) buf, strlen((char*)buf), HAL_MAX_DELAY);
}
HAL_Delay(7); // it takes 7ms to change the operation mode from config to ndof
reg[0] = REG_UNIT_SEL; // tell the bno055, that we want to change the units
reg[1] = 0x00; // set all units to metric
ret = HAL_I2C_Master_Transmit(&hi2c1, BNO055_ADDR, reg, 2, HAL_MAX_DELAY);
// check, if transmission was fine
if (ret != HAL_OK) {
strcpy((char*)buf, "Error Rx\r\n");
HAL_UART_Transmit(&huart2, (const uint8_t*) buf, strlen((char*)buf), HAL_MAX_DELAY);
} else {
strcpy((char*)buf, "Config OK\r\n");
HAL_UART_Transmit(&huart2, (const uint8_t*) buf, strlen((char*)buf), HAL_MAX_DELAY);
}
HAL_Delay(7); // it takes 7ms to change the operation mode from config to ndof
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
//----------------- check calibration status -------------------
int isCalibrated = 1;
reg[0] = REG_CALIB_STAT;
ret = HAL_I2C_Master_Transmit(&hi2c1, BNO055_ADDR, reg, 1, HAL_MAX_DELAY);
// check, if transmission was fine
if (ret != HAL_OK) {
strcpy((char*)buf, "Error Tx\r\n");
HAL_UART_Transmit(&huart2, (const uint8_t*) buf, strlen((char*)buf), HAL_MAX_DELAY);
} else {
// read calibration status
ret = HAL_I2C_Master_Receive(&hi2c1, BNO055_ADDR, (uint8_t*)buf, 1, HAL_MAX_DELAY);
if (ret != HAL_OK) {
strcpy((char*)buf, "Error Rx\r\n");
HAL_UART_Transmit(&huart2, (const uint8_t*)buf, strlen((char*)buf), HAL_MAX_DELAY);
} else {
// when calib_stat is 0, sensor is not calibrated, when it's 3, it is fully calibrated
// calibration status for fusion sensors is saved in the bits 6 and 7
if ((char)buf[0] >> 6 != 3) {
strcpy((char*)buf, "Sensor not calibrated \r\n");
//HAL_UART_Transmit(&huart2, (const uint8_t*)buf, strlen((char*) buf), HAL_MAX_DELAY);
isCalibrated = 0;
}
}
}
//----------------------- read sensor data ------------------------
if (isCalibrated == 1 || isCalibrated == 0) { // || isCalibrated == 0 -> give results, even if sensor is not properly calibrated
// read linear accel data
double l_accel_x = read_linear_accel();
uint32_t msecs = msecs + HAL_GetTick() - ticks; // milliseconds after last call
ticks = HAL_GetTick();
if(progress_count == 10) {
sprintf((char*)buf, "linear speed x:%0.4f\r\n", l_pos_x);
HAL_UART_Transmit(&huart2, (const uint8_t*)buf, strlen((char*) buf), HAL_MAX_DELAY);
}
if(ticks >= 5000 && progress_count == 0) {
x_pos_five_sec = l_pos_x;
progress_count = 1;
sprintf((char*)buf, "linear speed x:%0.4f\r\n", l_pos_x);
HAL_UART_Transmit(&huart2, (const uint8_t*)buf, strlen((char*) buf), HAL_MAX_DELAY);
}
if(ticks >= 10000 && progress_count == 1) {
double x_diff = 0 - l_pos_x;
double k = x_diff / pow(ticks, 2);
x_pos_five_sec = x_pos_five_sec + k * pow(5000,2);
sprintf((char*)buf, "linear speed x:%0.4f\r\n", l_pos_x);
HAL_UART_Transmit(&huart2, (const uint8_t*)buf, strlen((char*) buf), HAL_MAX_DELAY);
sprintf((char*)buf, "linear speed x:%0.4f\r\n", x_pos_five_sec);
HAL_UART_Transmit(&huart2, (const uint8_t*)buf, strlen((char*) buf), HAL_MAX_DELAY);
progress_count = 2;
}
if(progress_count == 3 && ticks <= 60000) {
accel_calib_sum += l_accel_x;
accel_calib_count += 1;
}
if(progress_count == 3 && ticks > 60000) {
accel_calib_value = accel_calib_sum / accel_calib_count;
progress_count = 4;
}
if(progress_count == 4) {
// todo: save last x values in queue and calculate moving average with corrected values
enqueue(l_accel_x - accel_calib_value);
sprintf((char*)buf, "linear speed x:%0.4f\r\n", l_speed_x);
HAL_UART_Transmit(&huart2, (const uint8_t*)buf, strlen((char*) buf), HAL_MAX_DELAY);
}
HAL_Delay(10);
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */
}
