2021年全国大学生电子设计大赛 （三）TM123G解读

• 模块装备图：
• • 总览：
  • 接口功能图（一）
  • 模块功能图（二）正面
  • 模块功能图（三）背面
• 芯片原理图:
• 总体分析与解析
• • 头文件目录
  • 灯光闪烁函数： INT_1ms_Task()
  • 所有传感器读取函数： Loop_Task_0()
  • 姿态环以及电机初始化：Loop_Task_1
  • 姿态环角控制
  • 飞行任务设置：void Loop_Task_5
  • 特定飞行任务函数：Loop_Task_8
  • 电压以及温度控制函：Loop_Task_9
  • 系统任务多线程配置函数：
  • 线程执行函数
• 初始化函数结构分析：
• • LED初始化： Dvr_LedInit(void)
  • 传感器数据读取函数
  • • 陀螺仪加速度提取：Drv_Icm20602_Read
    • 电子罗盘磁力计数据：Drv_AK8975_Read();
    • 读取气压计的数据：(s32)Drv_Spl0601_Read()
  • 惯性传感器检测函数
  • • 静止检测函数：motionless_check(dT_ms);
    • 陀螺仪MPU6050 函数 MPU6050_Data_Offset();
    • 惯性传感器转换单位 函数段
    • 姿态解算更新函数 IMU_Update_Task(1);
  • 获取加速度函数 (一)WCZ_Acc_Get_Task();
  • 获取加速度函数 ()WCXY_Acc_Get_Task();

模块装备图：

总览：

接口功能图（一）

模块功能图（二）正面

模块功能图（三）背面

芯片原理图:

总体分析与解析

以一键起飞加定高为例：

头文件目录

#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++)
	{
		//获取系统当前时间，单位US
		 time_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;   // +-8G
				sum_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] 							 
		# 加速度计转换成厘米 每平方秒 量程 +- 8G
			sensor.Acc_cmss[i] = (sensor.Acc[i] *RANGE_PN16G_TO_CMSS );//   /65535 * 16*981; +-8G	
		}

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

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

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