3.17Ano

Ano"/>

3.17Ano

3.17这个是高度环pid的文件

void Auto_Take_Off_Land_Task(u8 dT_ms)//一键自动飞行
{  这里真正实现了，自动飞行 。static u16 take_off_ok_cnt;one_key_take_off_task(dT_ms);//调用一键飞行函数	if(flag.unlock_sta){if(flag.taking_off){	if(flag.auto_take_off_land == AUTO_TAKE_OFF_NULL){flag.auto_take_off_land = AUTO_TAKE_OFF;		}}}else{auto_taking_off_speed = 0;	flag.auto_take_off_land = AUTO_TAKE_OFF_NULL;	}
	if(flag.auto_take_off_land ==AUTO_TAKE_OFF){//设置最大起飞速度s16 max_take_off_vel = LIMIT(Ano_Parame.set.auto_take_off_speed,20,200);take_off_ok_cnt += dT_ms;auto_taking_off_speed = AUTO_TAKE_OFF_KP *(Ano_Parame.set.auto_take_off_height - wcz_hei_fus.out);//计算起飞速度		//auto_taking_off_speed在后面会给高度环的公式相加		auto_taking_off_speed = LIMIT(auto_taking_off_speed,0,max_take_off_vel);//限制在一个范围		//退出起飞流程条件1，满足高度或者流程时间大于5000毫秒。if(take_off_ok_cnt>=5000 || (Ano_Parame.set.auto_take_off_height - loc_ctrl_2.exp[Z] <2))//(auto_ref_height>AUTO_TAKE_OFF_HEIGHT){flag.auto_take_off_land = AUTO_TAKE_OFF_FINISH;			}//退出起飞流程条件2，2000毫秒后判断用户正在控制油门。if(take_off_ok_cnt >2000 && ABS(fs.speed_set_h_norm[Z])>0.1f)// 一定已经taking_off,如果还在推杆，退出起飞流程{flag.auto_take_off_land = AUTO_TAKE_OFF_FINISH;}	}else {take_off_ok_cnt = 0;		if(flag.auto_take_off_land ==AUTO_TAKE_OFF_FINISH){auto_taking_off_speed = 0;			}		}
if(flag.auto_take_off_land == AUTO_LAND)//AUTO_LAND=3{//设置自动下降速度auto_taking_off_speed = -(s16)LIMIT(Ano_Parame.set.auto_landing_speed,20,200);}
}

之后的是高度环，我之前只是了解，但是还没看懂程序是怎么实现的，看了些视频，还有一半，明天再总结

单级pid串级pid视频

同上 这个大爷讲的很透彻

3.18瞌睡的看完pid的视频了，也简单了解了程序是如何实现的，来捋一捋这个文件的逻辑和，流程。

刚开始文件有函数1Auto_Take_Off_Land_Task(u8 dT_ms)//一键自动飞行 我把这个叫成函数1，
上面已经说了，这个函数1是实现了一键起飞（比较内层），
这个函数1里面又有一个one_key_take_off_task(dT_ms);//调用一键飞行 这个函数是2
这个函数2则是给函数1加了很多限制，比如标志位，条件等。
这个函数通过了，给auto_take_off_land赋AUTO_TAKE_OFF的时候 给 auto_taking_off_speed起飞参数赋值，然后就可以起飞 了。

然后这个Auto_Take_Off_Land_Task一键起飞任务放在Alt_2level_Ctrl高度2环这里，这个高度环每10ms执行一次，
这样就很通顺了。

以后写平移程序，也能仿照这样，写前进任务放在高度环或者位置环里面一直检测，如何（实现）就可以仿照高度环里面的一键起飞任务，

直到我遥控器信号触发。哈哈看到完成毕设的希望了。

后面是高度1环的函数，明天再看。

3.20
今天还完成了关于高度速度设置代码的深究，想配置一个函数来达到设置自由定高的效果
，很复杂，放弃这个方法，代码我没保存。
今天完成了math文件 注释‘熟悉
程序就不写’了 这是别人的
math

今天还完成了电机校准，程序下载还在解决。
jlink模式在keil5上已经调好了，程序成功下载了方法如下
下载配置

alt_val_2.out  = LIMIT(alt_val_2.out,-150,150);//高度2环输出这个参数
输出到高度1环
loc_ctrl_1.exp[Z] = 0.6f *fs.alt_ctrl_speed_set + alt_val_2.out;//速度前馈0.6f，直接给速度mc.ct_val_thr = loc_ctrl_1.out[Z];//高度1环输出参数  输出到电机 motor_step[m2] = mc.ct_val_thr  -mc.ct_val_yaw +mc.ct_val_rol +mc.ct_val_pit;其他三个参数是角度pid参数 rol、yaw 、pit 

输出到pwm我就有些陌生了，看了些文章。
电调和pwm

这个好像有些帮助但是，目前我还没能写出来。

前两天在改文献综述和开题报告，写的很差，粗心大意。希望我以后严谨些.

3.25这个是角度环Ano_AttCtrl
开头是pid初始化

void Att_2level_PID_Init()
void Att_1level_PID_Init()前两个的不放代码了，一切正常
void Set_Att_1level_Ki(u8 mode)
{if(mode == 0){arg_1[ROL].ki = arg_1[PIT].ki = 0;}else if(mode == 1){arg_1[ROL].ki = Ano_Parame.set.pid_att_1level[ROL][KI];arg_1[PIT].ki = Ano_Parame.set.pid_att_1level[PIT][KI];}else {arg_1[ROL].ki = arg_1[PIT].ki = CTRL_1_KI_START;//=0}}没看明白，只知道不同模式下角度1环rol pit 积分项初始化不同，这需要理解姿态解算和pid，我目前还没看懂 暂存---	
void Set_Att_2level_Ki(u8 mode)
和上一个一样，这个是2环，暂存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] += 0.25f *(( sensor.Gyro_deg[X] ) - att_1l_ct.fb_angular_velocity[ROL]);att_1l_ct.fb_angular_velocity[PIT] += 0.25f *((-sensor.Gyro_deg[Y] ) - att_1l_ct.fb_angular_velocity[PIT]);att_1l_ct.fb_angular_velocity[YAW] += 0.25f *((-sensor.Gyro_deg[Z] ) - att_1l_ct.fb_angular_velocity[YAW]);/*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];//ROL = 0,PIT = 1,YAW = 2,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);	
}
角度1环控制，注释的很明白。

Ano_LocCtrl位置pid控制，只有1环。

/*角度环PID参数初始化*/ 不用说
void Loc_1level_PID_Init()
/*位置速度环*/
void Loc_1level_Ctrl(u16 dT_ms,s16 *CH_N)
{static float loc_hand_exp_vel[2]={0};static unsigned short waite_gps_loc_cnt = 0;float ne_pos_control[2];unsigned char vel_diff = 5;float pos_ctrl_h_out[2];float pos_ctrl_w_out[2];我只把对我有用的代码截了下来，像uwb和gps有关的代码删了//仅有光流和UWBif(switchs.uwb_on && switchs.of_flow_on && (!switchs.gps_on)){mode_f[1] = 4;if(mode_f[1] != mode_f[0]){Loc_1level_PID_Init();mode_f[0] = mode_f[1];}	//==	//期望赋值
//		loc_ctrl_1.exp[X] = fs.speed_set_h[X];
//		loc_ctrl_1.exp[Y] = fs.speed_set_h[Y];h2w_2d_trans(fs.speed_set_h,imu_data.hx_vec,loc_ctrl_1.exp);//低通滤波LPF_1_(5.0f,dT_ms*1e-3f,imu_data.w_acc[X],vel_fb_d_lpf[X]);LPF_1_(5.0f,dT_ms*1e-3f,imu_data.w_acc[Y],vel_fb_d_lpf[Y]);		//反馈赋值HXYZ（水平航向坐标）if(sens_hd_check.of_ok){vel_fb_h[0] = ano_of.of2_dx_fix;//OF_DX2;vel_fb_h[1] = ano_of.of2_dy_fix;//OF_DY2;}else//sens_hd_check.of_df_ok{vel_fb_h[0] = of_rdf.gnd_vel_est_h[X];vel_fb_h[1] = of_rdf.gnd_vel_est_h[Y];	}//转换NWU（北西天）坐标h2w_2d_trans(vel_fb_h,imu_data.hx_vec,vel_fb_w);//反馈赋值+加速度超前loc_ctrl_1.fb[X] = vel_fb_w[0] + 0.03f *vel_fb_d_lpf[X];loc_ctrl_1.fb[Y] = vel_fb_w[1] + 0.03f *vel_fb_d_lpf[Y];//速度修正值赋值，用于积分fb_speed_fix[0] = uwb_data.w_vel_cmps[0];fb_speed_fix[1] = uwb_data.w_vel_cmps[1];for(u8 i =0;i<2;i++){PID_calculate( dT_ms*1e-3f,            //周期（单位：秒）loc_ctrl_1.exp[i] ,				//前馈值loc_ctrl_1.exp[i] ,				//期望值（设定值）loc_ctrl_1.fb[i] ,			//反馈值（）&loc_arg_1[i], //PID参数结构体&loc_val_1[i],	//PID数据结构体50,//积分误差限幅10 *flag.taking_off			//integration limit，积分限幅)	;	//fixPID_calculate( dT_ms*1e-3f,            //周期（单位：秒）loc_ctrl_1.exp[i] ,				//前馈值loc_ctrl_1.exp[i] ,				//期望值（设定值）fb_speed_fix[i] ,			//反馈值（）&loc_arg_1_fix[i], //PID参数结构体&loc_val_1_fix[i],	//PID数据结构体50,//积分误差限幅10 *flag.taking_off			//integration limit，积分限幅)	;	pos_ctrl_w_out[i] = loc_val_1[i].out + loc_val_1_fix[i].out;	//(PD)+(I)	}	//NWU转HXYZ水平航向坐标w2h_2d_trans(pos_ctrl_w_out,imu_data.hx_vec,pos_ctrl_h_out); //输出赋值loc_ctrl_1.out[0] = pos_ctrl_h_out[0];loc_ctrl_1.out[1] = pos_ctrl_h_out[1];	}//仅有光流else if(switchs.of_flow_on && (!switchs.gps_on)){mode_f[1] = 1;if(mode_f[1] != mode_f[0]){Loc_1level_PID_Init();mode_f[0] = mode_f[1];}loc_ctrl_1.exp[X] = fs.speed_set_h[X];loc_ctrl_1.exp[Y] = fs.speed_set_h[Y];//LPF_1_(5.0f,dT_ms*1e-3f,imu_data.h_acc[X],vel_fb_d_lpf[X]);LPF_1_(5.0f,dT_ms*1e-3f,imu_data.h_acc[Y],vel_fb_d_lpf[Y]);		if(sens_hd_check.of_ok){loc_ctrl_1.fb[X] = ano_of.of2_dx_fix + 0.03f *vel_fb_d_lpf[X];loc_ctrl_1.fb[Y] = ano_of.of2_dy_fix + 0.03f *vel_fb_d_lpf[Y];fb_speed_fix[0] = ano_of.of2_dx_fix;//OF_DX2FIX;fb_speed_fix[1] = ano_of.of2_dy_fix;//OF_DY2FIX;}else//sens_hd_check.of_df_ok{loc_ctrl_1.fb[X] = of_rdf.gnd_vel_est_h[X] + 0.03f *vel_fb_d_lpf[X];loc_ctrl_1.fb[Y] = of_rdf.gnd_vel_est_h[Y] + 0.03f *vel_fb_d_lpf[Y];fb_speed_fix[0] = of_rdf.gnd_vel_est_h[X];fb_speed_fix[1] = of_rdf.gnd_vel_est_h[Y];		}for(u8 i =0;i<2;i++){PID_calculate( dT_ms*1e-3f,            //周期（单位：秒）loc_ctrl_1.exp[i] ,				//前馈值loc_ctrl_1.exp[i] ,				//期望值（设定值）loc_ctrl_1.fb[i] ,			//反馈值（）&loc_arg_1[i], //PID参数结构体&loc_val_1[i],	//PID数据结构体50,//积分误差限幅10 *flag.taking_off			//integration limit，积分限幅)	;	//fixPID_calculate( dT_ms*1e-3f,            //周期（单位：秒）loc_ctrl_1.exp[i] ,				//前馈值loc_ctrl_1.exp[i] ,				//期望值（设定值）fb_speed_fix[i] ,			//反馈值（）&loc_arg_1_fix[i], //PID参数结构体&loc_val_1_fix[i],	//PID数据结构体50,//积分误差限幅10 *flag.taking_off			//integration limit，积分限幅)	;	loc_ctrl_1.out[i] = loc_val_1[i].out + loc_val_1_fix[i].out;	//(PD)+(I)	}		}if (loc_hand_exp_vel[X] || loc_hand_exp_vel[Y])				//判断是否有手动期望速度{ne_pos_control[0] = 0;									//位置控制量清零ne_pos_control[1] = 0;waite_gps_loc_cnt = 50;									//期望速度归零之后等待500ms}else{if (waite_gps_loc_cnt > 0){ne_pos_control[0] = 0;					//位置控制量清零ne_pos_control[1] = 0;waite_gps_loc_cnt--;if (waite_gps_loc_cnt == 0)				//估计位置已经稳定 记录期望位置以及对位置进行控制{Gps_information.hope_latitude = Gps_information.latitude_offset;		//期望位置等于当前位置Gps_information.hope_longitude = Gps_information.longitude_offset;}}else{Gps_information.hope_latitude_err = Gps_information.hope_latitude - Gps_information.latitude_offset;		//纬度误差Gps_information.hope_longitude_err = Gps_information.hope_longitude - Gps_information.longitude_offset;		//经度误差length_limit(&(Gps_information.hope_latitude_err), &(Gps_information.hope_longitude_err), MAX_SPEED*1.2f, ne_pos_control);	//控制模长限制}}		loc_ctrl_1.exp[X] =  ne_pos_control[0]*Ano_Parame.set.pid_gps_loc_2level[KP] + loc_hand_exp_vel[X]*imu_data.hx_vec[0] - loc_hand_exp_vel[Y]*imu_data.hx_vec[1];		//期望速度（航向坐标转换到世界坐标NED）loc_ctrl_1.exp[Y] = -ne_pos_control[1]*Ano_Parame.set.pid_gps_loc_2level[KP] + loc_hand_exp_vel[X]*imu_data.hx_vec[1] + loc_hand_exp_vel[Y]*imu_data.hx_vec[0];		loc_ctrl_1.fb[X] =  (Gps_information.last_N_vel) + (wcx_acc_use*0.2f);			//速度反馈+加速度提前loc_ctrl_1.fb[Y] = -(Gps_information.last_E_vel) + (wcy_acc_use*0.2f);fb_speed_fix[X] =  (Gps_information.last_N_vel);fb_speed_fix[Y] = -(Gps_information.last_E_vel);for(u8 i =0;i<2;i++){PID_calculate( dT_ms*1e-3f,            //周期（单位：秒）loc_ctrl_1.exp[i] ,				//前馈值loc_ctrl_1.exp[i] ,				//期望值（设定值）loc_ctrl_1.fb[i] ,			//反馈值（）&loc_arg_1[i], //PID参数结构体&loc_val_1[i],	//PID数据结构体50,//积分误差限幅10 *flag.taking_off			//integration limit，积分限幅)	;		//fixPID_calculate( dT_ms*1e-3f,            //周期（单位：秒）loc_ctrl_1.exp[i] ,				//前馈值loc_ctrl_1.exp[i] ,				//期望值（设定值）fb_speed_fix[i] ,			//反馈值（）&loc_arg_1_fix[i], //PID参数结构体&loc_val_1_fix[i],	//PID数据结构体50,//积分误差限幅10 *flag.taking_off			//integration limit，积分限幅)	;	if (!flag.taking_off){loc_val_1_fix[i].err_i = 0;}				}//pos_ctrl_w_out[0] = loc_val_1[0].out + loc_val_1_fix[0].out;//(PD)+(I)pos_ctrl_w_out[1] = loc_val_1[1].out + loc_val_1_fix[1].out;//(PD)+(I)w2h_2d_trans(pos_ctrl_w_out, imu_data.hx_vec, pos_ctrl_h_out);	//世界坐标（NWU）控制结果转换到航向坐标下loc_ctrl_1.out[X] = pos_ctrl_h_out[0];loc_ctrl_1.out[Y] = pos_ctrl_h_out[1];}//姿态模式，直接用期望速度转为角度（期望角度）else{mode_f[1] = 255;if(mode_f[1] != mode_f[0]){Loc_1level_PID_Init();mode_f[0] = mode_f[1];}loc_ctrl_1.out[X] = (float)MAX_ANGLE/MAX_SPEED *fs.speed_set_h[X] ;loc_ctrl_1.out[Y] = (float)MAX_ANGLE/MAX_SPEED *fs.speed_set_h[Y] ;}
}

这个看的我一脸懵，很长，看完思路的这样的：在光流模式和遥控器模式期望速度转化为角度处理后赋给loc_ctrl_1.out[X]
这个参数在角度控制文件里赋给exp_rol_tmp = - loc_ctrl_1.out[Y];
在处理后赋给att_2l_ct.exp_rol
这个参数输入到pid处理，就实现了位置控制，看完了，并没有明白它的真正意思，还需要资料来理解。

楼下研究生学长的四轴用的是无名的adrc方案。他们的飞控及零件很便宜，自抗扰技术也比pid稳定，如果可以还是用adrc的比较好。

最近在理解姿态解算，应该快了再过两天就能把公式搞清楚了。

目前遥控飞行还差轧带和飞控校准。