模块装备图：

总览：

接口功能图（一）

模块功能图（二）正面

模块功能图（三）背面

芯片原理图:

总体分析与解析

以一键起飞加定高为例：

头文件目录

#include "Ano_Scheduler.h"
#include "Drv_Bsp.h"
#include "Drv_icm20602.h"
#include "Ano_LED.h"
#include "Ano_FlightDataCal.h"
#include "Ano_Sensor_Basic.h"#include "Drv_gps.h"
#include "Ano_DT.h"
#include "Ano_RC.h"
#include "Ano_Parameter.h"
#include "Drv_led.h"
#include "Drv_ak8975.h"
#include "Drv_spl06.h"
#include "Ano_FlightCtrl.h"
#include "Ano_AttCtrl.h"
#include "Ano_LocCtrl.h"
#include "Ano_AltCtrl.h"
#include "Ano_MotorCtrl.h"
#include "Ano_Parameter.h"
#include "Ano_MagProcess.h"
#include "Ano_Power.h"
#include "Ano_OF.h"
#include "Drv_heating.h"
#include "Ano_FlyCtrl.h"
#include "Ano_UWB.h"
#include "Drv_OpenMV.h"
#include "Ano_OPMV_CBTracking_Ctrl.h"
#include "Ano_OPMV_LineTracking_Ctrl.h"
#include "Ano_OPMV_Ctrl.h"
#include "Ano_OF_DecoFusion.h"
#include "Drv_mv.h"

灯光闪烁函数： INT_1ms_Task()

void INT_1ms_Task()
{
//  if(fc_sta.start_ok == 0) return;//标记1ms执行lt0_run_flag ++;//灯光驱动LED_1ms_DRV();//循环计数circle_cnt[0] ++;//20次循环circle_cnt[0] %= CIRCLE_NUM;//if(!circle_cnt[0]){}
}

所有传感器读取函数： Loop_Task_0()

static void Loop_Task_0()//1ms执行一次
{// /*传感器数据读取*/Fc_Sensor_Get();/*惯性传感器数据准备*/Sensor_Data_Prepare(1);/*姿态解算更新*/IMU_Update_Task(1);/*获取WC_Z加速度*/WCZ_Acc_Get_Task();WCXY_Acc_Get_Task();/*飞行状态任务*/Flight_State_Task(1,CH_N);/*开关状态任务*/Swtich_State_Task(1);/*光流融合数据准备任务*/ANO_OF_Data_Prepare_Task(0.001f);/*数传数据交换*/ANO_DT_Data_Exchange();
}

姿态环以及电机初始化：Loop_Task_1

static void Loop_Task_1(u32 dT_us)   //2ms执行一次
{//  float t1_dT_s;t1_dT_s = (float)dT_us *1e-6f;//========================/*姿态角速度环控制*/Att_1level_Ctrl(2*1e-3f);/*电机输出控制*/Motor_Ctrl_Task(2);   //
}

姿态环角控制

static void Loop_Task_2(u32 dT_us)   //6ms执行一次
{//  float t2_dT_s;t2_dT_s = (float)dT_us *1e-6f;//========================/*获取姿态角（ROLL PITCH YAW）*/calculate_RPY();User_my_yaw_2level(6,line);   //寻线YAW修正/*姿态角度环控制*/Att_2level_Ctrl(6e-3f,CH_N);////
}

飞行任务设置：void Loop_Task_5

static void Loop_Task_5(u32 dT_us)   //11ms执行一次
{
//  float t2_dT_s = (float)dT_us *1e-6f;//0.008f;////========================/*遥控器数据处理*/RC_duty_task(11);/*飞行模式设置任务*/Flight_Mode_Set(11);/*高度数据融合任务*/WCZ_Fus_Task(11);GPS_Data_Processing_Task(11);/*高度速度环控制*/Alt_1level_Ctrl(11e-3f);/*高度环控制*/Alt_2level_Ctrl(11e-3f);/*--*/  AnoOF_DataAnl_Task(11);/*灯光控制*/ LED_Task2(11);//
}

特定飞行任务函数：Loop_Task_8

