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();
- 任务调度:
- 飞行状态任务:Flight_State_Task(1,CH_N)
- 开关状态任务: Swtich_State_Task(1);
- 光流融合数据转换任务: ANO_OF_Data_Prepare_Task(0.001f);
- 数传数据交换任务:ANO_DT_Data_Exchange();
- 系统控制输出:
- 姿态角速度环控制 Att_1level_Ctrl(2*1e-3f);
- 姿态角度环控制 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)
模块装备图:
总览:
![在这里插入图片描述](https://img-blog.csdnimg.cn/2021051310141636.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L01ha2VyX3BhY2U=,size_16,color_FFFFFF,t_70)
接口功能图(一)
![在这里插入图片描述](https://img-blog.csdnimg.cn/20210513102020590.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L01ha2VyX3BhY2U=,size_16,color_FFFFFF,t_70)
模块功能图(二)正面
![在这里插入图片描述](https://img-blog.csdnimg.cn/20210513102918793.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L01ha2VyX3BhY2U=,size_16,color_FFFFFF,t_70)
模块功能图(三)背面
![在这里插入图片描述](https://img-blog.csdnimg.cn/20210513102836459.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L01ha2VyX3BhY2U=,size_16,color_FFFFFF,t_70)
芯片原理图:
![在这里插入图片描述](https://img-blog.csdnimg.cn/20210513103201137.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L01ha2VyX3BhY2U=,size_16,color_FFFFFF,t_70)
总体分析与解析
以一键起飞加定高为例:
头文件目录
#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()
{
lt0_run_flag ++;
LED_1ms_DRV();
circle_cnt[0] ++;
circle_cnt[0] %= CIRCLE_NUM;
if(!circle_cnt[0])
{
}
}
所有传感器读取函数: Loop_Task_0()
static void Loop_Task_0()
{
Fc_Sensor_Get();
Sensor_Data_Prepare(1);
IMU_Update_Task(1);
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)
{
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)
{
float t2_dT_s;
t2_dT_s = (float)dT_us *1e-6f;
calculate_RPY();
User_my_yaw_2level(6,line);
Att_2level_Ctrl(6e-3f,CH_N);
}
飞行任务设置:void Loop_Task_5
static void Loop_Task_5(u32 dT_us)
{
float t2_dT_s = (float)dT_us *1e-6f;
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)
{
u8 dT_ms = 20;
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);
}
电压以及温度控制函:Loop_Task_9
static void Loop_Task_9(u32 dT_us)
{
Power_UpdateTask(50);
Thermostatic_Ctrl_Task(50);
Ano_Parame_Write_task(50);
}
系统任务多线程配置函数:
static sched_task_t sched_tasks[] =
{
{Loop_Task_1 , 2000, 0 },
{Loop_Task_2 , 6000, 0 },
{Loop_Task_5 , 11000, 0 },
{Loop_Task_8 , 20000, 0 },
{Loop_Task_9 , 50000, 0 },
};
线程执行函数
u8 Main_Task(void)
{
uint8_t index = 0;
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++)
{
time_now = GetSysRunTimeUs();
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()
{
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 )
{
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 ) ;
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;
}
}
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;
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;
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;
# 加速度计转换成厘米 每平方秒 量程 +- 8G
sensor.Acc_cmss[i] = (sensor.Acc[i] *RANGE_PN16G_TO_CMSS );
}
姿态解算更新函数 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)
{
}
if(reset_imu_f==0 )
{
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
{
imu_state.gkp = 0.2f;
}
imu_state.gki = 0.01f;
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);
}
获取加速度函数 (一)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)
{
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)
{
flying_cnt += dT_ms;
}
else
{
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);
}
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];
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;
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)
{
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)
{
}
}
}
开关状态任务: 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 )
{
if(of_quality_delay<500)
{
of_quality_delay += dT_ms;
}
else
{
of_quality_ok = 1;
}
}
else
{
of_quality_delay =0;
of_quality_ok = 0;
}
if(jsdata.of_alt<600)
{
of_tof_on_tmp = 1;
jsdata.valid_of_alt_cm = jsdata.of_alt;
if(of_alt_delay<1500)
{
of_alt_delay += dT_ms;
}
else
{
of_alt_ok = 1;
}
}
else
{
of_tof_on_tmp = 0;
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)
{
switchs.tof_on = 1;
}
else
{
switchs.tof_on = 0;
}
}
else
{
switchs.tof_on = 0;
}
if(uwb_data.online && flag.flight_mode == LOC_HOLD)
{
switchs.uwb_on = 1;
}
else
{
switchs.uwb_on = 0;
}
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)))
{
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;
}
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] );
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],
&val_1[i],
200,
CTRL_1_INTE_LIM *flag.taking_off
) ;
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)
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)