目标航向为当前航向trg_heading
true
speedHoldEnable
左右推进器的基准值加上mp__bais=s_bais=1500+m
false
headingHoldEnbale
最终计算得左右推进器的值右推进器 s=s_bais+s_yaw+s_thro+s_heading左推进器 p=p_bais-p_yaw+p_thro-p_heading
trg_heading+=4n=trg_heading
s_bais=1500p_basis=1500
headingHoldEnbale=true
n=0
s_bais+=y*thro_gainp_basis+=y*thro_gain
y=0
heading_Hold_Enable
右推进器 s_yaw=x*heading_gain左推进器 p_yaw=x*heding_gain
headingHoldEnbale=false
航速保持开关命令speed_hold_toggle(m)
speedHoldEnbale=true
目标航向为n值trg_heading=n
计算当前的航向差heading_error=trg_heading-heading
航向保持开关命令heading_hold_toggle(n)
左右推进器控制流程
x=0
headlon(n)
拐弯命令yaw(x)
speedlon(m)
headloff()
前进命令thro(y)
右推进器 s_thro=y*thro_gain左推进器 p_thro=y*thro_gain
speedHoldEnbale
n=trg_heading
speedHoldEnbale=false左右推进器的基准值设为1500p_bais=1500s_bais=1500
左右推进器基准值取当前左右推进器的平均值
右推进器s_heading=0左推进器p_heading=0
右推进器s_heading=heading_error*heading_hold_loop_gain左推进器p_heading=heading_error*heading_hold_loop_gain
m=0
接收命令
speedloff()