extern struct _MV_       MV;static void Loop_Task_8(u32 dT_us)   //20ms执行一次
{u8 dT_ms = 20;//(u8)(dT_us *1e-3f);//==========================///*罗盘数据处理任务*/Mag_Update_Task(20);/*程序指令控制*/FlyCtrl_Task(20);//ANO_OFDF_Task(20);/*--*/Ano_UWB_Data_Calcu_Task(20);/*位置速度环控制*/Loc_1level_Ctrl(20,CH_N);/*用户*/MV_Decoupling(20);  //对数据处理传到飞机    解旋转Loc_2level_Ctrl(20,&MV);Tracking_Ctrlw(0.02f);      //小飞机改的程序
//  Tracking_Ctrl(0.02f);      //绕框//     Proce0_Ctrl(0.02f);       //定点匿名程序
//  /*OPMV检测是否掉线*/
//  OpenMV_Offline_Check(20);
//  /*OPMV色块追踪数据处理任务*/
//  ANO_CBTracking_Task(20);
//  /*OPMV寻线数据处理任务*/
//  ANO_LTracking_Task(20);
//  /*OPMV控制任务*/
//  ANO_OPMV_Ctrl_Task(20);}

电压以及温度控制函：Loop_Task_9

static void Loop_Task_9(u32 dT_us)   //50ms执行一次
{///*电压相关任务*/Power_UpdateTask(50);//恒温控制（不能直接注释掉，否则开机过不了校准）Thermostatic_Ctrl_Task(50);//   /*延时存储任务*/Ano_Parame_Write_task(50);
}

系统任务多线程配置函数：

static sched_task_t sched_tasks[] =
{//任务n,    周期us,   上次时间us{Loop_Task_1 ,  2000,  0 },{Loop_Task_2 ,  6000,  0 },
//  {Loop_Task_2 ,  2500,  0 },
//  {Loop_Task_3 ,  2500,  0 },
//  {Loop_Task_4 ,  2500,  0 },{Loop_Task_5 ,  11000,  0 },
//  {Loop_Task_6 ,  9090,  0 },
//  {Loop_Task_7 ,  9090,  0 },{Loop_Task_8 , 20000,  0 },{Loop_Task_9 , 50000,  0 },
//  {Loop_Task_10,100000,  0 },
};

线程执行函数

u8 Main_Task(void)
{uint8_t index = 0;//查询1ms任务是否需要执行if(lt0_run_flag!=0){//lt0_run_flag--;Loop_Task_0();}//循环判断其他所有线程任务，是否应该执行uint32_t time_now,delta_time_us;for(index=0;index < TASK_NUM;index++){//获取系统当前时间，单位UStime_now = GetSysRunTimeUs();//SysTick_GetTick();//进行判断，如果当前时间减去上一次执行的时间，大于等于该线程的执行周期，则执行线程if(time_now - sched_tasks[index].last_run >= sched_tasks[index].interval_ticks){delta_time_us = (u32)(time_now - sched_tasks[index].last_run);//更新线程的执行时间，用于下一次判断sched_tasks[index].last_run = time_now;//执行线程函数，使用的是函数指针sched_tasks[index].task_func(delta_time_us);}    }return 0;
}

初始化函数结构分析：

LED初始化： Dvr_LedInit(void)

分析：可以看到初始化函数由两个部分构成
- 第一部分：ROM_SysCtlPeripheralEnable （GPIO口 ）
- 第二部分：ROM_GPIOPinTypeGPIOOutput（GPIO口 ，GPIO引脚 ）

#define LED1_SYSCTL          SYSCTL_PERIPH_GPIOD
#define LED2_SYSCTL         SYSCTL_PERIPH_GPIOD
#define LED3_SYSCTL         SYSCTL_PERIPH_GPIOA
#define LEDS_SYSCTL         SYSCTL_PERIPH_GPIOF
#define LED1_PORT           GPIOD_BASE
#define LED2_PORT           GPIOD_BASE
#define LED3_PORT           GPIOA_BASE
#define LEDS_PORT           GPIOF_BASE
#define LED1_PIN            GPIO_PIN_0
#define LED2_PIN            GPIO_PIN_1
#define LED3_PIN            GPIO_PIN_6
#define LEDS_PIN            GPIO_PIN_4
void Dvr_LedInit(void)
{ROM_SysCtlPeripheralEnable(LED1_SYSCTL);ROM_SysCtlPeripheralEnable(LED2_SYSCTL);ROM_SysCtlPeripheralEnable(LED3_SYSCTL);ROM_SysCtlPeripheralEnable(LEDS_SYSCTL);ROM_GPIOPinTypeGPIOOutput(LED1_PORT, LED1_PIN);ROM_GPIOPinTypeGPIOOutput(LED2_PORT, LED2_PIN);ROM_GPIOPinTypeGPIOOutput(LED3_PORT, LED3_PIN);ROM_GPIOPinTypeGPIOOutput(LEDS_PORT, LEDS_PIN);Drv_LedOnOff(LED_B, 1);
}

传感器数据读取函数

解析 ：飞控三大传感器数据

• 陀螺仪 加速度
• 电子罗盘
• 气压计
• 这些函数以及封装好了 为的是方便我们解析出这个程序

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++;}

陀螺仪加速度提取：Drv_Icm20602_Read

void Drv_Icm20602_Read( void )
{//读取传感器寄存器，连续读14个字节icm20602_readbuf(MPUREG_ACCEL_XOUT_H,14,mpu_buffer);//数据赋值ICM_Get_Data();
}

电子罗盘磁力计数据：Drv_AK8975_Read();

void Drv_AK8975_Read(void)
{   ak8975_enable(1);Drv_Spi0SingleWirteAndRead(AK8975_HXL_REG|0x80);for(u8 i=0; i<6; i++)ak8975_buf[i] = Drv_Spi0SingleWirteAndRead(0xff);ak8975_enable(0);ak8975_Trig();}

