# FOFController
**Repository Path**: mortyliu/FOFController
## Basic Information
- **Project Name**: FOFController
- **Description**: FOF（404）控制器设计仓库
- **Primary Language**: C++
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No
## Statistics
- **Stars**: 0
- **Forks**: 1
- **Created**: 2022-01-09
- **Last Updated**: 2022-04-09
## Categories & Tags
**Categories**: Uncategorized
**Tags**: None
## README
# FOF 控制器项目
## git clone 项目方法
工程使用父子Git结构管理，为了一次性完整的clone下整个工程，使用下述指令
```sh
/*首次clone 仓库*/
git clone https://gitee.com/mortyliu/FOFController.git --recursive ;
cd ./FOFController;
git submodule foreach git checkout master;
/*拉取最新提交*/
git pull --rebase
git submodule update
//或者到每个submoudle 执行 git pull --rebase 解决conflict
/*提交改动*/
1.到子仓库提交改动
2.回到父仓库提交改动信息
```
**git submodule foreach git checkout master**: 拉取所有子仓库的master 分支；
## 文件夹结构
FOFController_MCU_Frameware: MCU domain 代码
FOFController_MCU_Hardware: MCU 硬件设计
FOFController_UpperCom_Frameware: 上位机 代码
## 使用开发工具
MCU 硬件设计: KiCad
MCU 域代码使用:
STMCubeMx6.4.0 生成 代码，然后进入CLion 配置开发
编译器：arm-none-eabi-gcc / arm-none-eabi-g++
烧录和Debug软件：OpenOCD stlink
逻辑分析仪：Seleae
上位机 开发使用:
CMake 组织 Qt5 + Matlab开发 尽量满足多平台支持
## 系统架构
#### MCU 域架构
##### BSP：
BSP下半部采用CubeMX生成硬件驱动，提供GPIO控制、通讯总线控制、系统时钟配置、中断配置、DMA控制器配置API。（自动生成）
BSP上半部通过二次封装HAL库提供接口，屏蔽一定的底层操作逻辑，向上提供所有传感器的读取写入、配置、总线操作、系统控制A$$PI。（自己实现）
##### Middleware：
* System View：提供系统运行状态追踪的服务，可以通过Tracer工具查看系统运行时对于中断、任务调度、临界区访问相关的状态。（移植）
* freeRTOS: 使用freeRTOS的任务调度器组件 。（移植）
* ROS Messag: 负责ROS消息包发送和接收；消息队列内存的管理；丢包检测和错包检测。（自己实现）
##### APPs/Tasks：
* 系统控制算法
* 控制系统输出滤波
* 传感器滤波算法
* 传感器数据融合算法
* 指令管理模块
* 其他任务
#### 上位机架构
##### 前端：
主要负责图形绘制、函数绘制、图形化交互功能
##### 后端：
1. QT的图形化界面框架支持上位机的图形界面服务
2. Matlab提供的数学工具对输入的离散数据进行FFT等信号变换运算服务；Simulink 仿真滤波器服务
3. 实现ROS 消息管理器，对MCU传输的数据进行实时解析
##### 主要任务
1. 解析MCU数据，对传感器和执行器的数据可视化展示
2. 对传感器和执行器数据进行时域和频域分析
3. 仿真滤波器对现有信号进行拟真处理，查看滤波器导致相位延迟和滤波后信号
4. 配置MUC端控制参数
5. 解析MCU端上发数据
### 上发数据格式
#### 测试阶段数据格式
数据用;隔开。
mag_x ;mag_y ;mag_z ; acc_x ; acc_y; acc_z;
| 传感器 | 数据 |
| -------- | :----------------------------------------------------------- |
| IST8310 | mag_x
mag_y
mag_z |
| BMI160 | acc_x
acc_y
acc_z
gyro_x
gyro_y
gyro_z |
| ICM20602 | acc_x
acc_y
acc_z
gyro_x
gyro_y
gyro_z |
| BMP280 | air pressure
temperature |
| ······ | |
### 加速度计和磁力计 的椭球拟合
因为加速度和磁力在局部地区的值是方向不变，大小恒定的，所以将传感器三轴转动后，测量到的三维坐标点应该在一个球面上，同时三轴应该正交。 普遍采用的最小二乘法，通过梯度下降获得最小误差。
#### 对于轴正交和对齐的模型
轴对齐的椭球标准方程 （1.1）
$(\frac{x-O_x}{R_X})^2 + (\frac{y-O_y}{R_Y})^2+ (\frac{z-O_z}{R_Z})^2 = 1$ (1.1)
展开 f(1) 获得多项式 （1.2）
$\frac{1}{R_x^2}x^2 + \frac{1}{R_y^2}y^2 + \frac{1}{R_z^2}z^2 + (-\frac{2O_x}{R_x^2})x + (-\frac{2O_y}{R_y^2})y + (-\frac{2O_z}{R_z^2})z + (\frac{O_x^2}{R_x^2} + \frac{O_y^2}{R_y^2} + \frac{O_z^2}{R_z^2} -1) = 0$ （1.2）
两边同乘 $R_x^2$ 获得多项式 （1.3）
$x^2 + \frac{R_x^2}{R_y^2}y^2 + \frac{R_x^2}{R_z^2}z^2 + (-2O_x)x + (-\frac{2R_x^20_y}{R_y^2})y + (-\frac{2R_x^20_z}{R_z^2})z+ O_x^2 + \frac{R_x^2}{R_y^2}O_y^2 + \frac{R_x^2}{R_z^2}O_z^2 - R_x^2 = 0$ （1.3）
可以将上式转化为矩阵形式 (1.4)
$y^2A + z^2B + xC + yD +zE +F= - x^2$ (1.4)
$\begin{bmatrix} y^2 & z^2 & x & y &z &1\end{bmatrix} \begin{bmatrix} A\\ B\\ C\\ D \\E \\F\end{bmatrix} = -x^2$ 可以变为 $X\theta = Y$ （1.5）
其中X 为样本矩阵，在本实验中为 m*6 矩阵，m为样本量， 6为样本特征，$\theta$为参数矩阵，是$6x1$ 向量
最小二乘 代价方程
$J(\theta)=\frac{1}{2}(X\theta - Y)^T（X\theta-Y）$ （1.6）
对$\theta$ 求偏导后，获得
$\frac{\partial{J(\theta)}}{\partial{\theta}} = X^T(X\theta - Y) $ (1.7)
式1.7 取0 时为最小二乘的 解
$\theta = (X^TX)^{-1}X^TY)$
获得校准参数
#### 对于轴不对齐和轴不正交的模型
椭球模型 公式如下
$a_1x^2 + a_2x^y + a_3z^2 + a_4xy + a_5 xz + a_6 yz + a_7x + a_8y + a_9z =1 $
对比轴对齐模型，有了二次交叉相
### 磁力计矫正数据记录
ist_8310_calibrate_raw_data.xslx:
x\y轴 乘6.4 系数，
z 乘10
通过轴不对齐正交模型计算获得以下拟合参数
,
soft_iron distortion 比较轻微，可以看到Soft Iron Calibrate Matrix 接近于一个对角矩阵
使用参数，获得以下结果
排除实验噪声和随机漂移，可以看到新的数据可以很好的和矫正后的数据拟合，soft_iron_distortion 矫正矩 已经接近一个对称矩阵。数据已经拟合为 一个球体。图中看到
### 重力计校准
重力计和磁力计的矫正方法一致，都是使用
### ARM Cortex-m7 DSP模块