# Fusion

**Repository Path**: batjokersk/fusion

## Basic Information

- **Project Name**: Fusion
- **Description**: 使用stm32f405融合WT931的磁力计、陀螺仪、加速度计，参考自https://github.com/xioTechnologies/Fusion
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: main
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 0
- **Created**: 2024-07-02
- **Last Updated**: 2024-08-06

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# 移植说明
## 1.文件移植
bsp、modules文件夹内的所有文件均需移植入工程内。除文件移植外，CubeMX也需要配置。本工程使用ST的HAL库开发，HAL库文件均可正常使用。
## 2.工程配置
1. wt931传感器：
   使用USART1作为传输串口，需要配置波特率为921600，引脚为：
   PA9->USART1_TX;
   PA10->USART1_RX;
   并开启全局中断.注意在NVIC的code generation界面取消勾选 usart1 的IRQHandler
   ![alt text](images/image0.png)
2. 按键：
   使用板载按键作为传感器重启的信号来源，按下将会重启传感器。**这只是一个demo，如果不需要可以删除，但需要一并删除ins_task.c中的外部中断回调函数**。引脚为：
   PA0;
   并配置为下降沿触发。配置如下：
   ![alt text](images/image.png)
3. FreeRTOS：
   建议惯导系统的堆栈大小开250words，运行频率为1kHz
4. DSP库：
   目前惯导算法矩阵运算量很小（远小于EKF），没有使用DSP，无需开启。
## 3.程序使用：
1. 头文件引用：在需要使用惯导系统的文件中引入```#include "ins_task.h"```，这是唯一需要使用的头文件。
2. 惯导系统初始化：
   1. 在完成传感器磁力计校正后，将磁力计误差矩阵填入softIronMatrix(in the file *ins_task.c*)中，示例如下：
   ```c
   const FusionMatrix softIronMatrix = {1.0f, -0.0194f, 0.0129f, -0.0194f, 0.9692f, -0.0007f, 0.0129f, -0.0007f, 0.999f}; // 将磁力计校准完成时的误差矩阵填入其中
   ```
    由于传感器自身具备校准的原因，其他校准参数可以不填。
    2. 调用初始化函数(in the file *ins_task.c*)，示例如下：
   ```c
    static INS_Data_s *INS_Data;
    INS_Data = INS_Init();
   ```
   仅需调用以上的初始化程序，系统解算数据存放在INS_Data中。下面是INS_Data_s结构体的详细解释
   ```c
    typedef struct
    {
    float roll, pitch, yaw; // 算法解算出的数据
    WT931Data *wt931data;   // 传感器传回的原始数据
    } INS_Data_s;
   ```
   **注意：INS_Init()中包含了部分bsp层的初始化，如DWT、USART。**
3. 惯导解算系统任务：
   INS_TASK()即为惯导解算任务，将该函数放在RTOS中即可，注意运行频率为1kHz：
   ```c
   void StartInsTask(void const *argument)
    {
    /* USER CODE BEGIN StartInsTask */
    /* Infinite loop */
    for (;;)
    {
        INS_TASK();
        osDelay(1);
    }
    /* USER CODE END StartInsTask */
    }
   ```
4. 传感器重启：
   目前没有设置传感器自动重启，而是通过按键触发重启。传感器重启函数为：WitRestartSensor();它在modules层wt931.c中定义。传感器重启不会影响算法计算出的欧拉角。使用示例如下：
   ```c
   void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
    {
    if (GPIO_Pin == GPIO_PIN_0)
    {
        WitRestartSensor();
        // FusionAhrsReset(&ahrs);//惯导系统重启函数。如果发现在无磁干扰的情况下，算法欧拉角和传感器自身欧拉角差异过大，可以重启惯导系统。
    }
    }
   ```

## 3.注意事项
1. 正式开始使用前，务必用上位机进行磁力计校准
2. 上位机中，陀螺仪自动校准建议关闭，理由是：自动校准会导致低于0.7°/s的角速度无法被检测到，从而出现积分误差（来自官方答疑界面）。但是否关闭建议还是根据使用场景来判断比较合适。
3. 上位机中可以配置传感器数据回传频率，也就是说传感器可以按照一定频率自动传回数据，因此算法更新时无需手动读取。配置如下：
   ![alt text](images/image-1.png)