读取气压计的数据：(s32)Drv_Spl0601_Read()

float Drv_Spl0601_Read ( void )
{spl0601_get_raw_temp();temperature = spl0601_get_temperature();spl0601_get_raw_pressure();baro_pressure = spl0601_get_pressure();alt_3 = ( 101400 - baro_pressure ) / 1000.0f;height = 0.82f * alt_3 * alt_3 * alt_3 + 0.09f * ( 101400 - baro_pressure ) * 100.0f alt_high = ( height - baro_Offset ) ; //cm +return alt_high;
}

惯性传感器检测函数

静止检测函数：motionless_check(dT_ms);

解析: 通过判断T来决定是否是静止状态
而T的判断标准是 原数据减去旧的角度数据

void Sensor_Basic_Init()
{# 重新相对传感器的偏移量Center_Pos_Set();sensor.acc_z_auto_CALIBRATE = 1; # 开机自动校准对准Z轴sensor.gyr_CALIBRATE = 2;  # 开机自动校准陀螺仪
}
void motionless_check(u8 dT_ms)
{u8 t = 0;for(u8 i = 0;i<3;i++){g_d_sum[i] += 3*ABS(sensor.Gyro_Original[i] - g_old[i]) ;g_d_sum[i] -= dT_ms ;    g_d_sum[i] = LIMIT(g_d_sum[i],0,200);if( g_d_sum[i] > 10){t++;}g_old[i] = sensor.Gyro_Original[i];}if(t>=2){flag.motionless = 0;    }else{flag.motionless = 1;}}

陀螺仪MPU6050 函数 MPU6050_Data_Offset();

