多旋翼位置控制mc pos control源碼簡單分析

2017730日 v1.5.5版本，源碼TOC\o"1-2"\h\z\u 四、源碼簡 3、外環(huán)控制過程的實 4、外環(huán)處理、position_setoint_type處 5、位置控制內(nèi) 6、期望姿態(tài)的轉(zhuǎn) float32 #NEDfloat32pitch_bodyfloat32yaw_bodyfloat32yaw_sp_move_rate rad/s用戶給的偏航的移動速率)#Forquaternion-basedattitudecontrolfloat32[4] Desiredquaternionbool #float32 #boolroll_reset_integral #PID計算中重置積分項，當(dāng)飛行模式發(fā)生變化時boolpitch_reset_integral #Resetpitchintegralpart(navigationlogicchange)boolyaw_reset_integral #Resetyawintegralpart(navigationlogicchange)boolfw_control_yaw booldisable_mc_yaw_control #用于垂直起降控制偏航boolfloat32 PID控制產(chǎn)生期望的姿態(tài)，最后進行期望姿態(tài)數(shù)據(jù)的發(fā)布，commander文件中，并定義了某種飛行模式下的一組標(biāo)志位，用于決定位置控制和姿態(tài)控制的執(zhí)行過程。用戶可以使用或者QGroundControl地面站來ACROANGLE是偏航角速度，這一點跟主模式相同。油門控制的是上升\/下降速度，與ALTCTL模式中的+懸停高度，則飛機將沿直線返由另一臺通過串行線路與MAVLink連接的電腦提供。這些外部的設(shè)定值可以MAVROS或者Dronekit這種應(yīng)用程序接口提供。主要的代碼流程首先就是按照C++語言的格式C語言的main函數(shù)但是在該處變成了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[])是任務(wù)的?？臻g大小，第五個是任務(wù)的函數(shù)，最后一個一般是null。void_local_pos_sub=對于其他的的數(shù)據(jù)則 的方式檢更新param_find函數(shù)將自己定義的私有變量和系統(tǒng)參數(shù)進行匹配，再用param_get函數(shù)將系統(tǒng)參數(shù)拷貝到私有參數(shù)下供自己使用。mc_att_control也有這兩種數(shù)據(jù)結(jié)構(gòu)。這樣兩種數(shù)據(jù)結(jié)構(gòu)：局部變量，但是這些變量部分獲取還是從_params_handles拷貝的，進而相當(dāng)于是對系統(tǒng)中參數(shù)并在系統(tǒng)中標(biāo)記為“used”，查找不到就返回invalid。相當(dāng)于這里的參數(shù)的處理不就而_params這種數(shù)據(jù)結(jié)構(gòu)是用于本文件使用的，相當(dāng)于對前面獲取的系統(tǒng)參數(shù)進行再拷Param_find（）函數(shù)的作用（用于將_params_handles數(shù)據(jù)結(jié)構(gòu)中與系統(tǒng)參數(shù)相匹配）回系統(tǒng)參數(shù)并在系統(tǒng)中標(biāo)記為“usedinvalidparam_twindow64unsignedlonglongint“usedinvalid函數(shù)體只有一個函數(shù)param_find_internal()，表面意思就是內(nèi)部參數(shù)查找。明白了這個函數(shù)也就知道函數(shù)的作用，下面將著重分析param_find_internal()這個函數(shù)的作用。param_find_internal(constchar*name,boolnotification)，boolnotification這個參數(shù)_paramsParam_get（）函數(shù)的作用（從_params_handles數(shù)據(jù)結(jié)構(gòu)中獲取參數(shù)賦剛剛主要介紹位置控制數(shù)據(jù)的兩種來源，subscribe訂閱的數(shù)據(jù)，和參數(shù)文件拷貝params說是基于GPS的位置自動控制。采用的是等距方位投影的方法 Projection)先map_projection_reproject()再map_projection_project()。這種方式將位置轉(zhuǎn)換為和高度，然后用位置估計參數(shù)來更新和高度，接著轉(zhuǎn)換回位置參考點，屬于GPS數(shù)z軸速度）Manual標(biāo)志的變量代表搖桿的數(shù)據(jù)，將的數(shù)據(jù)直接賦值給了期望的速度2、將期望的速度進行歸一化，縮放到地理坐標(biāo)系下實際速度大小是，旋轉(zhuǎn)矩陣req_vel_sp(2)=-scale_control(_manual.z-0.5f,0.5f,_params.alt_ctl_dz,將油門信號_manual.z加一控制死區(qū)，并限制在【-1，+1】區(qū)間內(nèi)，油門信號在sensor.cppmath::Matrix<3,3> R_yaw_sp.from_euler(0.0f,0.0f,math::Vector<3>req_vel_sp_scaled=R_yaw_sp*為，作為者給出的俯仰和橫滾控制信號是基于當(dāng)前飛機航向的，而速度設(shè)定值是基于NEDRC輸入的控制信號由當(dāng)前航向旋轉(zhuǎn)至正北向計算出的速度設(shè)定值才會與者的意圖一致。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=這個時候外環(huán)不需要執(zhí)行了_run_pos_controlfalse;既然不進行位置控制了，位置的期望值剛剛說的control_manual(dt)主要作用就是從拿數(shù)據(jù)并轉(zhuǎn)化為期望的速度設(shè)定（手動Off_board俗稱離線外部控制模式，即飛機所需要的一些數(shù)據(jù)：位置、速度都是來源于飛控mocapmavlink發(fā)送過來的。外部模式是遵循m2m理念來設(shè)計的模式用外接或機載電腦來命令pixhawk實現(xiàn)飛行，分析等，都可以使用外部設(shè)備（機載電腦或其他智能設(shè)備）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,然后將參數(shù)填入param1param2...，將target_system和 ponent負值，并發(fā)布這個topic就可以命令px4了,target_system和 ponent的值必須為vehicle_status這個topic里面的值，我們只需要訂閱這個topic并進行負值就可以了。 如果我們publish了命令，那么px4在執(zhí)行完命令以后會相應(yīng)的發(fā)布一個 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中還有很多有用令，可以執(zhí)行摸索 1offboardoffboard本就是一種應(yīng)該全部由代碼去完成的模式。 進入offboard模式令id是topic的發(fā)布o(jì)ffboard_control_mode。為了保證飛行的安全性，px4決定，必須要位置最低每秒2此的頻率發(fā)布o(jì)ffboard_control_modetopicoffboardonline，這是為了安全考慮，如果機載計算機px4500msoffboardoffboard_control_modetopic，這樣才能保證安全性. mand切換到offboard模式了offboard模式以后，我們會發(fā)現(xiàn)，無法使用位置命令，速度命令來命令飛機飛行，這是因為offboard_control_mode發(fā)布是我們沒有進行正確的設(shè)置。offboard_control_modetopic結(jié)構(gòu)如下：#Off-boardcontrolmodeboolignore_thrustboolignore_attitudeboolignore_bodyrateboolignore_positionboolignore_velocityboolignore_acceleration_boolignore_alt_holdAutomission模式等。{///////////////復(fù)位位置設(shè)定值在自動模式下或者我們沒有在動作控制模式(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;////////如果當(dāng)前三重位置設(shè)定值合法,將當(dāng)前設(shè)定值和高度轉(zhuǎn)換成地坐標(biāo)系的if(_pos_sp_triplet.current.valid)/*projectsetpointtolocalframe/*這個函數(shù)在此cpp里面經(jīng)常使用，是將將轉(zhuǎn)換成地坐標(biāo)系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對應(yīng)位置的每一個元素，結(jié)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是單位向量(當(dāng)前位置設(shè)定-前一次位置設(shè)定)，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是前一次位置設(shè)定指向當(dāng)前位置設(shè)定的單位向量(當(dāng)*所以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/*如果飛行器一個設(shè)定位置前面，則到當(dāng)前設(shè)定位置*/if((pos_sp_s-curr_sp_s)*prev_curr_s>0.0f){pos_sp_s=}pos_sp_s=pos_s+(pos_sp_s-}}}}/*先根據(jù)任務(wù)設(shè)定前一個、當(dāng)前、下一個位置標(biāo)定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函數(shù)中，引入“scale”Scale=_params.pos_pscalescaledspace。這樣處理的目的是將所positionerror1的話，control_auto2scale①在cross_sphere_line函數(shù)中使用，具體可以參考cross_sphere_line函數(shù)說明；②在此處(上圖)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過去目標(biāo)點→當(dāng)前目標(biāo)點→當(dāng)前位置之間的夾角為b過去目標(biāo)點→當(dāng)前目標(biāo)點與當(dāng)前目標(biāo)點→未來目標(biāo)點兩段向量之間的夾角為a當(dāng)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，保持當(dāng)前速度。A的角度越小，cosAB的角度越小，cosB11010x+x<0.4后迅速下降，之前則約等于10.4個單位長度之前，保持當(dāng)前速度，在距離小于0.4個單位長度之后，開始，距離越近，越多?？偨Y(jié)：當(dāng)A,B均為銳角的時候，需要修正pos_sp_s來防止飛機，修正量的大小取決于Acurr_pos_s_len1時更新。因此，當(dāng)需要轉(zhuǎn)彎的時候，飛機會。3、curr_pos_s_len1,當(dāng)前位置與當(dāng)前目標(biāo)點的距離（紫色線段）1個單位長度。球與連線有交點，那么取離curr_sp更近的交點作為期望位置。curr_pos_s_len1prev_spprev_sp的連線與球的交點，作為新pos_spcurr_sp后。那么將當(dāng)前位置與當(dāng)前目標(biāo)的連線與球的交點作為的pos_sp_sprev_sp與curr_sp之間，從球心向連線做垂線，垂線與球該函數(shù)在control_auto中調(diào)用，其作用為使飛機沿“previous_setpoint——current_setpointtrajectory”postion與trajectory的垂直距離cd_len,來實時的改變pos_sp。但值得注意的是r=1control_auto中引入了“scale”的概念，半徑為1的目的實際上就是相當(dāng)于使期望速度達到“巡航速度”。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飛機已經(jīng)降落在地上，=takeoff的起飛過程。位置控制的過程其實就是計算推力設(shè)定值的過程，由于需要主要的是水平面的處理和z軸的處理不一樣，z軸在推力需要一個最小的配平量，以抵加速階段：加速階段，階梯型提高推力，加速階段，一直加速到加速階段完成_takeoff_jumpedthrust