2021年全国大学生电子设计大赛 （三）TM123G解读
作者：互联网
2021年全国大学生电子设计大赛 （三）TM123G解读
模块装备图：
总览：
接口功能图（一）
模块功能图（二）正面
模块功能图（三）背面
芯片原理图:
总体分析与解析
以一键起飞加定高为例：
头文件目录
#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();
标签：Task,sensor,sum,Ano,TM123G,2021,include,电子设计,Loop 来源： https://blog.csdn.net/Maker_pace/article/details/116739015