static u8 off_cnt;if(sensor.gyr_CALIBRATE || sensor.acc_CALIBRATE || sensor.acc_z_auto_CALIBRATE){       if(flag.motionless == 0 || sensor_val[A_Z]<(GRAVITY_ACC_PN16G/2) || (flag.mems_temperature_ok == 0)){gyro_sum_cnt = 0;acc_sum_cnt=0;acc_z_auto_cnt = 0;for(u8 j=0;j<3;j++){acc_auto_sum_temp[j] = sum_temp[G_X+j] = sum_temp[A_X+j] = 0;}sum_temp[TEM] = 0;}off_cnt++;          if(off_cnt>=10){    off_cnt=0;         if(sensor.gyr_CALIBRATE){LED_STA_CALI_GYR = 1;gyro_sum_cnt++;for(u8 i = 0;i<3;i++){sum_temp[G_X+i] += sensor.Gyro_Original[i];}if( gyro_sum_cnt >= OFFSET_AV_NUM ){LED_STA_CALI_GYR = 0;for(u8 i = 0;i<3;i++){save.gyro_offset[i] = (float)sum_temp[G_X+i]/OFFSET_AV_NUM;sum_temp[G_X + i] = 0;}gyro_sum_cnt =0;if(sensor.gyr_CALIBRATE == 1){if(sensor.acc_CALIBRATE == 0){data_save();}}sensor.gyr_CALIBRATE = 0;
//                  ANO_DT_SendString("GYR init OK!");}}if(sensor.acc_CALIBRATE == 1){LED_STA_CALI_ACC = 1;acc_sum_cnt++;sum_temp[A_X] += sensor_val_rot[A_X];sum_temp[A_Y] += sensor_val_rot[A_Y];sum_temp[A_Z] += sensor_val_rot[A_Z] - GRAVITY_ACC_PN16G;// - 65535/16;   // +-8Gsum_temp[TEM] += sensor.Tempreature;if( acc_sum_cnt >= OFFSET_AV_NUM ){LED_STA_CALI_ACC = 0;for(u8 i=0 ;i<3;i++){save.acc_offset[i] = sum_temp[A_X+i]/OFFSET_AV_NUM;sum_temp[A_X + i] = 0;}acc_sum_cnt =0;sensor.acc_CALIBRATE = 0;
//                  ANO_DT_SendString("ACC init OK!");data_save();}   }}}
}

惯性传感器转换单位 函数段

 for(u8 i =0 ;i<3;i++){# 陀螺仪转换到度每秒 量程+-2000度sensor.Gyro_deg[i] = sensor.Gyro[i] *0.061036f ;# 陀螺仪转换到弧度每秒 量程+-2000度sensor.Gyro_rad[i] = sensor.Gyro_deg[i] *0.01745f;//sensor.Gyro[i]                            # 加速度计转换成厘米 每平方秒 量程 +- 8Gsensor.Acc_cmss[i] = (sensor.Acc[i] *RANGE_PN16G_TO_CMSS );//   /65535 * 16*981; +-8G  }

姿态解算更新函数 IMU_Update_Task(1);

作用： 重力传感器 以及 磁力计 进行姿态计算
注释如下： 如果准备飞行 复位重力标记和磁力计复位标记
校准陀螺仪 不保存
自动复位
已经置位复位标记
设置重力互补融合修正Kp系数
设置重力互补融合修正ki系数
设置罗盘互补融合修正ki系数
磁力计修正使能
姿态计算 更新 融合

void IMU_Update_Task(u8 dT_ms)
{// 如果准备飞行 复位重力标记和磁力计复位标记        if(flag.unlock_sta ){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.2f;//0.4f;}// 设置重力互补融合修正ki系数imu_state.gki = 0.01f;// 设置罗盘互补融合修正ki系数imu_state.mkp = 0.1f;}imu_state.M_fix_en = sens_hd_check.mag_ok;        //磁力计修正使能//姿态计算 更新 融合IMU_update(dT_ms *1e-3f, &imu_state,sensor.Gyro_rad, sensor.Acc_cmss, mag.val,&imu_data);//x3_dT_1[2] * 0.000001f
}

获取加速度函数 (一)WCZ_Acc_Get_Task();

获得 Z 轴上的加速度
注释： 获取最小周期

void WCZ_Acc_Get_Task()//最小周期
{wcz_acc_use += 0.03f *(imu_data.w_acc[Z] - wcz_acc_use);
}

获取加速度函数 (二)WCXY_Acc_Get_Task();

获得 X Y轴上的加速度
注释： 最小周期

void WCXY_Acc_Get_Task(void)//最小周期
{wcx_acc_use += 0.015f *(imu_data.w_acc[X] - wcx_acc_use);wcy_acc_use += 0.015f *(imu_data.w_acc[Y] - wcy_acc_use);
}

任务调度：

飞行状态任务：Flight_State_Task(1,CH_N)

飞行状态任务调度：
注释如下：

• 设置油门摇杆量
• 推油门启动
• 起飞1秒 后 认为已经在飞行
• 设置 上升速度
• 设置 下降速度
• 飞控系统 z速度目标标量综合设定
• 速度设定量 正负 参考ANO 坐标参考方向
• 飞控系统 XY速度 目标综合量设定
• 调用检测着陆的函数
• 倾斜过上大锁

void Flight_State_Task(u8 dT_ms,s16 *CH_N)
{s16 thr_deadzone;static float max_speed_lim,vel_z_tmp[2];// 设置油门摇杆量thr_deadzone = (flag.wifi_ch_en != 0) ? 0 : 50;fs.speed_set_h_norm[Z] = my_deadzone(CH_N[CH_THR],0,thr_deadzone) *0.0023f;fs.speed_set_h_norm_lpf[Z] += 0.5f *(fs.speed_set_h_norm[Z] - fs.speed_set_h_norm_lpf[Z]);// 推油门启动if(flag.unlock_sta){   if(fs.speed_set_h_norm[Z]>0.01f && flag.motor_preparation == 1) // 0-1{flag.taking_off = 1;}  }       fc_stv.vel_limit_z_p = MAX_Z_SPEED_UP;fc_stv.vel_limit_z_n = -MAX_Z_SPEED_DW; if( flag.taking_off ){if(flying_cnt<1000)//800ms{flying_cnt += dT_ms;}else{// 起飞1秒 后 认为已经在飞行flag.flying = 1;  }if(fs.speed_set_h_norm[Z]>0){// 设置 上升速度 vel_z_tmp[0] = (fs.speed_set_h_norm_lpf[Z] *MAX_Z_SPEED_UP);}else{// 设置 下降速度vel_z_tmp[0] = (fs.speed_set_h_norm_lpf[Z] *MAX_Z_SPEED_DW);}// 飞控系统 z速度目标标量综合设定vel_z_tmp[1] = vel_z_tmp[0] + program_ctrl.vel_cmps_h[Z] + pc_user.vel_cmps_set_z;vel_z_tmp[1] = LIMIT(vel_z_tmp[1],fc_stv.vel_limit_z_n,fc_stv.vel_limit_z_p);fs.speed_set_h[Z] += LIMIT((vel_z_tmp[1] - fs.speed_set_h[Z]),-0.8f,0.8f);//}else{fs.speed_set_h[Z] = 0 ;}float speed_set_tmp[2];
// 速度设定量 正负 参考ANO 坐标参考方向fs.speed_set_h_norm[X] = (my_deadzone(+CH_N[CH_PIT],0,50) *0.0022f);fs.speed_set_h_norm[Y] = (my_deadzone(-CH_N[CH_ROL],0,50) *0.0022f);LPF_1_(3.0f,dT_ms*1e-3f,fs.speed_set_h_norm[X],fs.speed_set_h_norm_lpf[X]);LPF_1_(3.0f,dT_ms*1e-3f,fs.speed_set_h_norm[Y],fs.speed_set_h_norm_lpf[Y]);max_speed_lim = MAX_SPEED;if(switchs.of_flow_on && !switchs.gps_on ){max_speed_lim = 1.5f *wcz_hei_fus.out;max_speed_lim = LIMIT(max_speed_lim,50,150);}    fc_stv.vel_limit_xy = max_speed_lim;//飞控系统 XY速度 目标综合量设定speed_set_tmp[X] = fc_stv.vel_limit_xy *fs.speed_set_h_norm_lpf[X] + program_ctrl.vel_cmps_h[X] + pc_user.vel_cmps_set_h[X];speed_set_tmp[Y] = fc_stv.vel_limit_xy *fs.speed_set_h_norm_lpf[Y] + program_ctrl.vel_cmps_h[Y] + pc_user.vel_cmps_set_h[Y];length_limit(&speed_set_tmp[X],&speed_set_tmp[Y],fc_stv.vel_limit_xy,fs.speed_set_h_cms);fs.speed_set_h[X] = fs.speed_set_h_cms[X];fs.speed_set_h[Y] = fs.speed_set_h_cms[Y];   // 调用检测着陆的函数land_discriminat(dT_ms);// 倾斜过上大锁if(rolling_flag.rolling_step == ROLL_END){if(imu_data.z_vec[Z<0.25f)  //75度//倾斜过 上大锁{//if(mag.mag_CALIBRATE==0){imu_state.G_reset = 1;}flag.unlock_cmd = 0;}} //
//校准中 ，复位重力方向if(sensor.gyr_CALIBRATE != 0 || sensor.acc_CALIBRATE != 0 ||sensor.acc_z_auto_CALIBRATE){imu_state.G_reset = 1;}
// 复位重力方向时，认为传感器失效if(imu_state.G_reset == 1){flag.sensor_imu_ok = 0;LED_STA.rst_imu = 1;WCZ_Data_Reset(); //复位 高度数据融合}else if(imu_state.G_reset == 0){ if(flag.sensor_imu_ok == 0){flag.sensor_imu_ok = 1;LED_STA.rst_imu = 0;ANO_DT_SendString("IMU OK!");}}/*飞行状态复位*/if(flag.unlock_sta == 0){flag.flying = 0;landing_cnt = 0;flag.taking_off = 0;flying_cnt = 0;    flag.rc_loss_back_home = 0;//复位融合if(flag.taking_off == 0){//          wxyz_fusion_reset();}}}

开关状态任务： Swtich_State_Task(1);

开关状态任务作用：判断各个任务状态是否有效

• 光流模块
• 光流质量 大于 60*
• 光流质量 大于50*
• 或者飞行之前
• 认为光流可用。判定可用延迟时间
• 光流高度 600 cm内有效
• 延时 1.5s 判断光流是否有效
• 判定高度无效
• GPS
• UWB
• OPENMV

void Swtich_State_Task(u8 dT_ms)
{switchs.baro_on = 1;// 光流模块if(sens_hd_check.of_ok || sens_hd_check.of_df_ok)// {if(sens_hd_check.of_ok){jsdata.of_qua = OF_QUALITY;jsdata.of_alt = (u16)OF_ALT;}else if(sens_hd_check.of_df_ok){jsdata.of_qua = of_rdf.quality;jsdata.of_alt = Laser_height_cm;}//if(jsdata.of_qua>50 )//|| flag.flying == 0) || flag.flying 光流质量 大于 60* // 光流质量 大于50* / 或者飞行之前 // 认为光流可用。判定可用延迟时间{if(of_quality_delay<500){of_quality_delay += dT_ms;}else{of_quality_ok = 1;}}else{of_quality_delay =0;of_quality_ok = 0;}// 光流高度 600 cm内有效if(jsdata.of_alt<600){//of_tof_on_tmp = 1;jsdata.valid_of_alt_cm = jsdata.of_alt;
// 延时 1.5s 判断光流是否有效if(of_alt_delay<1500){of_alt_delay += dT_ms;            }else{// 判定高度有效of_alt_ok = 1;}}else{// of_tof_on_tmp = 0;// 延时1.5秒判断激光高度是否有效if(of_alt_delay>0){of_alt_delay -= dT_ms; }else{//判定高度无效of_alt_ok = 0;}              }if(flag.flight_mode == LOC_HOLD){        if(of_alt_ok && of_quality_ok){switchs.of_flow_on = 1;}else{switchs.of_flow_on = 0;}}else{of_tof_on_tmp = 0;switchs.of_flow_on = 0;}    switchs.of_tof_on = of_tof_on_tmp;}else{switchs.of_flow_on = switchs.of_tof_on = 0;}//if(sens_hd_check.tof_ok){if(0)//(Laser_height_mm<1900){switchs.tof_on = 1;}else{switchs.tof_on = 0;}}else{switchs.tof_on = 0;}//GPS  //UWBif(uwb_data.online && flag.flight_mode == LOC_HOLD){switchs.uwb_on = 1;}else{switchs.uwb_on = 0;}//OPMVif(opmv.offline==0 && flag.flight_mode == LOC_HOLD){switchs.opmv_on = 1;}else{switchs.opmv_on = 0;}}

光流融合数据转换任务： ANO_OF_Data_Prepare_Task(0.001f);

函数名 ANO_OF_DATA_Check_Task
功能说明 ： 光流准备数据任务
参数 周期时间 （s）
返回值 无

void ANO_OF_Data_Prepare_Task(float dT_s)
{//ANO_OF_Data_Get(&dT_s,OF_DATA_BUF);OF_INS_Get(&dT_s,RADPS_X,RADPS_Y,imu_data.w_acc[0],imu_data.w_acc[1]);
}

数传数据交换任务：ANO_DT_Data_Exchange();

• 函数名： ANO_DT_Data_Exchange();
• 作用：数传数据交换任务
• 提示：Data_Exchange函数处理各种数据发送请求，比如想实现每5ms发送一次传感器数据至上位机，即在此函数内实现
• 调用时长：此函数应由用户每1ms调用一次

void ANO_DT_Data_Exchange(void)
{static u16 cnt = 0;static u16 senser_cnt  = 10;static u16 senser2_cnt    = 50;static u16 user_cnt   = 10;static u16 status_cnt     = 15;static u16 rcdata_cnt     = 20;static u16 motopwm_cnt    = 20;static u16 power_cnt  = 50;static u16 speed_cnt      = 50;static u16 sensorsta_cnt = 500;static u16 omv_cnt = 100;static u16 location_cnt = 500;static u8    flag_send_omv = 0; if((cnt % senser_cnt) == (senser_cnt-1))f.send_senser = 1;if((cnt % senser2_cnt) == (senser2_cnt-1))f.send_senser2 = 1;   if((cnt % user_cnt) == (user_cnt-2))f.send_user = 1;if((cnt % status_cnt) == (status_cnt-1))f.send_status = 1;    if((cnt % rcdata_cnt) == (rcdata_cnt-1))f.send_rcdata = 1;   if((cnt % motopwm_cnt) == (motopwm_cnt-2))f.send_motopwm = 1;    if((cnt % power_cnt) == (power_cnt-2))f.send_power = 1;      if((cnt % speed_cnt) == (speed_cnt-3))f.send_speed = 1;      if((cnt % sensorsta_cnt) == (sensorsta_cnt-2)){f.send_sensorsta = 1;     }   if((cnt % omv_cnt) == (omv_cnt-2)){flag_send_omv = 1;        }   if((cnt % location_cnt) == (location_cnt-3)){f.send_location = 1;        }if(++cnt>1000) cnt = 0;if(f.send_version){f.send_version = 0;ANO_DT_Send_Version(4,300,100,400,0);}else if(f.paraToSend < 0xffff){ANO_DT_SendParame(f.paraToSend);f.paraToSend = 0xffff;}else if(f.send_status){f.send_status = 0;ANO_DT_Send_Status(imu_data.rol,imu_data.pit,imu_data.yaw,wcz_hei_fus.out,(flag.flight_mode+1),flag.unlock_sta);    }   else if(f.send_speed){f.send_speed = 0;ANO_DT_Send_Speed(loc_ctrl_1.fb[Y],loc_ctrl_1.fb[X],loc_ctrl_1.fb[Z]);}else if(f.send_user){f.send_user = 0;ANO_DT_Send_User();ANO_DT_Send_User1();}else if(f.send_senser){f.send_senser = 0;ANO_DT_Send_Senser(sensor.Acc[X],sensor.Acc[Y],sensor.Acc[Z],sensor.Gyro[X],sensor.Gyro[Y],sensor.Gyro[Z],mag.val[X],mag.val[Y],mag.val[Z]);}    else if(f.send_senser2){f.send_senser2 = 0;ANO_DT_Send_Senser2(baro_height,ref_tof_height,sensor.Tempreature_C*10);//原始数据} else if(flag_send_omv){flag_send_omv = 0;if(f.send_omv_ct){f.send_omv_ct = 0;ANO_DT_SendOmvCt(opmv.cb.color_flag,opmv.cb.sta,opmv.cb.pos_x,opmv.cb.pos_y,opmv.cb.dT_ms);}else if(f.send_omv_lt){f.send_omv_lt = 0;ANO_DT_SendOmvLt(opmv.lt.sta, opmv.lt.angle, opmv.lt.deviation, opmv.lt.p_flag, opmv.lt.pos_x, opmv.lt.pos_y, opmv.lt.dT_ms);}}else if(f.send_rcdata){f.send_rcdata = 0;s16 CH_GCS[CH_NUM];for(u8 i=0;i<CH_NUM;i++){if((chn_en_bit & (1<<i)))//(Rc_Pwm_In[i]!=0 || Rc_Ppm_In[i] !=0  )//该通道有值{CH_GCS[i] = CH_N[i] + 1500;}else{CH_GCS[i] = 0;}}ANO_DT_Send_RCData(CH_GCS[2],CH_GCS[3],CH_GCS[0],CH_GCS[1],CH_GCS[4],CH_GCS[5],CH_GCS[6],CH_GCS[7],0,0);} else if(f.send_motopwm){f.send_motopwm = 0;
#if MOTORSNUM == 8ANO_DT_Send_MotoPWM(motor[0],motor[1],motor[2],motor[3],motor[4],motor[5],motor[6],motor[7]);
#elif MOTORSNUM == 6ANO_DT_Send_MotoPWM(motor[0],motor[1],motor[2],motor[3],motor[4],motor[5],0,0);
#elif MOTORSNUM == 4ANO_DT_Send_MotoPWM(motor[0],motor[1],motor[2],motor[3],0,0,0,0);
#else#endif}    else if(f.send_power){f.send_power = 0;ANO_DT_Send_Power(Plane_Votage*100,0);}else if(f.send_sensorsta){f.send_sensorsta = 0;ANO_DT_SendSensorSta(switchs.of_flow_on ,switchs.gps_on,switchs.opmv_on,switchs.uwb_on,switchs.of_tof_on);}else if(f.send_location){f.send_location = 0;ANO_DT_Send_Location(switchs.gps_on,Gps_information.satellite_num,(s32)Gps_information.longitude,(s32)Gps_information.latitude,123,456);}else if(f.send_vef){ANO_DT_Send_VER();f.send_vef = 0;}ANO_DT_Data_Receive_Anl_Task();
}

系统控制输出：

姿态角速度环控制 Att_1level_Ctrl(2*1e-3f);

void Att_1level_Ctrl(float dT_s)
{  // 改变控制参数 （最小控制周期内）ctrl_parameter_change_task();// 目标角速度赋值for(u8 i = 0;i<3;i++){att_1l_ct.exp_angular_velocity[i] = val_2[i].out;// val_2[i].out;//}att_1l_ct.exp_angular_velocity[ROL] = LIMIT(att_1l_ct.exp_angular_velocity[ROL],-MAX_ROLLING_SPEED,MAX_ROLLING_SPEED);att_1l_ct.exp_angular_velocity[PIT] = LIMIT(att_1l_ct.exp_angular_velocity[PIT],-MAX_ROLLING_SPEED,MAX_ROLLING_SPEED);/* 反馈角速度赋值 */att_1l_ct.fb_angular_velocity[ROL] = ( sensor.Gyro_deg[X] );att_1l_ct.fb_angular_velocity[PIT] = (-sensor.Gyro_deg[Y] );att_1l_ct.fb_angular_velocity[YAW] = (-sensor.Gyro_deg[Z] );/* PID计算 */                                    for(u8 i = 0;i<3;i++){PID_calculate( dT_s,            //周期 （单位：秒）0, //前馈值att_1l_ct.exp_angular_velocity[i],     //期待值（设定值）att_1l_ct.fb_angular_velocity[i],       //反馈值&arg_1[i], //PID参数结构体&val_1[i],    //PIDÊý¾Ý½á¹¹Ìå200,//»ý·ÖÎó²îÏÞ·ùCTRL_1_INTE_LIM *flag.taking_off           //integration limit£¬»ý·Ö·ù¶ÈÏÞ·ù)  ; ct_val[i] = (val_1[i].out);}/*¸³Öµ£¬×îÖÕ±ÈÀýµ÷½Ú*/mc.ct_val_rol =                   FINAL_P *ct_val[ROL];mc.ct_val_pit = X_PROPORTION_X_Y *FINAL_P *ct_val[PIT];mc.ct_val_yaw =                   FINAL_P *ct_val[YAW];/*Êä³öÁ¿ÏÞ·ù*/mc.ct_val_rol = LIMIT(mc.ct_val_rol,-1000,1000);mc.ct_val_pit = LIMIT(mc.ct_val_pit,-1000,1000);mc.ct_val_yaw = LIMIT(mc.ct_val_yaw,-400,400);
}_rolling_flag_st rolling_flag;

姿态角度环控制 Att_2level_Ctrl(6e-3f,CH_N);

电机输出控制 Motor_Ctrl_Task(2);

任务处理及控制：

线程任务执行函数： Main_Task

遥控器数据处理 RC_duty_task(11);

飞行模式设置任务 Flight_Mode_Set(11);

高度数据融合任务 (一)WCZ_Fus_Task(11);

高度数据融合任务 (二) GPS_Data_Processing_Task(11);

高度速度环控 Alt_1level_Ctrl(11e-3f);

高度环控制（一） Alt_2level_Ctrl(11e-3f)

高度环控制（二） AnoOF_DataAnl_Task(11);

灯光控制 LED_Task2(11);

罗盘数据处理任务 Mag_Update_Task(20);

程序指令控制 （一） ANO_OFDF_Task(20);

程序指令控制 （二） FlyCtrl_Task(20);

程序指令控制 （三） Ano_UWB_Data_Calcu_Task(20);

位置速度环控制 Loc_1level_Ctrl(20,CH_N);

电压： Power_UpdateTask(50);

恒温控制 : Thermostatic_Ctrl_Task(50);

延时存储 : Ano_Parame_Write_task(50);

用户自定义：

数据处理及传递 （解螺旋）MV_Decoupling(20)

【持续更新中...】2021年全国大学生电子设计大赛 （三）匿名四轴拓空者飞控系统设计解读相关推荐

1. 2021年全国大学生电子设计大赛（一）赛题解析与预测

   2021年全国大学生电子设计大赛赛题解析与预测 参考链接 往年赛题解析: 2015年无人机赛题: (2015)基本目标: (2015)发挥目标: (2015)赛题分解: 2017年赛题分析: (201 ...

2. 2021年全国大学生电子设计大赛每一个注意问题11.05

   2021年全国大学生电子设计大赛注意问题11.05 A,信号失真度测量装置 B,三相AC-DC变换电路 C 三端口DC-DC变换器 D,基于互联网的摄像测量系统 E 数字-模拟信号混合传输收发机 F ...

3. 【大学生项目与竞赛】2021年全国大学生电子设计大赛 （二）模块储备

   2021年全国大学生电子设计大赛 (二)赛题解析与预测 储备: 硬件储备: 飞控主板: 机架 动力套件: 电池: 电池充电器 光流传感器 OPENMV: 超声波传感器: 软件储备(飞控): 天穹飞控: ...

4. 2021年全国大学生电子设计大赛F题——智能送药小车，全方位解决方案+程序代码（详细注释）山东赛区国奖

   目录 1.赛题及硬件方案分析: 2.用到的主要器件清单: 3.各部分思路及代码实现 (1).小车舵机.马达驱动 (2).蓝牙通信 (3).单片机与OpenMV的串口通信 (4).单片机与OpenMV的 ...

5. 2021年全国大学生电子设计大赛G题无人机方案

   无人机踩坑提醒: 比赛前报名表千万别填第一个,不然你将会是踩坑小组,评委会把所有评分点以及扣分点通过你总结出来(文章会随时补充) 方案介绍: 本人比赛期间采用的是纯光流加openmv视觉闭环,这里建议 ...

6. 总结一下2021年全国大学生电子设计大赛A题用到的MSP432P401开发板

   作者:嵌入式历练者 ID : Eterlove 记下相关笔记,记录我的学习生活!站在巨人的肩上Standing on Shoulders of Giants! 该文章为原创,转载请注明出处和作者:ht ...

7. 2021年全国大学生电子设计大赛——信号失真度测量装置（A 题）实验报告

8. 2021年全国大学生电子设计竞赛F题中数字识别这一技术分支实践与学习笔记

   文章目录 Chapter 1 简介 Chapter 2 制作思路 Section 1 环境与成像效果及思路 Section 2 制作并训练数据集 Section 3 电脑环境配置 Section 4 ...

9. 数字-模拟信号混合传输收发机（E 题）--2021 年全国大学生电子设计竞赛

   数字-模拟信号混合传输收发机(E 题)–2021 年全国大学生电子设计竞赛 一 任务 设计并制作在同一信道进行数字-模拟信号混合传输的无线收发机.其中,数字信号由 4 个 0~9 的一组数字构成:模拟 ...

10. 2021年全国大学生电子设计竞赛重新启动通知及进度安排

    大家好,我是张巧龙,昨天晚上收到了电赛重新启动的通知. 2021年全国大学生电子设计竞赛 重新启动通知及进度安排 (电组字[2021]03号) 各赛区组织委员会.各有关高等学校: 根据全国疫情变化情况 ...

最新文章

1. mysql5.6基于GTID主从复制设置
2. 为何终端防护对ICS如此重要
3. 头文件（C++11）、从文件输入输出
4. 【GNN】AAAI2021 | 图神经网络研究进展解读
5. 百度AI快车道深圳专场，揭秘CV目标检测核心技术
6. SecureCRT 连接Win10内置ubuntu问题层层突围
7. ls命令输出的文件颜色
8. bt协议详解 DHT篇（上）
9. 高斯-勒让德积分学习
10. alpha-beta剪枝 个人理解
11. 腊八节福利送上 , 抽奖活动来啦！5本SpringMVC+MyBatis相关、3本Android Studio相关、6本Kafka相关
12. Cloudera Manager —— 端到端的企业数据中心管理工具
13. 防晒新时代，小红书美妆品牌营销趋势洞察
14. CentOs7关于图形界面系统和图形界面程序
15. MySQL实现主主同步（三台服务器）
16. Process Lasso 介绍
17. 分享源码学习，淘宝/天猫/京东2022年年货节任务自动助手软件，自动完成炸年兽任务
18. iOS篇—Demo6—抽奖转盘
19. vc zip,unzip打包，解包
20. 陌生人，咱俩一起买个房呗？

热门文章

1. python实现将点云的.bin格式文件转化为.txt格式
2. 解决sqliteman创建失败的一种方法
3. java 主板序列号_Java获得硬盘和主板的序列号
4. hbase解决海量图片存储
5. 在 Vue 中实现粒子特效 Particle Effect for Vue
6. snipaste如何滚动截图_试用了20个截图工具，我写下这份超全的软件指南。?
7. Python爬虫用到的一些浏览器代理标识
8. QQ音乐付费格式转换
9. 房产中介管理系统,房产中介预约看房系统,看房预约系统毕设作品
10. c语言房屋中介管理系统代码,房屋中介管理系统简易源代码