多旋翼位置控制mc pos control源码简单分析

2017730日 v1.5.5版本，源码TOC\o"1-2"\h\z\u 四、源码简 3、外环控制过程的实 4、外环处理、position_setoint_type处 5、位置控制内 6、期望姿态的转 float32 #NEDfloat32pitch_bodyfloat32yaw_bodyfloat32yaw_sp_move_rate rad/s用户给的偏航的移动速率)#Forquaternion-basedattitudecontrolfloat32[4] Desiredquaternionbool #float32 #boolroll_reset_integral #PID计算中重置积分项，当飞行模式发生变化时boolpitch_reset_integral #Resetpitchintegralpart(navigationlogicchange)boolyaw_reset_integral #Resetyawintegralpart(navigationlogicchange)boolfw_control_yaw booldisable_mc_yaw_control #用于垂直起降控制偏航boolfloat32 PID控制产生期望的姿态，最后进行期望姿态数据的发布，commander文件中，并定义了某种飞行模式下的一组标志位，用于决定位置控制和姿态控制的执行过程。用户可以使用或者QGroundControl地面站来ACROANGLE是偏航角速度，这一点跟主模式相同。油门控制的是上升\/下降速度，与ALTCTL模式中的+悬停高度，则飞机将沿直线返由另一台通过串行线路与MAVLink连接的电脑提供。这些外部的设定值可以MAVROS或者Dronekit这种应用程序接口提供。主要的代码流程首先就是按照C++语言的格式C语言的main函数但是在该处变成了extern"C"EXPORTintmc_pos_control_main(intargc,char 的启动在启动代 rc.mc_app里面有详细的说明然后：startpx4_task_tpx4_task_spawn_cmd(constchar*name,intscheduler,intpriority,intstack_size,px4_main_tentry,char*constargv[])是任务的栈空间大小，第五个是任务的函数，最后一个一般是null。void_local_pos_sub=对于其他的的数据则 的方式检更新param_find函数将自己定义的私有变量和系统参数进行匹配，再用param_get函数将系统参数拷贝到私有参数下供自己使用。mc_att_control也有这两种数据结构。这样两种数据结构：局部变量，但是这些变量部分获取还是从_params_handles拷贝的，进而相当于是对系统中参数并在系统中标记为“used”，查找不到就返回invalid。相当于这里的参数的处理不就而_params这种数据结构是用于本文件使用的，相当于对前面获取的系统参数进行再拷Param_find（）函数的作用（用于将_params_handles数据结构中与系统参数相匹配）回系统参数并在系统中标记为“usedinvalidparam_twindow64unsignedlonglongint“usedinvalid函数体只有一个函数param_find_internal()，表面意思就是内部参数查找。明白了这个函数也就知道函数的作用，下面将着重分析param_find_internal()这个函数的作用。param_find_internal(constchar*name,boolnotification)，boolnotification这个参数_paramsParam_get（）函数的作用（从_params_handles数据结构中获取参数赋刚刚主要介绍位置控制数据的两种来源，subscribe订阅的数据，和参数文件拷贝params说是基于GPS的位置自动控制。采用的是等距方位投影的方法 Projection)先map_projection_reproject()再map_projection_project()。这种方式将位置转换为和高度，然后用位置估计参数来更新和高度，接着转换回位置参考点，属于GPS数z轴速度）Manual标志的变量代表摇杆的数据，将的数据直接赋值给了期望的速度2、将期望的速度进行归一化，缩放到地理坐标系下实际速度大小是，旋转矩阵req_vel_sp(2)=-scale_control(_manual.z-0.5f,0.5f,_params.alt_ctl_dz,将油门信号_manual.z加一控制死区，并限制在【-1，+1】区间内，油门信号在sensor.cppmath::Matrix<3,3> R_yaw_sp.from_euler(0.0f,0.0f,math::Vector<3>req_vel_sp_scaled=R_yaw_sp*为，作为者给出的俯仰和横滚控制信号是基于当前飞机航向的，而速度设定值是基于NEDRC输入的控制信号由当前航向旋转至正北向计算出的速度设定值才会与者的意图一致。if(fabsf(req_vel_sp(0))<_params.hold_xy_dz&&fabsf(req_vel_sp(1))<if(_params.hold_max_xy<FLT_EPSILON||vel_xy_mag< _pos_hold_engaged= _pos_sp(0)= _pos_sp(1)= if(!_pos_hold_engaged) _pos_sp(0)= _pos_sp(1)= _run_pos_control= _vel_sp(0)= _vel_sp(1)= _pos_sp(0)=_pos_sp(1)=_run_pos_control=这个时候外环不需要执行了_run_pos_controlfalse;既然不进行位置控制了，位置的期望值刚刚说的control_manual(dt)主要作用就是从拿数据并转化为期望的速度设定（手动Off_board俗称离线外部控制模式，即飞机所需要的一些数据：位置、速度都是来源于飞控mocapmavlink发送过来的。外部模式是遵循m2m理念来设计的模式用外接或机载电脑来命令pixhawk实现飞行，分析等，都可以使用外部设备（机载电脑或其他智能设备）mavlink指外部模式接受来自CortexSPI，串口，IIC模式，把offboardmand在进入offboard前，须清楚一些基本的操作命令,所有可以使用令都在 iduorbtopic的float32 #Parameter1,asdefinedbyMAVLinkuint32VEHICLE_CMDfloat32param2 #Parameter2,asdefinedbyMAVLinkuint32VEHICLE_CMDenum.float32param3 #Parameter3,asdefinedbyMAVLinkuint32VEHICLE_CMDenum.float32param4 #Parameter4,asdefinedbyMAVLinkuint32VEHICLE_CMDenum.float64param5 #Parameter5,asdefinedbyMAVLinkuint32VEHICLE_CMDenum.float64param6 #Parameter6,asdefinedbyMAVLinkuint32VEHICLE_CMDenum.float32param7 #Parameter7,asdefinedbyMAVLinkuint32VEHICLE_CMDenum.uint32command #CommandID,asdefinedMAVLinkbyuint32VEHICLE_CMDenum.uint32target_system #Systemwhichshouldexecutethecommand ponent#Componentwhichshouldexecutethecommand,0forallcomponentsuint32source_system #Systemsendingthecommand ponent#Componentsendingtheuint8confirmation #0:Firsttransmissionofthiscommand.1-255:Confirmationtransmissions(e.g.forkillcommand)我们需要做的就是，将想要的commandid填入变量uint32command,然后将参数填入param1param2...，将target_system和 ponent负值，并发布这个topic就可以命令px4了,target_system和 ponent的值必须为vehicle_status这个topic里面的值，我们只需要订阅这个topic并进行负值就可以了。 如果我们publish了命令，那么px4在执行完命令以后会相应的发布一个 mand_ack的描述uint8VEHICLE_RESULT_DENIED=2uint8VEHICLE_RESULT_UNSUPPORTED=3uint8VEHICLE_RESULT_FAILED=4uint32ORB_QUEUE_LENGTH=uint16commanduint8resulttopicresult5 { _command.param11.0f;//=_command.param3=0.0f;//自定义子模式为无if( mand_pub!=nullptr){ }else }mand_ack_sfds.fd= {intpret=px4_poll(&fds,1,1000)检 mand_acktopic是否有更if(pret<= mand_ack_sub, mand_ack_stopic}if(_ack.result== {}//commandmavlink_log_critical(&mavlink_log_pub,can'tgointoposctlmode,continue!");}例子2:无人 _command.param1=1.0f;//1.0为0.0为加if mand_pub!=nullptr) }else在}mand中还有很多有用令，可以执行摸索 1offboardoffboard本就是一种应该全部由代码去完成的模式。 进入offboard模式令id是topic的发布offboard_control_mode。为了保证飞行的安全性，px4决定，必须要位置最低每秒2此的频率发布offboard_control_modetopicoffboardonline，这是为了安全考虑，如果机载计算机px4500msoffboardoffboard_control_modetopic，这样才能保证安全性. mand切换到offboard模式了offboard模式以后，我们会发现，无法使用位置命令，速度命令来命令飞机飞行，这是因为offboard_control_mode发布是我们没有进行正确的设置。offboard_control_modetopic结构如下：#Off-boardcontrolmodeboolignore_thrustboolignore_attitudeboolignore_bodyrateboolignore_positionboolignore_velocityboolignore_acceleration_boolignore_alt_holdAutomission模式等。{///////////////复位位置设定值在自动模式下或者我们没有在动作控制模式(MCmode)下/*resetpositionsetpointonAUTOmodeactivationorifwearenotinMCmode*/if(!_mode_auto||!_vehicle_status.is_rotary_wing){if(!_mode_auto)_mode_auto=}_reset_pos_sp=_reset_alt_sp=true;}//Pollpositionsetpointboolupdated;orb_check(_pos_sp_triplet_sub,&updated);if(updated){//Makesurethatthepositionsetpointis_pos_sp_triplet.current.valid=}}boolcurrent_setpoint_valid=false;boolprevious_setpoint_valid=false;math::Vector<3>prev_sp;math::Vector<3>curr_sp;////////如果当前三重位置设定值合法,将当前设定值和高度转换成地坐标系的if(_pos_sp_triplet.current.valid)/*projectsetpointtolocalframe/*这个函数在此cpp里面经常使用，是将将转换成地坐标系xy值*/_pos_sp_triplet.current.lat,_pos_sp_triplet.current.lon,&curr_sp.data[0],&curr_sp.data[1]);curr_sp(2)=-(_pos_sp_triplet.current.alt-if(PX4_ISFINITE(curr_sp(0))&&PX4_ISFINITE(curr_sp(1))&&PX4_ISFINITE(curr_sp(2))){current_setpoint_valid=}}if(_pos_sp_triplet.previous.valid){_pos_sp_triplet.previous.lat,_pos_sp_triplet.previous.lon,&prev_sp.data[0],&prev_sp.data[1]);prev_sp(2)=-(_pos_sp_triplet.previous.alt-_ref_alt);if(PX4_ISFINITE(prev_sp(0))&&PX4_ISFINITE(prev_sp(1))&&PX4_ISFINITE(prev_sp(2))){previous_setpoint_valid=}}if(current_setpoint_valid){ /*scaledspace:1==positionerrorresultingmaxallowedspeedmath::Vector<3>scale=_params.pos_p.edivide(_params.vel_max);//TODOaddmultparamhere/*用_params.pos_p的每一个元素除以_params.vel_max对应位置的每一个元素，结scalefor(unsignedinti=0;i<N;res.data[i]=data[i]/return}/*convertcurrentsetpointtoscaledspace*/math::Vector<3>curr_sp_s=curr_sp.emult(scale); 用curr_sp的每一个元素和scaleMatrix<Type,M,N>emult(constMatrix<Type,M,N>&other){Matrix<Type,M,N>constMatrix<Type,M,N>&self=for(size_ti=0;i<M;i++)for(size_tj=0;j<N;j++)res(i,j)=self(i,j)*other(i,}}return} /*bydefaultusecurrentsetpointasis*/math::Vector<3>pos_sp_s=curr_sp_s;if(_pos_sp_triplet.current.type==position_setpoint_s::SETPOINT_TYPE_POSITION&&previous_setpoint_valid){/*follow"previous-current"line-if((curr_sp-prev_sp).length()>MIN_DIST)/*findX-crosspointofunitsphereandtrajectorymath::Vector<3>pos_s_pos.emult(scale);//copy的_local_pos*比例math::Vector<3>prev_sp_s=prev_sp.emult(scale);//_s的都是乘以比例的scalemath::Vector<3>prev_curr_s=curr_sp_s-prev_sp_s;math::Vector<3>curr_pos_s=pos_s-curr_sp_s;floatcurr_pos_s_len=curr_pos_s.length();if(curr_pos_s_len<1.0f)if(_pos_sp_triplet.next.valid){math::Vector<3>next_sp;_pos_sp_triplet.next.lat,_pos_sp_triplet.next.lon,&next_sp.data[0],&next_sp.data[1]);next_sp(2)=-(_pos_sp_triplet.next.alt-if((next_sp-curr_sp).length()>MIN_DIST){math::Vector<3>next_sp_s=next_sp.emult(scale);/*calculateangleprev-curr-next*/math::Vector<3>curr_next_s=next_sp_s-curr_sp_s;returnsthenormalizedversionofthis*return*this/}/*cos(a)*curr_next,a=anglebetweencurrentandnexttrajectorysegments/*prev_curr_s_norm是单位向量(当前位置设定-前一次位置设定)，curr_next_s/*ab=丨a丨丨bcosα，floatcos_a_curr_next=prev_curr_s_norm*/*cos(b),b=anglepos-curr_sp-prev_spprev_curr_s_norm是前一次位置设定指向当前位置设定的单位向量(当*所以cos_bposcurr_spprev_spfloatcos_b=-curr_pos_s*prev_curr_s_norm/ifcos_a_curr_next0.0f&&cos_b0.0fa、bfloatcurr_next_s_len=/*ifcurr-nextdistanceislargerthanunitradius,limititif(curr_next_s_len>1.0f){}/*feedforwardpositionsetpointoffsetmath::Vector<3>pos_ff=prev_curr_s_norm*cos_a_curr_next*cos_b*cos_b*(1.0f-curr_pos_s_len)*(1.0f-expf(-curr_pos_s_len*curr_pos_s_len*20.0f));pos_sp_s+=pos_ff;}}}}else{boolnear=cross_sphere_line(pos_s,1.0f,prev_sp_s,curr_sp_s,pos_sp_s);if(near){/*unitspherecrossestrajectory*/}else/*copteristoofarfromtrajectory/*ifcopterisbehindprevwaypoint,godirectlytoprevwaypoint pos_spprev_spcurr_spif((pos_sp_s-prev_sp_s)*prev_curr_s<0.0f){pos_sp_s=prev_sp_s;}/*ifcopterisinfrontofcurrwaypoint,godirectlytocurrwaypoint/*如果飞行器一个设定位置前面，则到当前设定位置*/if((pos_sp_s-curr_sp_s)*prev_curr_s>0.0f){pos_sp_s=}pos_sp_s=pos_s+(pos_sp_s-}}}}/*先根据任务设定前一个、当前、下一个位置标定prev_p_s、crr_sps、ex_sp_)，用pos_s/*movesetpointnotfasterthanmaxallowedspeed*/math::Vector<3>pos_sp_old_s=_pos_sp.emult(scale);/*differencebetweencurrentanddesiredpositionsetpoints,1=maxspeed*/math::Vector<3>d_pos_m=(pos_sp_s-pos_sp_old_s).edivide(_params.pos_p);floatd_pos_m_len=d_pos_m.length();if(d_pos_m_len>dt)pos_sp_s=pos_sp_old_s+(d_pos_m/d_pos_m_len*}/*scaleresultbacktonormalspace_pos_sp=/*updateyawsetpointifneeded/*yaw_att_sp.yaw_body=}ifwe'realreadynearthecurrenttakeoffsetpointdon'tresetincaseweswitchbacktothismakesthetakeofffinishif((_pos_sp_triplet.current.type==&&_pos_sp_triplet.current.acceptance_radius>0.0f/*needtodetectwe'recloseabitbeforethenavigatorswitchesfromtakeofftonextwaypoint*/&&(_pos-_pos_sp).length()<_pos_sp_triplet.current.acceptance_radius*1.2f){_reset_pos_sp=_reset_alt_sp=/*otherwise:incaseofinterruptedmissiondon'tgotowaypointbutstayatcurrentposition/*}else_reset_pos_sp=_reset_alt_sp=}}else/*nowaypoint,donothing,setpointwasalreadyreset}}以上计算都是基于map_projection_project(&_ref_pos,sp.latsp.lon,&sp.data[0&sp.data[1]);control_auto函数中，引入“scale”Scale=_params.pos_pscalescaledspace。这样处理的目的是将所positionerror1的话，control_auto2scale①在cross_sphere_line函数中使用，具体可以参考cross_sphere_line函数说明；②在此处(上图)position_setpointsp_move_rate不大于“巡航速度”。实际上就是(pos_sp_s–pos_sp_old_s)/dt=sp_move_rate_s;sp_move_rate_s/_params.pos_p=position_error<1;previous.validfalse&¤t.validtrueRTLDESCENSTATE12维。=2curr_pos_s_len1过去目标点→当前目标点→当前位置之间的夹角为b过去目标点→当前目标点与当前目标点→未来目标点两段向量之间的夹角为a当a与b均为锐角的时候，需要修正pos_sp_s，来避免飞机到达航点前不必要的。修正方式为在prev_spcurr_sp（上图红色虚线pos_sp_s，math::Vector<3>pos_ff=prev_curr_s_norm*cos_a_curr_next*cos_b*cos_b*(1.0f-curr_pos_s_len)*(1.0f-expf(-curr_pos_s_len*curr_pos_s_len*20.0f));pos_sp_s+=11，若该长0~1pos_sp_s1，保持当前速度。A的角度越小，cosAB的角度越小，cosB11010x+x<0.4后迅速下降，之前则约等于10.4个单位长度之前，保持当前速度，在距离小于0.4个单位长度之后，开始，距离越近，越多。总结：当A,B均为锐角的时候，需要修正pos_sp_s来防止飞机，修正量的大小取决于Acurr_pos_s_len1时更新。因此，当需要转弯的时候，飞机会。3、curr_pos_s_len1,当前位置与当前目标点的距离（紫色线段）1个单位长度。球与连线有交点，那么取离curr_sp更近的交点作为期望位置。curr_pos_s_len1prev_spprev_sp的连线与球的交点，作为新pos_spcurr_sp后。那么将当前位置与当前目标的连线与球的交点作为的pos_sp_sprev_sp与curr_sp之间，从球心向连线做垂线，垂线与球该函数在control_auto中调用，其作用为使飞机沿“previous_setpoint——current_setpointtrajectory”postion与trajectory的垂直距离cd_len,来实时的改变pos_sp。但值得注意的是r=1control_auto中引入了“scale”的概念，半径为1的目的实际上就是相当于使期望速度达到“巡航速度”。1）、_pos_sp_triplet.current.typeposition_setpoint_s::SETPOINT_TYPE_IDLE什么意思消息_pos_sp_triplet里面包含：px4/position_setpointpreviouspx4/position_setpointcurrentpx4/position_setpointnextuint8SETPOINT_TYPE_POSITION=0 #positionsetpointuint8SETPOINT_TYPE_VELOCITY=1 #velocitysetpointuint8SETPOINT_TYPE_LOITER=2 #loitersetpointuint8SETPOINT_TYPE_TAKEOFF=3 #takeoffsetpointuint8SETPOINT_TYPE_LAND=4#landsetpoint,altitudemustbeignored,descenduntillandinguint8SETPOINT_TYPE_IDLE=5#donothing,switchoffmotorsorkeepatidlespeed(MC)uint8SETPOINT_TYPE_OFFBOARD=6#setpointinNEDframe(x,y,z,vx,vy,vz)setbyuint8SETPOINT_TYPE_FOLLOW_TARGET=7 #setpointinNEDframe(x,y,z,vx,vy,vz)setbyfollowtargetuint8 #donothing,switchoffmotorsorkeepatidlespeed上面是一种不正常的航点类型，下面还有一种也是不正常的，landed飞机已经降落在地上，=takeoff的起飞过程。位置控制的过程其实就是计算推力设定值的过程，由于需要主要的是水平面的处理和z轴的处理不一样，z轴在推力需要一个最小的配平量，以抵加速阶段：加速阶段，阶梯型提高推力，加速阶段，一直加速到加速阶段完成_takeoff_jumpedthrust