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)
- 传感器数据读取函数
- 惯性传感器检测函数
- 任务调度：
- 系统控制输出：
- 任务处理及控制：
- 线程任务执行函数： 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);
- 初始化函数结构分析：
模块装备图：
总览：
接口功能图（一）
模块功能图（二）正面
模块功能图（三）背面
芯片原理图:
总体分析与解析
以一键起飞加定高为例：
头文件目录
#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);
作用： 重力传感器 以及 磁力计 进行姿态计算
注释如下： 如果准备飞行 复位重力标记和磁力计复位标记
校准陀螺仪 不保存
自动复位
已经置位复位标记
设置重力互补融合修正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
//UWB
if(uwb_data.online && flag.flight_mode == LOC_HOLD)
{
switchs.uwb_on = 1;
}
else
{
switchs.uwb_on = 0;
}
//OPMV
if(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 == 8
ANO_DT_Send_MotoPWM(motor[0],motor[1],motor[2],motor[3],motor[4],motor[5],motor[6],motor[7]);
#elif MOTORSNUM == 6
ANO_DT_Send_MotoPWM(motor[0],motor[1],motor[2],motor[3],motor[4],motor[5],0,0);
#elif MOTORSNUM == 4
ANO_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;
评论（0）
您还未登录，请登录后发表或查看评论