这个文件代码有点乱啊，反正没怎么看懂，涉及到一键起飞和降落，以及关于不同任务对应不同的灯光的切换。而且注释也还可以，凑活着看下呗，其实这个并不是什么关键文件，看不懂就算了呗，下一个
#include "Ano_FlightCtrl.h"
#include "Ano_Imu.h"
#include "Drv_icm20602.h"
#include "Ano_MagProcess.h"
#include "Drv_spl06.h"
#include "Ano_MotionCal.h"
#include "Ano_AttCtrl.h"
#include "Ano_LocCtrl.h"
#include "Ano_AltCtrl.h"
#include "Ano_MotorCtrl.h"
#include "Drv_led.h"
#include "Ano_RC.h"
#include "Drv_vl53l0x.h"
#include "Ano_OF.h"
/*PID参数初始化*/
void All_PID_Init(void)
{
/*姿态控制，角速度PID初始化*/
Att_1level_PID_Init();
/*姿态控制，角度PID初始化*/
Att_2level_PID_Init();
/*高度控制，高度速度PID初始化*/
Alt_1level_PID_Init();
/*高度控制，高度PID初始化*/
Alt_2level_PID_Init();
/*位置速度控制PID初始化*/
Loc_1level_PID_Init();
}
/*控制参数改变任务*/
//也不知道单独设置外环和内环的KI有什么用
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);
}
}
/************************************************************************************************************
*************************************************************************************************************
*************************************************************************************************************
*************************************************************************************************************
*************************************************************************************************************/
/*一键翻滚（暂无）*/
void one_key_roll()
{
if(flag.flying && flag.auto_take_off_land == AUTO_TAKE_OFF_FINISH)
{
if(rolling_flag.roll_mode==0)
{
rolling_flag.roll_mode = 1;
}
}
}
static u16 one_key_taof_start; //感觉像是一个CNT和FLAG
/*一键起飞任务（主要功能为延迟）*/
void one_key_take_off_task(u16 dt_ms)
{
if(one_key_taof_start != 0) //如过不为0那么必然是解锁了，见下面一键起飞函数
{
one_key_taof_start += dt_ms; //延迟，等待至大于1400
if(one_key_taof_start > 1400 && flag.motor_preparation == 1)
{ //如果大于1400且电机准备好
one_key_taof_start = 0; //清0
if(flag.auto_take_off_land == AUTO_TAKE_OFF_NULL)
//如果还尚未执行自动启飞任务
{
flag.auto_take_off_land = AUTO_TAKE_OFF;
//执行自动启飞任务
//解锁、起飞
flag.taking_off = 1; //起飞
}
}
}
}
//下面两个函数被上面函数调用
/*一键起飞*/
void one_key_take_off()
{
if(flag.unlock_en)
{
one_key_taof_start = 1;
flag.fly_ready = 1;
}
}
/*一键降落*/
void one_key_land()
{
flag.auto_take_off_land = AUTO_LAND;
}
/************************************************************************************************************
*************************************************************************************************************
*************************************************************************************************************
*************************************************************************************************************
*************************************************************************************************************/
_flight_state_st fs;
s16 flying_cnt,landing_cnt;
extern s32 ref_height_get;
float stop_baro_hpf;
/*降落检测*/
static s16 ld_delay_cnt ;
void land_discriminat(s16 dT_ms)
{
// static s16 acc_delta,acc_old;
// acc_delta = imu_data.w_acc[Z]- acc_old;
// acc_old = imu_data.w_acc[Z];
/*油门归一值小于0.1并且垂直方向加速度小于阈值 或者启动自动降落*/
if((fs.speed_set_h_norm[Z] < 0.1f && imu_data.w_acc[Z]<200) || flag.auto_take_off_land == AUTO_LAND)
{
if(ld_delay_cnt>0)
{
ld_delay_cnt -= dT_ms;
}
}
else
{
ld_delay_cnt = 200;
}
/*意义是：如果向上推了油门，就需要等垂直方向加速度小于200cm/s2 保持200ms才开始检测*/
if(ld_delay_cnt <= 0 && (flag.thr_low || flag.auto_take_off_land == AUTO_LAND) )
{
/*油门最终输出量小于250并且没有在手动解锁上锁过程中，持续1秒，认为着陆，然后上锁*/
if(mc.ct_val_thr<250 && flag.fly_ready == 1 && flag.locking != 2)//ABS(wz_spe_f1.out <20 ) //还应当 与上速度条件，速度小于正20厘米每秒。
{
if(landing_cnt<1500)
{
landing_cnt += dT_ms;
}
else
{
flying_cnt = 0;
flag.taking_off = 0;
///
landing_cnt =0;
flag.fly_ready =0;
flag.flying = 0;
}
}
else
{
landing_cnt = 0;
}
}
else
{
landing_cnt = 0;
}
}
/*飞行状态任务*/
void Flight_State_Task(u8 dT_ms,s16 *CH_N)
{
s16 thr_deadzone;
static float max_speed_lim;
/*设置油门摇杆量*/
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.2f *(fs.speed_set_h_norm[Z] - fs.speed_set_h_norm_lpf[Z]);
/*推油门起飞*/
if(flag.fly_ready)
{
if(fs.speed_set_h_norm[Z]>0.01f && flag.motor_preparation == 1) // 0-1
{
flag.taking_off = 1;
}
}
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)
{
/*设置上升速度*/
fs.speed_set_h[Z] = fs.speed_set_h_norm_lpf[Z] *MAX_Z_SPEED_UP;
}
else
{
/*设置下降速度*/
fs.speed_set_h[Z] = fs.speed_set_h_norm_lpf[Z] *MAX_Z_SPEED_DW;
}
}
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)
{
max_speed_lim = 1.5f *wcz_hei_fus.out;
max_speed_lim = LIMIT(max_speed_lim,50,150);
}
speed_set_tmp[X] = max_speed_lim *fs.speed_set_h_norm_lpf[X];
speed_set_tmp[Y] = max_speed_lim *fs.speed_set_h_norm_lpf[Y];
length_limit(&speed_set_tmp[X],&speed_set_tmp[Y],max_speed_lim,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度 *************************** 倾斜过大上锁，慎用。
{
flag.fly_ready = 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_ok = 0;
WCZ_Data_Reset(); //复位高度数据融合
}
else if(imu_state.G_reset == 0)
{
if(flag.sensor_ok == 0)
{
flag.sensor_ok = 1;
ANO_DT_SendString("IMU OK!",sizeof("IMU OK!"));
}
}
/*飞行状态复位*/
if(flag.fly_ready == 0)
{
flag.flying = 0;
landing_cnt = 0;
flag.taking_off = 0;
flying_cnt = 0;
//复位融合
if(flag.taking_off == 0)
{
// wxyz_fusion_reset();
}
}
}
/*********************************************************************************************
**********************************************************************************************
*********************************************************************************************/
//不同飞行模式对应不同的灯光
static void LED_Switch()
{
if( (LED_state != 0 && LED_state <= 115) )
{
return;
}
//姿态稳定
if(flag.flight_mode == ATT_STAB)
{
if(flag.fly_ready)
{
LED_state = 131;
}
else
{
LED_state = 121;
}
}
//位置保持
else if(flag.flight_mode == LOC_HOLD)
{
if(flag.fly_ready)
{
LED_state = 132;
}
else
{
LED_state = 122;
}
}
//回航
else if(flag.flight_mode == RETURN_HOME)
{
if(flag.fly_ready)
{
LED_state = 133;
}
else
{
LED_state = 123;
}
}
}
/*********************************************************************************************
**********************************************************************************************
*********************************************************************************************/
static u8 of_light_ok;
static u16 of_light_delay;
//状态任务切换函数 与光流有关
//通过识别状态标志位来切换灯光
void Swtich_State_Task(u8 dT_ms)
{
switchs.baro_on = 1;
if(sens_hd_check.of_ok)//光流模块
{
if(OF_LIGHT>20 || flag.flying == 0) //光流亮度大于20或者在飞行之前，认为光流可用，判定可用延迟时间为1秒
{
if(of_light_delay<1000)
{
of_light_delay += dT_ms;
}
else
{
of_light_ok = 1;
}
}
else
{
of_light_delay =0;
of_light_ok = 0;
}
if(OF_ALT<1900 && flag.flight_mode == LOC_HOLD)
{
if(of_light_ok)
{
if(switchs.of_flow_on == 0)
{
LED_state = 13 ;//切换指示触发1下（1闪蓝）
}
switchs.of_flow_on = 1;
}
else
{
if(switchs.of_flow_on )
{
LED_state = 23 ;//切换指示触发1下（1闪红）
}
switchs.of_flow_on = 0;
}
if(switchs.of_tof_on == 0)
{
LED_state = 14 ;//切换指示触发1下（2闪蓝）
}
switchs.of_tof_on = 1;
}
else
{
if(switchs.of_tof_on )
{
LED_state = 24 ;//切换指示触发1下（2闪红）
}
switchs.of_tof_on = 0;
switchs.of_flow_on = 0;
}
}
if(sens_hd_check.tof_ok)//激光模块
{
if(tof_height_mm<1900)
{
if(switchs.tof_on == 0)
{
LED_state = 14 ;//切换指示触发1下（2闪蓝）
}
switchs.tof_on = 1;
}
else
{
if(switchs.tof_on )
{
LED_state = 24 ;//切换指示触发1下（2闪红）
}
switchs.tof_on = 0;
}
}
}
/*********************************************************************************************
**********************************************************************************************
*********************************************************************************************/
static void Speed_Mode_Switch()
{
// if( ubx_user_data.s_acc_cms > 60)// || ubx_user_data.svs_used < 6)
// {
// flag.speed_mode = 0;
// }
// else
// {
// flag.speed_mode = 1;
// }
}
u8 speed_mode_old = 255;
u8 flight_mode_old = 255;
void Flight_Mode_Set(u8 dT_ms)
{
LED_Switch();
Speed_Mode_Switch();
if(speed_mode_old != flag.speed_mode) //状态改变
{
speed_mode_old = flag.speed_mode;
if(flag.speed_mode == 1)
{
LED_state = 13;
}
else if(flag.speed_mode == 2)
{
LED_state = 14;
}
else
{
LED_state = 15;
}
//xy_speed_pid_init(flag.speed_mode);
}
///
//涉及到五通道，即一键起飞/降落
if(CH_N[AUX1]<-200)
{
flag.flight_mode = ATT_STAB;
}
else if(CH_N[AUX1]<200)
{
flag.flight_mode = LOC_HOLD;
}
else
{
flag.flight_mode = LOC_HOLD;
}
if(flight_mode_old != flag.flight_mode) //状态改变
{
flight_mode_old = flag.flight_mode;
}
}
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
