精华内容
下载资源
问答
  • 匿名OpenMV4资料包
  • 最近做比赛,购买了匿名的东西,这些代码就无偿分享给大家了,仅供学习,如有侵权,联系我删除。(其实我就是想换几个下载积分!!)
  • 本人最近一段时间在学习匿名飞控(使用的是匿名拓空者),也去网上看了很多资料和一些大佬的见解,学到了一些东西,所以想和大家一起学习讨论一下,互帮互助,共同进步。同时也是为了方便查找,防止遗忘。若是我有...

    本人最近一段时间在学习匿名飞控(使用的是匿名拓空者),也去网上看了很多资料和一些大佬的见解,学到了一些东西,所以想和大家一起学习讨论一下,互帮互助,共同进步。同时也是为了方便查找,防止遗忘。若是我有什么理解不对的地方,大家可以指出来。
    1.首先先看Drv_OpenMV.C这个C文件`

    void OpenMV_Byte_Get(u8 bytedata)
    {	
    	static u8 len = 0,rec_sta;
    	u8 check_val=0;
    	
    	//
    	openmv_buf[rec_sta] = bytedata;
    	//
    	if(rec_sta==0)
    	{
    		if(bytedata==0xaa)
    		{
    			rec_sta++;
    		}
    		else
    		{
    			rec_sta=0;
    		}
    	}
    	else if(rec_sta==1)
    	{
    		if(1)//(bytedata==0x29)未确定
    		{
    			rec_sta++;
    		}	
    		else
    		{
    			rec_sta=0;
    		}		
    	}
    	else if(rec_sta==2)
    	{
    		if(bytedata==0x05)
    		{
    			rec_sta++;
    		}
    		else
    		{
    			rec_sta=0;
    		}		
    	}
    	else if(rec_sta==3)
    	{
    		if(bytedata==0x41 || bytedata==0x42)
    		{
    			rec_sta++;
    		}
    		else
    		{
    			rec_sta=0;
    		}		
    	}
    	else if(rec_sta==4)
    	{
    		//
    		len = bytedata;
    		if(len<20)
    		{
    			rec_sta++;
    		}		
    		else
    		{
    			rec_sta=0;
    		}
    	}
    	else if(rec_sta==(len+5))
    	{
    		//
    		for(u8 i=0;i<len+5;i++)
    		{
    			check_val += openmv_buf[i];
    		}
    		//
    		if(check_val == bytedata)
    		{
    			//解析成功
    			OpenMV_Data_Analysis(openmv_buf,len+6);
    			//
    			rec_sta=0;
    		}
    		else
    		{
    			rec_sta=0;
    		}		
    	}
    	else
    	{
    		//	
    		rec_sta++;
    	}
    	
    }
    

    很明显,这个函数的作用没有什么好说的,就是从串口中获取openmv的字节数据然后对数据进行解析,获得四轴需要的数据。

    static void OpenMV_Data_Analysis(u8 *buf_data,u8 len)
    {
    	/*0x41为openmv检测色块时特有的功能位*/
    	if(*(buf_data+3)==0x41)
    	{
    		/*找寻的颜色*/
    		opmv.cb.color_flag = *(buf_data+5);
    		/*openmv正常检测到色块后的标志位*/
    		opmv.cb.sta = *(buf_data+6);
    		/*寻找到的色块中心X,Y坐标*/
    		opmv.cb.pos_x = (s16)((*(buf_data+7)<<8)|*(buf_data+8));
    		opmv.cb.pos_y = (s16)((*(buf_data+9)<<8)|*(buf_data+10));
    		opmv.cb.dT_ms = *(buf_data+11);
    		//色块追踪模式
    		opmv.mode_sta = 1;
    		//
    		f.send_omv_ct = 1;
    	}
    	/*openmv飞控循迹功能字*/
    	else if(*(buf_data+3)==0x42)
    	{
    		/*巡线检测标志位*/
    		opmv.lt.sta = *(buf_data+5);
    		//偏航角度
    		opmv.lt.angle = (s16)((*(buf_data+6)<<8)|*(buf_data+7));
    		//偏移距离
    		opmv.lt.deviation = (s16)((*(buf_data+8)<<8)|*(buf_data+9));
    		//是否悬停标志位
    		opmv.lt.p_flag = *(buf_data+10);
    		//中心点x坐标
    		opmv.lt.pos_x = (s16)((*(buf_data+11)<<8)|*(buf_data+12));
    		//中心点y坐标
    		opmv.lt.pos_y = (s16)((*(buf_data+13)<<8)|*(buf_data+14));
    		opmv.lt.dT_ms = *(buf_data+15);
    		//巡线模式
    		opmv.mode_sta = 2;
    		//
    		f.send_omv_lt = 1;
    	}
    	//
    	OpenMV_Check_Reset();
    }
    

    大家可以注意到,这个函数已经在第一个函数中被调用过了,而且根据注释的意思可以明白这个函数的作用就是对openmv发送过来的数据进行解析,得到相应的数据。因为在这里我们研究的是寻色块,所以主要对寻色块部分代码进行分析。

    void OpenMV_Offline_Check(u8 dT_ms)
    {
    	if(offline_check_time<OPMV_OFFLINE_TIME_MS)
    	{
    		offline_check_time += dT_ms;
    	}
    	else
    	{
    		opmv.offline = 1;
    		opmv.mode_sta = 0;
    	}
    	
    }
    

    这个函数也很简单,主要作用就是用于检测openmv是否连接到主控板。

    static void OpenMV_Check_Reset()
    {
    	offline_check_time = 0;
    	opmv.offline = 0;
    }
    

    这个函数的作用是openmv掉线后重新复位openmv。
    总体来说,这个c文件的内容非常简单容易理解。接下来我们要去查找解析过后的数据在哪里被调用,如何被调用。也就是确立从视觉识别到飞行控制等一系列流程是如何通过代码实现的。

    2.Ano_OPMV_CBTracking_Ctrl.c文件

    #define IMU_ROL                 (imu_data.rol)     //横滚角
    #define IMU_PIT                 (imu_data.pit)     //俯仰角
    #define RELATIVE_HEIGHT_CM           (jsdata.valid_of_alt_cm)  //相对高度
    //
    #define OF_DATA_SOURCE               ((sens_hd_check.of_ok)?1:0)
    #define VELOCITY_CMPS_X_S1           (OF_DX2FIX)                 //载体运动速度h_x
    #define VELOCITY_CMPS_Y_S1           (OF_DY2FIX)                 //载体运动速度h_y
    #define VELOCITY_CMPS_X_S2           (of_rdf.gnd_vel_est_h[0])   //载体运动速度h_x
    #define VELOCITY_CMPS_Y_S2           (of_rdf.gnd_vel_est_h[1])   //载体运动速度h_y
    //
    #define CBT_KP                  (0.80f)  //比例项
    #define CBT_KD                  (0.20f)  //微分项
    #define CBT_KF                  (0.20f)  //前馈项
    
    //需要操作赋值的外部变量:
    
    
    //===============================
    //全局变量:
    static u16 target_loss_hold_time;
    _ano_opmv_cbt_ctrl_st ano_opmv_cbt_ctrl;
    static s16 ref_carrier_velocity[2];
    static float decou_pos_pixel_lpf[2][2];
    
    //参数设定?
    /*
    	
    */
    #define PIXELPDEG_X    2.4f  //每1角度对应的像素个数,与分辨率和焦距有关,需要调试标定。//3.2f->(160*120,3.6mm)  2.4f->(160*120,2.8mm)
    #define PIXELPDEG_Y    2.4f  //每1角度对应的像素个数,与分辨率和焦距有关,需要调试标定。
    #define CMPPIXEL_X     0.01f     //每像素对应的地面距离,与焦距和高度有关,需要调试标定。//目前粗略标定
    #define CMPPIXEL_Y     0.01f     //每像素对应的地面距离,与焦距和高度有关,需要调试标定。
    #define TLH_TIME       1000   //判断目标丢失的保持时间。
    

    一进入这个c文件我就感觉头疼,看着这满屏幕的定义我脑子里都是“这是啥,这又是啥”。不过没关系,我们大体浏览一下接着往下看,说不定下面很简单呢。

    void ANO_CBTracking_Task(u8 dT_ms)
    {
    	//开启控制的条件,可以自己修改
    	//为色块追踪模式
    	if(opmv.mode_sta==1 && opmv_ct_sta.en)
    	{
    		//跟踪数据旋转解耦合
    		ANO_CBTracking_Decoupling(&dT_ms,IMU_ROL,IMU_PIT);
    		//跟踪数据计算
    		ANO_CBTracking_Calcu(&dT_ms,(s32)RELATIVE_HEIGHT_CM);
    	}
    	else
    	{
    		//reset
    		ano_opmv_cbt_ctrl.target_loss = 1;
    	}
    }
    

    这个函数的作用就是经过一系列的数学处理,计算出四轴追踪色块时的水平速度,简单来说就是实现色块追踪的功能。

    static void ANO_CBTracking_Decoupling(u8 *dT_ms,float rol_degs,float pit_degs)
    {
    	float dT_s = (*dT_ms) *1e-3f;
    
    	//有识别到目标
    	if(opmv.cb.sta != 0)//(opmv.cb.color_flag!=0)
    	{
    		//
    		ano_opmv_cbt_ctrl.target_loss = 0;
    		target_loss_hold_time = 0;
    	}
    	else
    	{
    		//延迟一定时间
    		if(target_loss_hold_time<TLH_TIME)
    		{
    			target_loss_hold_time += *dT_ms;
    		}
    		else
    		{
    			//目标丢失标记置位
    			ano_opmv_cbt_ctrl.target_loss = 1;
    		}
    	}
    	//将串口解析到的色块中心点坐标数据换到飞控坐标系
    	ano_opmv_cbt_ctrl.opmv_pos[0] =  opmv.cb.pos_y;
    	ano_opmv_cbt_ctrl.opmv_pos[1] = -opmv.cb.pos_x;
    	//
    	if(opmv.cb.sta != 0)
    	{
    		//更新姿态量对应的偏移量
    		ano_opmv_cbt_ctrl.rp2pixel_val[0] = -PIXELPDEG_X *pit_degs;
    		ano_opmv_cbt_ctrl.rp2pixel_val[1] = -PIXELPDEG_Y *rol_degs;
    		/*对得到的结果进行限幅*/
    		ano_opmv_cbt_ctrl.rp2pixel_val[0] = LIMIT(ano_opmv_cbt_ctrl.rp2pixel_val[0],-60,60);//高度120pixel
    		ano_opmv_cbt_ctrl.rp2pixel_val[1] = LIMIT(ano_opmv_cbt_ctrl.rp2pixel_val[1],-80,80);//宽度160pixel
    		//赋值参考的载体运动速度
    		if(OF_DATA_SOURCE==1)
    		{
    			//来源1  ANO_OF
    			ref_carrier_velocity[0] = VELOCITY_CMPS_X_S1;
    			ref_carrier_velocity[1] = VELOCITY_CMPS_Y_S1;		
    		}
    		else
    		{
    			//来源2 UP_OF
    			ref_carrier_velocity[0] = VELOCITY_CMPS_X_S2;
    			ref_carrier_velocity[1] = VELOCITY_CMPS_Y_S2;					
    		}
    	}
    	else
    	{
    		//姿态量对应偏移量保持不变
    		//参考的载体运动速度复位0
    		ref_carrier_velocity[0] = 0;
    		ref_carrier_velocity[1] = 0;
    	}
    
    	//
    	if(ano_opmv_cbt_ctrl.target_loss==0) //有效,没丢失
    	{
    		//得到平移偏移量,并低通滤波
    		decou_pos_pixel_lpf[0][0] += 0.2f *((ano_opmv_cbt_ctrl.opmv_pos[0] - ano_opmv_cbt_ctrl.rp2pixel_val[0]) - decou_pos_pixel_lpf[0][0]);
    		decou_pos_pixel_lpf[0][1] += 0.2f *((ano_opmv_cbt_ctrl.opmv_pos[1] - ano_opmv_cbt_ctrl.rp2pixel_val[1]) - decou_pos_pixel_lpf[0][1]);
    		//再滤一次
    		decou_pos_pixel_lpf[1][0] += 0.2f *(decou_pos_pixel_lpf[0][0] - decou_pos_pixel_lpf[1][0]);
    		decou_pos_pixel_lpf[1][1] += 0.2f *(decou_pos_pixel_lpf[0][1] - decou_pos_pixel_lpf[1][1]);
    		//赋值,这两个值将应用到下一个函数的色块跟踪处理计算,得到的是平移偏移量
    		ano_opmv_cbt_ctrl.decou_pos_pixel[0] = decou_pos_pixel_lpf[1][0];
    		ano_opmv_cbt_ctrl.decou_pos_pixel[1] = decou_pos_pixel_lpf[1][1];
    	}
    	else //丢失目标
    	{
    //		ano_opmv_cbt_ctrl.decou_pos_pixel[0] = ano_opmv_cbt_ctrl.decou_pos_pixel[1] = 0;
    		//低通复位到0
    		LPF_1_(0.2f,dT_s,0,ano_opmv_cbt_ctrl.decou_pos_pixel[0]);
    		LPF_1_(0.2f,dT_s,0,ano_opmv_cbt_ctrl.decou_pos_pixel[1]);
    
    	}
    	//
    
    }
    

    这个函数在实现色块追踪的函数中被调用过,该函数的主要作用就是解耦合。耦合简单来说就是两个事物之间相互作用和相互影响。在这里的耦合会影响到飞控对色块位置的解算,所以要消除这种关系。我们可以分析这种关系产生的原因,因为openmv固定在四轴上,有四轴基础的人都知道四轴的水平移动会引起四轴飞行姿态的改变,而飞行姿态的改变则会导致openmv镜头的移动,从而使得openmv对色块的定位不准确。好了,知道了这层关系后事情就变的简单了,无非就是消除这种关系使openmv发送回来的数据变得更准确而已。这里我会详细分析一下解耦合的算法。
    在这里插入图片描述

    在这里我们假设该图片为openmv识别到的图像。A0为四轴水平时色块的所在位置(也就是色块真实的位置)。但随后四轴会开始追踪色块,引起自身姿态的改变,镜头偏移,导致openmv发回的是A1的坐标。因为我们要获得A0的坐标。由高中物理可知,四轴沿A0A1的运动可以分解为沿roll和pitch方向的运动,据图分析,我们可以列出两个方程。
    y1-Δy=y0
    x1-Δx=x0
    我们可以知道Δx的大小与pitch的姿态量有关,pitch越大,Δx越大,存在一个正比关系。同理Δy与roll也存在正比关系。同时注意到

    #define PIXELPDEG_X    2.4f  //每1角度对应的像素个数,与分辨率和焦距有关,需要调试标定。//3.2f->(160*120,3.6mm)  2.4f->(160*120,2.8mm)
    #define PIXELPDEG_Y    2.4f  //每1角度对应的像素个数,与分辨率和焦距有关,需要调试标定。
    

    也就是roll和pitch每偏转一个角度,坐标(像素)就移动相应大小。也就是说我们只要能够得到姿态量就能根据方程知道该色块的真实坐标。得到了色块的真实坐标就完成了解耦合。这就是匿名飞控openmv寻色块的解耦合算法。

    static void ANO_CBTracking_Calcu(u8 *dT_ms,s32 relative_height_cm)
    {
    	//有效高度
    	static float relative_height_cm_valid;
    	/*过去的偏差*/
    	static float g_pos_err_old[2];
    	//相对高度赋值
    	if(relative_height_cm<500)
    	{
    		relative_height_cm_valid = relative_height_cm;
    	}
    	else
    	{
    		//null
    	}
    	//记录历史值
    	g_pos_err_old[0] = ano_opmv_cbt_ctrl.ground_pos_err_h_cm[0];
    	g_pos_err_old[1] = ano_opmv_cbt_ctrl.ground_pos_err_h_cm[1];
    	//得到地面偏差,单位厘米
    	/*这是根据实际得到的数值进行解算得到的实际偏差*/
    	ano_opmv_cbt_ctrl.ground_pos_err_h_cm[0] = CMPPIXEL_X *relative_height_cm_valid *ano_opmv_cbt_ctrl.decou_pos_pixel[0];
    	ano_opmv_cbt_ctrl.ground_pos_err_h_cm[1] = CMPPIXEL_Y *relative_height_cm_valid *ano_opmv_cbt_ctrl.decou_pos_pixel[1];
    	//计算微分偏差,单位厘米每秒
    	float gped_tmp[2];
    	//微分偏差=(实际偏差-过去偏差)cm*(1000/周期);单位为cm/s
    	gped_tmp[0] = (ano_opmv_cbt_ctrl.ground_pos_err_h_cm[0] - g_pos_err_old[0])*(1000/(*dT_ms));
    	gped_tmp[1] = (ano_opmv_cbt_ctrl.ground_pos_err_h_cm[1] - g_pos_err_old[1])*(1000/(*dT_ms));
    	//增加了对微分的滤波
    	ano_opmv_cbt_ctrl.ground_pos_err_d_h_cmps[0] += 0.2f *(gped_tmp[0] - ano_opmv_cbt_ctrl.ground_pos_err_d_h_cmps[0]);
    	ano_opmv_cbt_ctrl.ground_pos_err_d_h_cmps[1] += 0.2f *(gped_tmp[1] - ano_opmv_cbt_ctrl.ground_pos_err_d_h_cmps[1]);
    	//计算目标的地面速度,单位厘米每秒
    //	s16 temp[2];
    //	temp[0] = (s16)(ano_opmv_cbt_ctrl.ground_pos_err_d_h_cmps[0] + ref_carrier_velocity[0]) ;
    //	temp[1] = (s16)(ano_opmv_cbt_ctrl.ground_pos_err_d_h_cmps[1] + ref_carrier_velocity[1]) ;
    	ano_opmv_cbt_ctrl.target_gnd_velocity_cmps[0] = (ano_opmv_cbt_ctrl.ground_pos_err_d_h_cmps[0] + ref_carrier_velocity[0]) ;
    	ano_opmv_cbt_ctrl.target_gnd_velocity_cmps[1] = (ano_opmv_cbt_ctrl.ground_pos_err_d_h_cmps[1] + ref_carrier_velocity[1]) ;
    }
    

    这个函数首先将解耦合函数中得到的真实偏移量(像素)换成真实偏移量(厘米),其中涉及到的数学运算在文末会给大家推演一遍。这个函数最后算出的结果用于下面函数的前馈控制。

    void ANO_CBTracking_Ctrl(u8 *dT_ms,u8 en)
    {
    	//开启控制
    	if(en)
    	{
    		//距离偏差PD控制和速度前馈,该值为最终控制后的偏差
    		//最终偏差=反馈系数*反馈值+比例系数*偏差+微分系数*差值
    		ano_opmv_cbt_ctrl.exp_velocity_h_cmps[0]\
    		= CBT_KF *ano_opmv_cbt_ctrl.target_gnd_velocity_cmps[0]\
    		+ CBT_KP *(ano_opmv_cbt_ctrl.ground_pos_err_h_cm[0])\
    		+ CBT_KD *ano_opmv_cbt_ctrl.ground_pos_err_d_h_cmps[0];
    		
    		ano_opmv_cbt_ctrl.exp_velocity_h_cmps[1]\
    		= CBT_KF *ano_opmv_cbt_ctrl.target_gnd_velocity_cmps[1]\
    		+ CBT_KP *(ano_opmv_cbt_ctrl.ground_pos_err_h_cm[1])\
    		+ CBT_KD *ano_opmv_cbt_ctrl.ground_pos_err_d_h_cmps[1];
    	}
    	else
    	{
    		//如果没有偏差就赋0,保持原位不动
    		ano_opmv_cbt_ctrl.exp_velocity_h_cmps[0] = ano_opmv_cbt_ctrl.exp_velocity_h_cmps[1] = 0;
    	}
    
    }	
    
    

    这是该C文件的最后一个函数,就是前馈+PD控制算出x,y的速度,很简单。该C文件的作用就是将openmv发送回来的数据建立飞行控制追踪到目标色块。
    最后来到
    3.Ano_OPMV_Ctrl.c文件

    void ANO_OPMV_Ctrl_Task(u8 dT_ms)
    {
    	//判断高度标记的条件
    	if( RELATIVE_HEIGHT_CM >40)
    	{
    		//起飞大于40厘米,标记置位
    		opmv_ct_sta.height_flag =1;
    	}
    	//复位高度标记的条件
    	if(flag.unlock_sta ==0)
    	{
    		//上锁后标记复位
    		opmv_ct_sta.height_flag =0;
    	}
    	
    	//判断开启条件(可自行设计)
    	if
    	(
    		switchs.of_flow_on                   //光流有效
    		&& switchs.opmv_on                   //mv有效
    		&& opmv_ct_sta.height_flag !=0       //高度标记不为0
    		&& flag.flight_mode2 == 1            //AUX2通道模式值为1,参考FlightCtrl.c内程序
    	)
    	{
    		opmv_ct_sta.en = 1;
    	}
    	else
    	{
    		opmv_ct_sta.en = 0;
    	}
    	
    	
    	//不同模式执行不同功能
    	//当模式为色块追踪时,只进行pit、roll程控
    	if(opmv.mode_sta==1)
    	{
    		//
    		opmv_ct_sta.reset_flag = 0;
    		//
    		ANO_CBTracking_Ctrl(&dT_ms,opmv_ct_sta.en);
    		//调用用户程控函数赋值控制量
    		//openmv的水平速度控制
    		Program_Ctrl_User_Set_HXYcmps(ano_opmv_cbt_ctrl.exp_velocity_h_cmps[0],ano_opmv_cbt_ctrl.exp_velocity_h_cmps[1]);
    	}
    	//当模式为巡线模式时同时进行pit、yaw、roll程控
    	else if(opmv.mode_sta == 2)
    	{
    		//
    		opmv_ct_sta.reset_flag = 0;
    		//
    		ANO_LTracking_Ctrl(&dT_ms,opmv_ct_sta.en);
    		//调用用户程控函数赋值控制量
    		//openmv的水平速度控制
    		Program_Ctrl_User_Set_HXYcmps(ano_opmv_lt_ctrl.exp_velocity_h_cmps[0],ano_opmv_lt_ctrl.exp_velocity_h_cmps[1]);		
    		Program_Ctrl_User_Set_YAWdps(ano_opmv_lt_ctrl.exp_yaw_pal_dps);
    	}
    	else
    	{
    		//reset
    		if(opmv_ct_sta.reset_flag==0)
    		{
    			opmv_ct_sta.reset_flag = 1;
    			Program_Ctrl_User_Set_HXYcmps(0,0);		
    			Program_Ctrl_User_Set_YAWdps(0);			
    		}
    	}
    	
    }
    

    这个函数也很简单,就是对巡线和寻色块的一个综合控制,就是将xy的速度输出。

    最后,这是openmv从像素到厘米单位转换的数学推演:
    首先,我们假设偏移量为x,像素为p,openmv镜头中的距离为d,高度为h,夹角为θ,如图
    在这里插入图片描述
    当高度固定时,实际的偏移量为 (d/p)*x cm,其中d/p单位为 cm/像素 。当高度变化时,d会发生改变。d/p会发生变化,因此我们要计算一种所有高度都通用的计算方法。 首先我们根据图中列出一个方程:
    tanθ=2h/d
    之后我们假设四轴高度变化为H,距离变化为D,θ不会改变,同理可以列出一个方程:
    tanθ=2H/D
    我们要求的是D,因为知道了D,就可以通过(D/P)*x算出实际偏移量了。
    所以联立两个方程,消去θ,得到
    D=(H/h)*d
    所以实际的偏移量为[(d/h)*1/p]xH
    分析公式会发现,只有x,H为变量,d,h为第一次测量出来的数据,为定值,p是像素,为定值,所以可以将第一项看作是一个常数。

    	//得到地面偏差,单位厘米
    	/*这是根据实际得到的数值进行解算得到的实际偏差*/
    	ano_opmv_cbt_ctrl.ground_pos_err_h_cm[0] = CMPPIXEL_X *relative_height_cm_valid *ano_opmv_cbt_ctrl.decou_pos_pixel[0];
    	ano_opmv_cbt_ctrl.ground_pos_err_h_cm[1] = CMPPIXEL_Y *relative_height_cm_valid *ano_opmv_cbt_ctrl.decou_pos_pixel[1];
    

    这就是这部分代码的计算。

    展开全文
  • 匿名代码Openmv循迹代码解读

    千次阅读 2019-07-30 17:08:58
    /***************************************************************************************************...源码:匿名TI板飞控源码+openmvH4 说明:限于本人水平有限,并不能解读的很详细,还望各位大佬能够补充...

    /************************************************************************
    作者:不会写代码的菜鸟
    时间:2019.7.30
    源码:匿名TI板飞控源码+openmvH4
    说明:限于本人水平有限,并不能解读的很详细,还望各位大佬能够补充
    /
    //
    文件 Drv_OpenMV.c
    功能
    将从openmv串口所得的数据包进行解析,之后将解析的数据传出去,Openmv的掉线检测,Openmv的掉线检测复位
    /

    1.函数:static void OpenMV_Data_Analysis(u8 *buf_data,u8 len)
    名称:Openmv数据解析函数
    功能:解析数据包,并将数据传出去

    if(*(buf_data+3)==0x41)  //色块追踪数据包的功能字
    {
    	opmv.cb.color_flag = *(buf_data+5);
    	opmv.cb.sta = *(buf_data+6);
    	opmv.cb.pos_x = (s16)((*(buf_data+7)<<8)|*(buf_data+8));
    	opmv.cb.pos_y = (s16)((*(buf_data+9)<<8)|*(buf_data+10));
    	opmv.mode_sta = 1;  //色块追踪模式
    }
    else if(*(buf_data+3)==0x42) //飞控循迹的功能字,传递方式可以对照Openmv的数据打包方式更好理解
    {
    	opmv.lt.sta = *(buf_data+5);  //巡线检测到的标志位
    	opmv.lt.angle = (s16)((*(buf_data+6)<<8)|*(buf_data+7));  //偏航角度
    	opmv.lt.deviation = (s16)((*(buf_data+8)<<8)|*(buf_data+9));  //偏移距离
    	opmv.lt.p_flag = *(buf_data+10);  //是否是悬停的标志位
    	opmv.lt.pos_x = (s16)((*(buf_data+11)<<8)|*(buf_data+12));  //中心点的x坐标
    	opmv.lt.pos_y = (s16)((*(buf_data+13)<<8)|*(buf_data+14));  //中心点的Y坐标
    	opmv.mode_sta = 2;  //巡模式
    }
    

    ************************************************************************/
    //

    //
    文件 Ano_OPMV_Ctrl.c
    功能
    openmv检测到的数据经过一系列处理后,在这个文件夹里面被使用,实现飞机的自动巡线
    /************************************************************************
    1.函数:void ANO_OPMV_Ctrl_Task(u8 dT_ms)
    名称:Openmv控制任务
    功能:实现一系列的标志位的置位以及判断执行那种程控函数

    *opmv_ct_sta.height_flag =1(飞机高于四十厘米的高度标记置位)
    *opmv_ct_sta.en = 1(当光流,openmv,高度标记置位以及AUX2通道值在范围内允许Openmv正常巡线)
    *opmv.mode_sta(当数据包解析到这个为1的时候是色块追踪,解析到为2的时候为巡线)

    if(opmv.mode_sta==1)  //为色块追踪只进行pit,rol程控
    {
    	opmv_ct_sta.reset_flag = 0;
    	ANO_CBTracking_Ctrl(&dT_ms,opmv_ct_sta.en);
    	//调用用户程控函数赋值控制量
    Program_Ctrl_User_Set_HXYcmps(ano_opmv_cbt_ctrl.exp_velocity_h_cmps[0],ano_opmv_cbt_ctrl.exp_velocity_h_cmps[1]);
    }
    else if(opmv.mode_sta == 2)  //为巡线是pit,rol,yaw同时进行程控
    {
    	opmv_ct_sta.reset_flag = 0;
    	ANO_LTracking_Ctrl(&dT_ms,opmv_ct_sta.en);
    	//调用用户程控函数赋值控制量
    	
    Program_Ctrl_User_Set_HXYcmps(ano_opmv_lt_ctrl.exp_velocity_h_cmps[0],ano_opmv_lt_ctrl.exp_velocity_h_cmps[1]);		
    Program_Ctrl_User_Set_YAWdps(ano_opmv_lt_ctrl.exp_yaw_pal_dps);
    }
    

    ************************************************************************/

    //
    文件 Ano_OPMV_LineTracking_Ctrl.c
    功能
    对openmv的数据进行处理后给到程控去使用,总之在这里就是将得到的数据进行合适的数学处理,以及一些状态位的判断置位
    /************************************************************************
    1.函数:void ANO_LTracking_Task(u8 dT_ms)
    名称:巡线任务函数
    功能:放到伪线程里面进行执行,再执行里面的子函数
    ************************************************************************/

    /************************************************************************
    2.函数:static void ANO_LTracking_Decoupling(u8 *dT_ms,float rol_degs,float pit_degs)
    名称:巡线解耦和
    功能:解除因机体横滚旋转而造成寻线目标位置变化的耦合,也称作“旋转解耦”或“旋转补偿”。并耦合高度得到地面偏差,用解算的地面偏差给到期望速度,以修正偏差。
    *opmv.lt.sta(openmv识别标志位,即flag,识别到直线或者转角的标志位
    *ano_opmv_lt_ctrl.opmv_pos = -opmv.lt.deviation(用这个值最终得到这个反馈值ano_opmv_lt_ctrl.decou_pos_pixel)
    ************************************************************************/

    /************************************************************************
    3.函数:static void ANO_LTracking_Calcu(u8 *dT_ms,s32 relative_height_cm)
    名称:巡线计算处理
    功能:求出位置偏差以及位置偏差的微分,以便用到下面的void ANO_LTracking_Ctrl(u8 *dT_ms,u8 en)函数,执行PD+前馈控制
    *大致方法和前面的色块追踪大致相同
    ************************************************************************/

    /************************************************************************
    4.函数:void ANO_LT_StepProcedure(u8 *dT_ms)
    名称:巡线循迹分布控制函数
    功能:就是分步实现飞机的前进,左右转弯等等

    step_pro_sta(分步控制的标志位)
    ano_opmv_lt_ctrl.exp_yaw_pal_dps(将航角速度赋给这个)
    ano_opmv_lt_ctrl.exp_velocity_h_cmps[0]=0(飞机循迹过程中的飞行速度)
    8ano_opmv_lt_ctrl.exp_velocity_h_cmps[1]=0(左右位置的偏差距离)
    *confirm_cnt(确认次数,相当于一个延时的作用,当确定捕捉到后才往下执行转角的动作)
    

    *左转角度为负,右转角度为正
    这一部分的代码比较容易看懂,稍微有c语言基础的估计都能看懂,就不过多解读
    ************************************************************************/

    /************************************************************************
    5.函数:void ANO_LTracking_Ctrl(u8 *dT_ms,u8 en)
    名称:巡线执行任务
    功能:将前面滤波得到后的数据进行PD处理,内容也不多

        ano_opmv_lt_ctrl.exp_velocity_h_cmps[1]   
    	= LT_KP *ano_opmv_lt_ctrl.ground_pos_err_h_cm\
    	+ LT_KD *ano_opmv_lt_ctrl.ground_pos_err_d_h_cmps//这个左右距中心位置的偏差  
        ano_opmv_lt_ctrl.exp_yaw_pal_dps = -opmv.lt.angle *6;
        ano_opmv_lt_ctrl.exp_yaw_pal_dps = LIMIT(ano_opmv_lt_ctrl.exp_yaw_pal_dps,-100,100);//这个是对YAW角的偏航修正
    

    *en(这里面的修正当然要在这个标志位为1的时候才能起作用,否则都是面谈)


    这里面所有处理过的数据之后都要到程控函数里面去使用,才能实现循迹的功能,Ano_ProgramCtrl_User.c(程控函数)
    //

    展开全文
  • 匿名飞控openmv寻色块解读

    千次阅读 2019-07-26 17:31:23
    /*********************************************************************************************************...源码:匿名TI板飞控源码+openmvH4 说明:限于本人水平有限,并不能写的很详细,还望各位能够补充...

    /*************************************************************************************
    作者:不会写代码的菜鸟
    时间:2019.7.26
    源码:匿名TI板飞控源码+openmvH4
    说明:限于本人水平有限,并不能写的很详细,还望各位能够补充
    *************************************************************************************/


    文件:Drv.Uart.c
    这个文件夹里面写的是串口配置的初始化,大致写法都一样,Openmv接的是串口3,所以这里我们看串口3的驱动
    /*************************************************************************************
    1.函数:void UART3_IRQHandler(void);
    名称:串口3中断函数(匿名TI板openmv接串口3)
    功能:实现串口定时的数据获取
    定时器中断实现对openmv传感器数据的获取,com_data=ROM_UARTCharGet(UART)
    ,OpenMV_Byte_Get(com_data),这两个函数得到openmv的数据
    *************************************************************************************/

    /*************************************************************************************
    2.函数:void Drv_Uart3Init(uint32_t baudrate);
    名称:串口3基本配置
    功能:就是一些基本的串口配置
    *************************************************************************************/

    /*************************************************************************************
    3.函数:Drv_Uart3SendBuf(u8 *data, u8 len);
    名称:串口发送函数
    功能:通过串口进行一些数据的发送
    *************************************************************************************/


    文件:Drv_OpenMv.c
    这个文件夹写的是对从串口获取的数据包进行解析,对Openmv硬件的检测是否正常连接,还有Openmv掉线复位
    /*************************************************************************************
    1.函数:static void OpenMV_Data_Analysis(u8 buf_data,u8 len)
    名称:Openmv的数据解析
    功能:用于对从串口获取的数据进行解析
    if(
    (buf_data+3)==0x41)
    {
    opmv.cb.color_flag = (buf_data+5);
    opmv.cb.sta = (buf_data+6);
    opmv.cb.pos_x = (s16)((
    (buf_data+7)<<8)|
    (buf_data+8));
    opmv.cb.pos_y = (s16)(((buf_data+9)<<8)|(buf_data+10));
    },对于这一部分代码可以先看下Openmv源码的数据打包,0x41是openmv里面数据打包时点检测特有的功能字,如果解析到数据包里面有0x41,说明下面代码要进行的是对于色块追踪而处理的代码,之后里面的都是将数据包里面的参数传出来
    *opmv.cb.color_flag(找寻的颜色)
    *opmv.cb.sta(openmv正常检测到色块后标志位)
    *opmv.cb.sta(openmv正常检测到色块后的标志位)
    *opmv.cb.pos_x (寻到的色块的中心x坐标)
    *opmv.cb.pos_y(寻到的色块的中心y坐标)
    对于这一部分代码结合openmv的数据打包代码更易于理解
    ************************************************************************************/

    /*************************************************************************************
    2.函数:void OpenMV_Offline_Check(u8 dT_ms)
    名称:opnmv掉线检测
    功能:用于检测openmv是否还正常连接到主控板
    *************************************************************************************/

    /*************************************************************************************
    3…函数:static void OpenMV_Check_Reset()
    名称:opnmv掉线检测复位
    功能:当openmv掉线后重新复位openmv,相当于看门狗的作用
    *************************************************************************************/


    文件:Ano_OPMV_CBTracking_Ctrl.c
    这个文件里面写的是当从串口获取到Openmv数据后,并经过数据包解析之后,将解析得到的数据开始进行飞行处理,实现色块追踪的功能
    /*************************************************************************************
    1.函数:static void ANO_CBTracking_Decoupling(u8 *dT_ms,float rol_degs,float pit_degs)
    名称:色块跟踪解耦合
    功能:openmv检测到色块后,利用一些数学换算,以及滤波结合IMU得到的Pitch,rol,解算出平移偏移量
    *opmv.cb.sta(这个标志位是串口解析数据包得到的flag,检测到色块点位1)
    *ano_opmv_cbt_ctrl.opmv_pos[0] = opmv.cb.pos_y;
    *ano_opmv_cbt_ctrl.opmv_pos[1] = -opmv.cb.pos_x(这两句是将串口解析数据包得到的色块中心点的坐标换算到飞机的机体坐标)
    *ref_carrier_velocity[0] = VELOCITY_CMPS_X
    *ref_carrier_velocity[1] = VELOCITY_CMPS_Y(VELOCITY_CMPS_X,VELOCITY_CMPS_Y,这两个值是通过光流的图像处理得到的水平x,y的平移速度)
    *ano_opmv_cbt_ctrl.decou_pos_pixel[0] = decou_pos_pixel_lpf[1][0】
    *ano_opmv_cbt_ctrl.decou_pos_pixel[1] = decou_pos_pixel_lpf[1][1】(这两个值是最后这个函数得出的平移偏移量,这两个值将用到下面一个函数的色块跟踪计算处理)
    ************************************************************************************/

    /*************************************************************************************
    2.函数:static void ANO_CBTracking_Calcu(u8 *dT_ms,s32 relative_height_cm)
    名称: 色块跟踪计算处理
    功能:这个函数主要是利用前面色块跟踪解耦合函数得到的偏差,以及openmv的值将偏差转换到实际的距离上去
    *g_pos_err_old[0](这个数组的0,1分别是x,y轴的,在这里就说x轴,这个是过去偏差,用于后面要用到的微分)
    *ano_opmv_cbt_ctrl.ground_pos_err_h_cm[0](这个是根据实际得到的数值进行解算得到的实际偏差)
    ano_opmv_cbt_ctrl.ground_pos_err_d_h_cmps[0] = (ano_opmv_cbt_ctrl.ground_pos_err_h_cm[
    0]- g_pos_err_old[0])
    (1000/(*dT_ms));(这个是对平移偏差进行微分)
    *ano_opmv_cbt_ctrl.target_gnd_velocity_cmps[0](这个是参考传感器测到的高度计算用于下面对偏差的前馈控制)
    这个函数计算出来的值都是用于下面实际跟踪任务控制的前馈+PD的处理
    ************************************************************************************/

    /*************************************************************************************
    3.函数:void ANO_CBTracking_Ctrl_Task(u8 dT_ms)
    名称:色块跟踪任务控制
    功能:这个函数主要是用上面计算处理得出的结果进行前馈+PD的控制
    *switchs.of_flow_on(这个标志位是光流正常连接的时候置1)
    *switchs.opmv_on(这个标志位是openmv连接正常的时候置1)
    *ano_opmv_cbt_ctrl.exp_velocity_h_cmps[0](只分析了x轴的,y轴的也是同理,这个值是最终的控制后的偏差)
    = CBT_KF *ano_opmv_cbt_ctrl.target_gnd_velocity_cmps[0] (前馈值x前馈系数)
    +CBT_KP *(ano_opmv_cbt_ctrl.ground_pos_err_h_cm[0])(比例系数x偏差)
    +CBT_KD *ano_opmv_cbt_ctrl.ground_pos_err_d_h_cmps[0](微分系数x差值)
    *ano_opmv_cbt_ctrl.exp_velocity_h_cmps[0] = ano_opmv_cbt_ctrl.exp_velocity_h_cmps[1] = 0(如果没有偏差就赋0,保持原位不动)
    *Program_Ctrl_User_Set_HXYcmps(ano_opmv_cbt_ctrl.exp_velocity_h_cmps[0],ano_opmv_cbt_ctrl.exp_velocity_h_cmps[1])(将最终得出的偏差结果放到程控函数里面进行飞控的自主飞行,也就是利用openmv,脱离遥控器进行前后左右等的飞行)
    *************************************************************************************/


    文件:Ano_ProgramCtrl_User.c
    这个文件里面的函数是计算出x,y,z,yaw的平移速度,之后用到FlightCtrl.c文件里面进行飞机的自主飞行
    /*************************************************************************************
    1.函数:void Program_Ctrl_User_Set_HXYcmps(float hx_vel_cmps,float hy_vel_cmps)
    名称:水平坐标系程控速度功能函数
    功能:程控功能,航向水平坐标系下速度设定(实时控制),X速度(厘米每秒,正为前进,负为后退,Y速度(厘米每秒,正为左移,负为右移)
    *************************************************************************************/

    /*************************************************************************************
    2.函数:void Program_Ctrl_User_Set_Zcmps(float z_vel_cmps)
    名称:通用程控速度功能接口函数,速度(厘米每秒,正为上升,负为下降)
    功能:程控功能,上升下降速度设定(实时控制)
    ************************************************************************************/

    /*************************************************************************************
    3.函数:void Program_Ctrl_User_Set_YAWdps(float yaw_pal_dps)
    名称:YAW角程控功能接口函数
    功能:程控功能,航向速度设定(实时控制),速度(度每秒,正为右转,负为左转)
    *************************************************************************************/
    这个文件里面的函数得出的是各个方向的平移速度,之后在FlightCtrl.c文件里面去查看得出的结果的使用

    展开全文
  • 匿名科创--ANO_OPENMV视觉开发板介绍

    千次阅读 2019-06-29 12:25:44
    不可否认,OPENMV为大家提供了一个非常方便、易用的视觉开发环境,但是在实际应用时还会有一些不方便的地方,匿名团队专门设计打造了匿名ANO_OpenMV4,硬件与官方OPENMV4相同,主控采用STM32H743,相比OpenMV3有更快...

    1、ANO_OPENMV介绍

    官方OPENMV体积大?在飞行器上不方便安装?串口通过杜邦线容易接触不良?不可否认,OPENMV为大家提供了一个非常方便、易用的视觉开发环境,但是在实际应用时还会有一些不方便的地方,匿名团队专门设计打造了匿名ANO_OpenMV4,硬件与官方OPENMV4相同,主控采用STM32H743,相比OpenMV3有更快的处理速度,同时兼容多种镜头。

    2、ANO_OPENMV特点

    1. 我们在原版的基础上做了更简洁化的设计,相比于原版能让OpenMV更加配合四旋翼飞行器。镜头可与主板分离,方便mv的固定,更小巧更方便。
    2. 与此同时,克服了其他Mini版OpenMV大多无法兼容SD卡,无法更换其他图像传感器的缺点。预留拓展口,对原版OpemMV做到了全兼容。
    3. 我们为适应电赛巡线,寻点,扫二维码等需求,设计了专用的OV7725与MT9V034镜头,彩色图像便于识别颜色,全局快门镜头能让你拥有更高的图像刷新率,提高控制精度。
    4. 推出巡线识别、颜色识别等任务的例程给用户参考,方便学习、二次开发。

    3、硬件介绍

    1. 单独引出SH1.0接口的UART3,能与飞控直接连接,无需转接线。
    2. 引出串口2,给用户留出了更大的开发空间。
    3. 屏蔽罩保护,MV更简洁,不易损坏。
      在这里插入图片描述

    4、注意事项

    1. 镜头PCB长边正对机头方向为正确的安装方向(USB座朝向机尾)。
    2. OpenMV主板正面左上角为BOOT焊点,正常使用为拨开,如需dfu下载固件,则将焊点连接。dfu固件下载方式见官方手册。
    3. 正面左下角接口为swd接口顺序为从左往右(SWDIO GND SWCLK 3.3V),可用下载器下载bin文件,任何情况都不要将5V连入3.3V口否则会烧毁OpenMV甚至摄像头模块。

    5、灯光指示与问题解决

    1. 板子右上角为电源指示灯上电后常亮,如果不亮为供电异常或电源烧毁。
    2. 上电后绿色闪烁后,白色短亮后显示USB闪存器。如果闪存器没有出现或白灯常亮则为故障,尝试dfu重新下载固件。
    3. 无连接摄像头会弹出USB闪存器,白灯常亮,如有以上情况请检查FPC线摄像头连接。
    4. 在有文件保存的过程中红灯常亮。

    6、程序运行及调试

    1. 程序采用模块化编程,为了方便持续添加各种的识别任务,拿到源码后将所有源码集体复制到USB闪存器中,再次上电即可自动运行。
    2. 如果需要对源码调试,IDE运行时需要点到USB闪存器中main文件运行,修改后必须保存,等待MV红色灯熄灭后方可继续调试或脱机使用。
    3. 每个模块任务的阈值在每个任务中头部定义,具体方法见官方教程。
    4. 使用ANO_MV源码时注意MV摄像头长边方向与机头,USB与机尾部方向一致。
    展开全文
  • 2019年电赛B题OpenMV.zip

    2020-06-16 11:42:52
    2019全国电赛空中巡线,采用openmv视觉,现分享出来,无飞控资料,只是openmv各种巡线源码,可以搭载pix4,匿名,无名,通用,不喜勿喷
  • 匿名飞控的开关状态任务为函数Swtich_State_Task(u8 dT_ms),定义在Ano_FlightCtrl.c中,在任务调度中引用,1ms执行一次。主要负责确定一些传感器是否能用,如果正常就将相应的标志位置1(打开)。包括光流、激光、...
  • 自平衡立方体这个项目让我感受到,从材料到PCB到传感器模块都是可以自己设计的!!!!!...其实之前也在匿名的群里看到比如自己打板做openmv模块等等的。 。 ...

空空如也

空空如也

1 2
收藏数 21
精华内容 8
关键字:

匿名openmv