这个flightCtrl文件，是真的很长又很难。。。各种标志位啊，判断啊，开关啊，逻辑判断啊什么的，趁通宵把代码梳理一遍。
这个文件里的函数虽不算最多，但引用的头文件相当多，也就是说这个东西跟整个系统各个部分都有关系，难度也就最大。。。不过仔细看应该还是能看懂的，就是费点时间。
1、pid初始化：
void All_PID_Init(void)
{
Att_1level_PID_Init();
Att_2level_PID_Init();
Alt_1level_PID_Init();
Alt_2level_PID_Init();
Loc_1level_PID_Init();
}
正好一共五个环。。。
2、控制参数改变任务：
void ctrl_parameter_change_task()
{
if(0)
{
Set_Att_2level_Ki(0);
}
else
{
if(flag.auto_take_off_land ==AUTO_TAKE_OFF)
{
Set_Att_1level_Ki(2);
}
else
{
Set_Att_1level_Ki(1);
}
Set_Att_2level_Ki(1);
}
}
如果这个东西是这个，flag.auto_take_off_land ==AUTO_TAKE_OFF，那么就这样，否则就那样。。。仍然是调参，不知怎么用。
3、有关一键起飞
static u16 one_key_taof_start;
//主要功能为延时
void one_key_take_off_task(u16 dt_ms)
{
if(one_key_taof_start != 0)
{
one_key_taof_start += dt_ms;
if(one_key_taof_start > 1400 && flag.motor_preparation == 1)
{
one_key_taof_start = 0;
if(flag.auto_take_off_land == AUTO_TAKE_OFF_NULL)
{
flag.auto_take_off_land = AUTO_TAKE_OFF;
//解锁、起飞
flag.taking_off = 1;
}
}
}
//reset
if(flag.unlock_sta == 0)
{
one_key_taof_start = 0;
}
}
//一键起飞
void one_key_take_off()
{
if(flag.unlock_err == 0)
{
if(flag.auto_take_off_land == AUTO_TAKE_OFF_NULL && one_key_taof_start == 0)
{
one_key_taof_start = 1;
flag.unlock_cmd = 1;
}
}
}
有关一键起飞的内容，具体来讲是这样的：
1、在高度控制环void Alt_2level_Ctrl(float dT_s)中，
会自动调用自动起飞降落任务：void Auto_Take_Off_Land_Task(u8 dT_ms)
2、这个任务又会调用一键起飞任务：one_key_take_off_task(dT_ms)，同时如果此时flag.unlock_sta置位、flag.taking_off置位、flag.auto_take_off_land == AUTO_TAKE_OFF_NULL状态为起飞无效，那么就把auto_take_off_land设置为起飞，再后面会判断这个auto_take_off_land，如果是起飞，就赋值一个速度让它起飞，并且定时退出。
3、那么什么时候这些标志位会符合条件呢？就在它调用的one_key_take_off_task(dT_ms)里。在这个任务里，如果one_key_taof_start为1，那么就延时一段时间，然后使得
flag.taking_off = 1;flag.auto_take_off_land = AUTO_TAKE_OFF;
也就满足了刚刚的条件，也就可以起飞。
4、又是什么时候one_key_taof_start为1？就由我们来控制了。我们调用void one_key_take_off()，就可以了。。。
确实有点复杂。。。
4、降落判别：
void land_discriminat(s16 dT_ms)
也就是说判断什么时候该降落的函数。
if((fs.speed_set_h_norm[Z] < 0.1f) || flag.auto_take_off_land == AUTO_LAND)
{
if(ld_delay_cnt>0)
{
ld_delay_cnt -= dT_ms;
}
}
else
{
ld_delay_cnt = 200;
}
如果油门值持续小于0.1，或者启动了自动降落，那么就计数。
if(ld_delay_cnt <= 0 && (flag.thr_low || flag.auto_take_off_land == AUTO_LAND) )
{
if(mc.ct_val_thr<250 && flag.unlock_sta == 1 && flag.locking != 2)//ABS(wz_spe_f1.out <20 )
{
if(landing_cnt<1500)
{
landing_cnt += dT_ms;
}
else
{
flying_cnt = 0;
flag.taking_off = 0;
landing_cnt =0;
flag.unlock_cmd =0;
flag.flying = 0;
}
}
else
{
landing_cnt = 0;
}
}
计数完成后，且油门为低或者启动了自动降落，那么如果此时状态为：油门小于250，且解锁了，且并未在上锁的过程中，那么这个状态持续1.5s后：
flag.taking_off = 0;起飞标志位清零
flag.unlock_cmd =0; 锁定命令有效
5、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;
}
}
如果解锁了，那么有摇杆量、且电机准备好了的时候，taking_off，表示起飞了。
if(flag.taking_off)
{
if(flying_cnt<1000)//800ms
{
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);//ÏÞÖÆÔöÁ¿·ù¶È
}
如果已经起飞了，那么等待一秒，并将flying置一，表示已经在飞了。
如果油门值大于0，就是上升，设置上升速度，否则设置下降速度。
总的速度相加，赋值给vel_z_tmp[1]，并限幅。
最后赋值给fs.speed_set_h[Z]，作为系统给的最终的z轴速度。
如果没有起飞，fs.speed_set_h[Z]直接0.
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;
取遥控器的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];
同样，XY的速度等于这些相加，最后赋值给fs.这个东西。。
land_discriminat(dT_ms);
if(rolling_flag.rolling_step == ROLL_END)
{
if(imu_data.z_vec[Z]<0.25f)
{
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!");
}
}
检测imu，并改变标志位sensor_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)
{
}
}
如果解锁位位0，也就锁定状态，那么flag.taking_off = 0;flag.flying = 0;
flag.rc_loss_back_home = 0;（？？？）
没了，大体来讲，这个函数就是判断状态、改变标志位、赋值速度等等。。。
6、void Swtich_State_Task(u8 dT_ms)
开关状态任务。
大致的意思就是根据各个模块的状态来看是否打开开关。
1、气压计默认打开
2、光流：需要高度、光的质量达到要求、并且飞机处于LOC_HOLD模式，光流开关才会打开。激光模块一直是关的（？）
3、uwb不知道啥东西，但也只在定位模式才会有用
4、openmv也只会在定位的时打开。。。（这里有点疑惑，追踪和循迹都不是定位模式把，但需要用到openmv啊）
7、void Flight_Mode_Set(u8 dT_ms)：飞行模式设置
也会被轮询
if(CH_N[AUX1] <-100 && CH_N[AUX1]>-200)
{
flag.chn_failsafe = 1;
}
接收机失控，此时需要降落保护
else
{
flag.chn_failsafe = 0;
if(CH_N[AUX1]<-300)
{
flag.flight_mode = ATT_STAB;
}
else if(CH_N[AUX1]<200)
{
flag.flight_mode = LOC_HOLD;
}
else
{
flag.flight_mode = RETURN_HOME;
}
}
可以看出CH_N[AUX1]是更改模式的，可以失控、自稳、定位、回家
if(flight_mode_old != flag.flight_mode)
{
flight_mode_old = flag.flight_mode;
flag.rc_loss_back_home = 0;
}
如果状态改变，就把新值保存。flag.rc_loss_back_home = 0;是降落回家信号，但似乎没有用上过，因为要配合gps才能回家。。。
大概看完了。。。挺琐碎的，就当做个记录，最后跟工程一起总结把。。。
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
