代码解读十文件名“Ano

本部分主要是对IMU测量模块测量的值进行后续处理，同时在飞行过程中不断对数据进行更新，然后进行姿态解算，便于后续丢进PID中进行进一步处理。根据所处位置及函数调用情况不难发现此部分算是对底层的进一步封装，便于在任务调度器中进行调用

#include "Ano_FlightDataCal.h"
#include "Ano_Imu.h"
#include "Drv_icm20602.h"
#include "Ano_MagProcess.h"
#include "Drv_spl06.h"
#include "Drv_ak8975.h"
#include "Ano_MotionCal.h"
#include "Drv_vl53l0x.h"
#include "Drv_led.h"
#include "Ano_OF.h"u16 test_time_cnt;//传感器数据获取函数
void Fc_Sensor_Get()//1ms
{static u8 cnt;if(flag.start_ok){/*读取陀螺仪加速度计数据*/Drv_Icm20602_Read();//定期进行以下两个传感器数据的获取cnt ++;cnt %= 20;if(cnt==0){/*读取电子罗盘磁力计数据*/Drv_AK8975_Read();/*读取气压计数据*/baro_height = (s32)Drv_Spl0601_Read();}}    test_time_cnt++;}extern s32 sensor_val_ref[];static u8 reset_imu_f;//传感器更新函数
void IMU_Update_Task(u8 dT_ms)
{/*如果准备飞行，复位重力复位标记和磁力计复位标记*/if(flag.fly_ready ){imu_state.G_reset = imu_state.M_reset = 0;reset_imu_f = 0;}else {//如果静止if(flag.motionless == 0){
//                      imu_state.G_reset = 1;//自动复位//sensor.gyr_CALIBRATE = 2;}  if(reset_imu_f==0 )//&& flag.motionless == 1){imu_state.G_reset = 1;//自动复位 sensor.gyr_CALIBRATE = 2;//校准陀螺仪，不保存reset_imu_f = 1;     //已经置位复位标记}}if(0) {imu_state.gkp = 0.0f;imu_state.gki = 0.0f;}else{if(0){imu_state.gkp = 0.2f;}else{/*设置重力互补融合修正kp系数*/imu_state.gkp = 0.3f;//0.4f;}/*设置重力互补融合修正ki系数*/imu_state.gki = 0.002f;/*设置罗盘互补融合修正ki系数*/imu_state.mkp = 0.2f;}imu_state.M_fix_en = sens_hd_check.mag_ok;     //磁力计修正使能//             imu_state.obs_en = 1;
//              imu_data.obs_acc_w[X] = flow_decoupling.w_ref_acc[X];
//              imu_data.obs_acc_w[Y] = flow_decoupling.w_ref_acc[Y];//重要//调用姿态解算函数更新姿态/IMU_update(dT_ms *1e-3f, &imu_state,sensor.Gyro_rad, sensor.Acc_cmss, mag.val,&imu_data);//x3_dT_1[2] * 0.000001f
//
}static s16 mag_val[3];//磁力计更新函数
//要和Ano_MagProcess.c结合起来
void Mag_Update_Task(u8 dT_ms)
{//获取磁力计的值Mag_Get(mag_val);//调用磁力计数据处理函数Mag_Data_Deal_Task(dT_ms,mag_val,imu_data.z_vec[Z],sensor.Gyro_deg[X],sensor.Gyro_deg[Z]);}s32 baro_height,baro_h_offset,ref_height_get_1,ref_height_get_2,ref_height_used;
s32 baro2tof_offset,tof2baro_offset;float baro_fix1,baro_fix2,baro_fix;static u8 wcz_f_pause;
float wcz_acc_use;          //这个WCZ简写的至今没明白是什么意思，但是肯定和高度环有关，
//应该是气压计和激光有关的，是对其数据进行获取或者处理吧，
//简单了解下void WCZ_Acc_Get_Task()//最小周期
{wcz_acc_use += 0.2f *(imu_data.w_acc[Z] - wcz_acc_use);
}//void Baro_Get_Task()
//{ref_height_get += LIMIT((s32)user_spl0601_get() - ref_height_get,-20,20 );baro_height =(s32)user_spl0601_get();
//}u16 ref_tof_height;
static u8 baro_offset_ok,tof_offset_ok;void WCZ_Fus_Task(u8 dT_ms)
{if(flag.taking_off){baro_offset_ok = 2;}else{if(baro_offset_ok == 2){baro_offset_ok = 0;}}if(baro_offset_ok >= 1)//(flag.taking_off){ref_height_get_1 = baro_height - baro_h_offset + baro_fix  + tof2baro_offset;//气压计相对高度，切换点跟随TOF//baro_offset_ok = 0;}else{if(baro_offset_ok == 0 ){baro_h_offset = baro_height;if(flag.sensor_ok){baro_offset_ok = 1;}}}if((flag.flying == 0) && flag.auto_take_off_land == AUTO_TAKE_OFF  ){wcz_f_pause = 1;baro_fix = 0;}else{wcz_f_pause = 0;if(flag.taking_off == 0){baro_fix1 = 0;baro_fix2 = 0;}baro_fix2 = -BARO_FIX;baro_fix = baro_fix1 + baro_fix2 - BARO_FIX;//+ baro_fix3;}if((sens_hd_check.tof_ok || sens_hd_check.of_ok) && baro_offset_ok) //TOF或者OF硬件正常，且气压计记录相对值以后{if(switchs.tof_on || switchs.of_tof_on)                                                     //TOF数据有效{if(switchs.of_tof_on)                                                                                     //光流带TOF，光流优先{ref_tof_height = OF_ALT;}else{ref_tof_height = tof_height_mm/10;}if(tof_offset_ok == 1){ref_height_get_2 = ref_tof_height + baro2tof_offset;                 //TOF参考高度，切换点跟随气压计ref_height_used = ref_height_get_2;tof2baro_offset += 0.5f *((ref_height_get_2 - ref_height_get_1) - tof2baro_offset);//记录气压计切换点，气压计波动大，稍微滤波一下//tof2baro_offset = ref_height_get_2 - ref_height_get_1;               }else{baro2tof_offset = ref_height_get_1 - ref_tof_height ;                //记录TOF切换点tof_offset_ok = 1;}}else{tof_offset_ok = 0;ref_height_used = ref_height_get_1 ;}}else{ref_height_used = ref_height_get_1;}WCZ_Data_Calc(dT_ms,wcz_f_pause,(s32)wcz_acc_use,(s32)(ref_height_used));}

代码解读十文件名“Ano_FlightDataCal.c”相关推荐

代码解读一文件名“ANO_Imu.c”
我把这个文件的所有代码贴上来了,供大家参考,由于本人水平有限,且匿名代码注释比较少,所以很多也不是很懂,实在是一些莫名的定义太多了,什么w/x/y/z/h之类的,只求先能看懂大概逻辑,至于一些细节日后 ...
代码解读四文件名“Ano_AttCtrl.c”
这部分是关于匿名串级PID的,我觉得有需要的同学可以直接移植,不需要自己写了,确实有点麻烦,基本上代码里面都注释的很清楚了,且由于本人水平有限,所以也不是都很懂,只能做到这里了. #include & ...
代码解读六文件名“Ano_AltCtrl.c”
写了一大堆,也不知道对不对,贴上来让大家看看 #include "Ano_AltCtrl.h" //高度控制 #include "Ano_Imu.h" #inc ...
11月14日云栖精选夜读 | 动画+原理+代码，解读十大经典排序算法
排序算法是<数据结构与算法>中最基本的算法之一. 排序算法可以分为内部排序和外部排序,内部排序是数据记录在内存中进行排序,而外部排序是因排序的数据很大,一次不能容纳全部的排序记录,在排序过 ...
STM32学习心得十九：电容触摸按键实验及相关代码解读
记录一下,方便以后翻阅~ 主要内容 1) 电容触摸按键原理: 2)部分实验代码解读. 实验内容手触摸按键后,LED1灯翻转. 硬件原理图上图,TPAD与STM_ADC用跳线帽相连,即TPAD与PA ...
STM32学习心得十八：通用定时器基本原理及相关实验代码解读
记录一下,方便以后翻阅~ 主要内容: 1) 三种定时器分类及区别: 2) 通用定时器特点: 3) 通用定时器工作过程: 4) 实验一:定时器中断实验补充知识及部代码解读: 6) 实验二:定时器PWM输 ...
STM32学习心得三十四：外部SRAM原理及实验代码解读
记录一下,方便以后翻阅~ 主要内容: 1) IS62WV51216简介: 2) FSMC简介及相关寄存器介绍: 3) 相关实验代码解读. 参考手册: <STM32中文参考手册_V10>-第 ...
STM32学习心得二十一：实时时钟RTC和备份寄存器BKP特征、原理及相关实验代码解读
记录一下,方便以后翻阅~ 主要内容 1) RTC特征与原理: 2) BKP备份寄存器特征与原理: 3) RTC常用寄存器+库函数介绍: 4) 相关实验代码解读. 实验内容: 因为没有买LCD屏,所以计 ...
Pytorch_DDC（深度网络自适应，以resnet50为例）代码解读
最近跑了一下王晋东博士迁移学习简明手册上的深度网络自适应DDC(Deep Domain Confusion)的代码实现,在这里做一下笔记. 来源:Githup开源链接总结代码的大体框架如下: 1.数 ...

代码解读十文件名“Ano_FlightDataCal.c”

代码解读十文件名“Ano_FlightDataCal.c”相关推荐

最新文章

热门文章

代码解读十 文件名“Ano_FlightDataCal.c”

代码解读十 文件名“Ano_FlightDataCal.c”相关推荐

最新文章

热门文章

代码解读十文件名“Ano_FlightDataCal.c”

代码解读十文件名“Ano_FlightDataCal.c”相关推荐