課件7位置控制3mc pos control源碼簡單分析

Better備注：源碼對應(yīng)px4v1.5.5版本，源碼/PX4/Firmware/blob/v1.5.5/src/modules/mcposcontrol/mcposcontrol 四、源碼簡 3、外環(huán)控制過程的實 4、外環(huán)處理、position_setoint_type處 5、位置控制內(nèi) 6、期望姿態(tài)的轉(zhuǎn) ,,float32 float32yaw_bodyfloat32yaw_sp_move_rate #Forquaternion-basedattitudecontrol bool #float32 #boolroll_reset_integral 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文件中，并定義了某種飛行模式下的一組標志位，用于決定位置控制和姿態(tài)控制的執(zhí)行過程。用戶可以使用或者QGroundControl地面站來+懸停高度，則飛機將沿直線返由另一臺通過串行線路與MAVLink連接的電腦提供。這些外部的設(shè)定值可以由MAVROS或者Dronekit這種應(yīng)用程序接口提供。Better| extern EXPORTintmc_pos_control_main(intargc,charMc_pos_control的啟動在啟動代 里面有詳細的說明POSIX標準的任務(wù)創(chuàng)建函數(shù)：px4_task_spawn_cmd()，注意它的用法。px4_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ù)則用 的方式檢更新參數(shù)進行匹配，再用param_get函數(shù)將系統(tǒng)參數(shù)拷貝到私有參數(shù)下供自己使用。這樣兩種數(shù)據(jù)結(jié)構(gòu)：局部變量，但是這些變量部分獲取還是從_params_handles拷貝的，進而相當(dāng)于是對系統(tǒng)中參數(shù)并在系統(tǒng)中標記為“used”，查找不到就返回invalid。相當(dāng)于這里的參數(shù)的處理不就貝，用于本文件的使用，估計最后會不會再寫入系統(tǒng)中。Param_find（）函數(shù)的作用（用于將_params_handles數(shù)據(jù)結(jié)構(gòu)中與系統(tǒng)參數(shù)相匹配）回系統(tǒng)參數(shù)并在系統(tǒng)中標記為“used”，查找不到就返回invalidMulticopterPositionControl::MulticopterPositionControl(){}也就知道函數(shù)的作用，下面將著重分析param_find_internal()這個函數(shù)的作用。param_find_internal(constchar*name,boolnotification)，boolnotification這個參數(shù)_paramsParam_get（）函數(shù)的作用（從_params_handles數(shù)據(jù)結(jié)構(gòu)中獲取參數(shù)賦值給_params下的參數(shù)，間接地也匹配了系統(tǒng)參數(shù)，這個數(shù)據(jù)結(jié)構(gòu)只是適用于本文，或許處剛剛主要介紹位置控制數(shù)據(jù)的兩種來源，subscribe訂閱的數(shù)據(jù)，和參數(shù)文件拷貝說是基于GPS的位置自動控制。(AzimuthalEquidistantProjection)先map_projection_reproject()再map_projection_project()。這種方式將位置轉(zhuǎn)換為和率”指z軸速度）Manual標志的變量代表搖桿的數(shù)據(jù)，將的數(shù)據(jù)直接賦值給了期望的速度2req_vel_sp(2)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,math::Matrix<3,3>為，作為者給出的俯仰和橫滾控制信號是基于當(dāng)前飛機航向的，而速度設(shè)定值是基于NEDRC輸入的控制信號由當(dāng)前航向旋轉(zhuǎn)至正北向計算出的速度_pos_sp(0)=_pos_sp(1)=_pos_sp(0)=_pos_sp(1)=_vel_sp(0)=_vel_sp(1)=}_pos_sp(0)=_pos_sp(1)=這個時候外環(huán)不需要執(zhí)行了_run_pos_controlfalse;既然不進行位置控制了，位置的期望值Off_board俗稱離線外部控制模式，即飛機所需要的一些數(shù)據(jù)：位置、速度都是來源于飛控外部，如可以來源于mocap或者其他外載計算機通過mavlink發(fā)送過來的。CortexSPI，串口，IIC模式，把多mand在進入offboard前，須清楚一些基本的操作命令,所有可以使用令都 mandmsgfloat32 float32param2 #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ā)布一個 uint8VEHICLE_RESULT_DENIED=2uint8VEHICLE_RESULT_FAILED=4uint8resulttopicresult5個取值，非別 { if( mand_pub!=nullptr){ }else }mand_ack_sfdsfd= { if(pret<= }if(_ackresult== {}//如果執(zhí)行失敗，循環(huán)回去繼續(xù)發(fā)送commandmavlink_log_critical(&mavlink_log_pub,can'tgointoposctlmode,continue!");} _command.param1=1.0f;//1.0為0.0為加if mand_pub!={ }else} 1offboard本就是一種應(yīng)該全部由代碼去完成的模式。 我們需要先進行另外一個topic的發(fā)布offboard_control_mode。為了保證飛行的安全性，px4決定，必須要位置最低每秒2此的頻率發(fā)布offboard_control_modetopicoffboardonline，這是為了安全考慮，如果機載計算機px4500msoffboardoffboard_control_modetopic，這樣才能保證安全性. 這是因為offboard_control_mode發(fā)布是我們沒有進行正確的設(shè)置。offboard_control_modetopic結(jié)構(gòu)如下：boolignore_thrustboolignore_attitudeboolignore_positionboolboolignore_acceleration_boolignore_alt_hold{///////////////復(fù)位位置設(shè)定值在自動模式下或者我們沒有在動作控制模式(MCmode)下/*resetpositionsetpointonAUTOmodeactivationorifwearenotinMCmode*/if(!_mode_auto||!_vehicle_status.is_rotary_wing){_mode_auto=}_reset_pos_sp=}boolupdated;if(updated){//Makesurethatthepositionsetpointis}}boolcurrent_setpoint_valid=false;math::Vector<3>prev_sp;math::Vector<3>curr_sp;if(_pos_sp_triplet.current.valid){/*projectsetpointtolocalframe&curr_sp.data[0],&curr_sp.data[1]);if(PX4_ISFINITE(curr_sp(0))&&PX4_ISFINITE(curr_sp(2))){current_setpoint_valid=}}if(_pos_sp_triplet.previous.valid){&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(2))){}}if(current_setpoint_valid){ /*scaledspace:1==positionerrorresultingmaxallowedspeedmath::Vector<3>scale=_params.pos_p.edivide(_params.vel_max);//TODOaddmultparamhere果賦值給scalefor(unsignedinti=0;i<N;res.data[i]=data[i]/return}/*convertcurrentsetpointtoscaledspace*/ curr_sp的每一個元素和scale{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==/*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;&next_sp.data[0],&next_sp.data[1]);next_sp(2)=-(_pos_sp_tripletnext.alt-if((next_sp-curr_sp).length()>{math::Vector<3>next_sp_s=/*calculateangleprev-curr-next*/math::Vector<3>curr_next_s=next_sp_s-curr_sp_s;returnsthenormalizedversionofthis*return*this/}/*cos(a)*curr_next,a=anglebetweencurrentandnexttrajectorysegments/*ab=abcosα，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.0f)a、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*pos_sp_s+=}}}}else{boolnear=cross_sphere_line(pos_s,1.0f,prev_sp_s,curr_sp_s,pos_sp_s);if(near){}else/*copteristoofarfromtrajectory/*ifcopterisbehindprevwaypoint,godirectlytoprevwaypoint pos_spprev_spcurr_spif((pos_sp_s-prev_sp_s)*prev_curr_s<{pos_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-}}}}/*/*movesetpointnotfasterthanmaxallowedspeed*//*differencebetweencurrentanddesiredpositionsetpoints,1=maxspeed*/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'tresetincaseweswitchbacktothismakesthetakeofffinish&&_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]);函數(shù)的計算(將轉(zhuǎn)換成地坐標系xy值)也就是說是基于GPS的位置自動控control_auto函數(shù)中，引入“scale”scalescaledspace。這樣處理的目的是將所positionerror1的話，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.validtrueRTL=2curr_pos_s_len1，當(dāng)前位置與當(dāng)前目標的距離小于一個單位長度過去目標點→當(dāng)前目標點→當(dāng)前位置之間的夾角為b。過去目標點→→a當(dāng)a與b均為銳角的時候，需要修正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+=則這一項為1，保持當(dāng)前速度。B的角度越小，cosB在距離小于0.4個單位長度之后，開始，距離越近，越多。總結(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)前目標點的距離（紫色線段）1個單位長度。球與連線有交點，那么取離curr_sp更近的交點作為期望位置。curr_sp后。那么將當(dāng)前位置與當(dāng)前目標的連線與球的交點作為的pos_sp_s該函數(shù)在control_auto中調(diào)用，其作用為使飛機沿“previous_setpoint——current_setpointtrajectory”postion與trajectory的垂直距離cd_len,來實時的改變pos_sp。如下圖所示：r=1control_auto中引入了“scale”的概念，半徑為1的目的實際上就是相當(dāng)于使期望速度達到“巡航速度”。====px4/position_setpointpreviouspx4/position_setpointcurrentpx4/position_setpointnextuint8SETPOINT_TYPE_POSITION=0 uint8SETPOINT_TYPE_VELOCITY=1 uint8SETPOINT_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