def tiebian():
global sidePoints
sidePoints = np.array(sidePoints)
yawAim = getYawAim(sidePoints)
log.info('已加载导航点，共有%d个点' % len(sidePoints))
index = 0
pos = Point(g.getValue('ship').lat, g.getValue('ship').lng)
# 判断刚开始时，是否需要原地转
state = State.TURN if utils.getDistance(
pos, Point(sidePoints[0, 0], sidePoints[0, 1])) > 3 else State.GO
try:
while index < len(sidePoints):
yaw = g.getValue('ship').yaw
lat = g.getValue('ship').lat
lng = g.getValue('ship').lng
gz = g.getValue('ship').gz
speed = g.getValue('ship').speed
pShip = Point(lat, lng)
pAim = Point(sidePoints[index, 0], sidePoints[index, 1])
isFirst = True if index == 0 else False
if state == State.TURN:
thrust = 0
dire = PID_Turn(pShip, pAim, yaw, -gz, yawAim[index], isFirst)
if dire == 0:
# 转弯完毕，进入直行阶段
state = State.GO
elif state == State.GO:
# 判断直线段或是曲线段，之后五个点期望角度最大误差
slowSpeed = True if angleArray_ptp(
yawAim[index:index + 5]) > 15 or index == 0 else False
log.info('Go:当前期望方向角 %.1f 度，slowSpeed是 %s' % yawAim[index],
slowSpeed)
dire, thrust = PID_Go(pShip, pAim, yaw, -gz, yawAim[index],
slowSpeed, isFirst)
dist = utils.getDistance(pShip, pAim)
if state == State.GO and index > 0:
# 将最近点序号作为当前序号
index = index_cal(pShip, index,
SearchLength=10) #函数中使用了global量sidePoints
log.info('当前处于第 %d 个点' % index)
if dist < 1 and index == 0:
state = State.TURN
index += 1
if index == len(sidePoints):
# 跑完
thrust = 0
dire = 0
data = struct.pack("hhb", int(thrust), int(dire), 0)
g.getValue("can").send(0x544, data)
log.info("%s\t%d\t%.1f\t%.1f\t%d" %
(state, index, thrust, dire, dist))
time.sleep(1)
except Exception:
log.error('贴边Error', exc_info=True)
log.info('导航结束')
def serial_recv_thread():
global gps_speed
ser = serial.Serial("/dev/ttyAMA0", 115200)
gz = 0
flag_ang_control = 0
flag_finish = False
flag_back = False
while True:
if True or g.getValue('mode') != 1 and g.getValue('mode') != 0:
try:
msg = ser.readline().strip()
if msg.startswith('$') and msg.endswith('#'):
datas = msg.replace('$', '').replace('#', '').split(';')
if len(datas) >= 10:
yaw = float(datas[3])
gps_speed = float(datas[7])
if flag_finish: # 到达倒库点
angle_error = 279 - yaw
if angle_error > 180:
angle_error -= 360
elif angle_error <= -180:
angle_error += 360
if flag_back: # 在倒库点修完角，开始后退
# 边后退边修角，修角PID采用PID_Go中的
PID_P = 0.5
if angle_error * (-gz) > 0:
PID_D = -0.8
else:
PID_D = 0.8
dire = PID_P * angle_error + (-gz) * PID_D
dire = saturate(dire, -100, 100)
#考虑是否使用PID_Go然后thrust加负号
ser.write(
utils.parityGenerate("$E;-15,%d#" % dire))
dist = utils.getDistance(
Point(lat_rev1, lng_rev1),
Point(g.getValue('lat'),
g.getValue('lng'))) # 离最终点距离
print(dist, angle_error)
if dist < 2: # 离最终点距离小于2m，停止
time.sleep(1)
ser.write(utils.parityGenerate("$E;0,0#"))
break
continue
if abs(angle_error) < 10:
ser.write(utils.parityGenerate("$E;-15,0#"))
flag_back = True
dire = PID_Turn(angle_error, yaw_now,
-gz) # 在倒库点修角
# dire = angle_control_4(angle_error, -gz)
print("角度差、方向油门：%d\t%d" % (angle_error, dire))
ser.write(
utils.parityGenerate("$E;%d,%d#" % (0, dire)))
continue
(thrust, dire,
flag_ang_control) = control_reverse_point(
g.getValue('lat'), g.getValue('lng'), yaw,
gps_speed, -gz, lat_rev, lng_rev,
flag_ang_control)
# print(g.getValue('lat'), g.getValue(
# 'lng'), g.getValue('mode'), yaw, gps_speed, thrust, dire)
ser.write(
utils.parityGenerate("$E;%d,%d#" % (thrust, dire)))
if thrust == 0 and dire == 0:
flag_finish = True
elif msg.startswith('&') and msg.endswith('#'):
datas = msg.replace('&', '').replace('#', '').split(';')
gz = float(datas[0])
except KeyboardInterrupt:
break
except Exception:
continue
ser.close()
print('Finish')
gz = float(datas[0])
except KeyboardInterrupt:
break
except Exception:
continue
ser.close()
if __name__ == '__main__':
g.init()
# 开启串口接收线程
thread.start_new_thread(serial_recv_thread, ())
# 开启RTK线程
thread.start_new_thread(ntrip.fun, ())
# 等待定位成功
while g.getValue('lat') == None or g.getValue('lat') == 0:
time.sleep(1)
pass
logging.info('已定位')
# 获取导航点
getPoints('gps.csv')
if len(sidePoints) > 1:
logging.info('已加载导航点，共有%d个点' % len(sidePoints))
index = 0
pos = Point(g.getValue('lat'), g.getValue('lng'))
# 判断刚开始时，是否需要原地转
state = State.TURN if utils.getDistance(
pos, sidePoints[0]) > 3 else State.GO
while index < len(sidePoints):
pos = Point(g.getValue('lat'), g.getValue('lng'))
if state == State.TURN:
def serial_recv_thread():
ser = serial.Serial("/dev/ttyAMA0", 115200)
gz = 0
flag_ang_control = 0
flag_finish = False
flag_back = False
while True:
if True or g.getValue('mode') != 1 and g.getValue('mode') != 0:
try:
msg = ser.readline().strip()
if msg.startswith('$') and msg.endswith('#'):
datas = msg.replace('$', '').replace('#', '').split(';')
if len(datas) >= 10:
yaw = float(datas[3])
gps_speed = float(datas[7])
if flag_finish:
angle_error = 279 - yaw
if angle_error > 180:
angle_error -= 360
elif angle_error <= -180:
angle_error += 360
if flag_back:
dire = angle_control_5(angle_error, -gz)
dire = 0
ser.write(utils.parityGenerate(
"$E;-25,%d#" % dire))
dist = utils.getDistance(Point(lat_rev1, lng_rev1), Point(
g.getValue('lat'), g.getValue('lng')))
print(dist, angle_error)
if dist < 2:
time.sleep(1)
ser.write(utils.parityGenerate("$E;0,0#"))
break
continue
if abs(angle_error) < 10:
ser.write(utils.parityGenerate("$E;-25,0#"))
flag_back = True
dire = angle_control_4(angle_error, -gz)
print("角度差、方向油门：%d\t%d" % (angle_error, dire))
ser.write(utils.parityGenerate(
"$E;%d,%d#" % (0, dire)))
continue
(thrust, dire, flag_ang_control) = control_reverse_point(
g.getValue('lat'), g.getValue('lng'), yaw, gps_speed, -gz, lat_rev, lng_rev, flag_ang_control)
# print(g.getValue('lat'), g.getValue(
# 'lng'), g.getValue('mode'), yaw, gps_speed, thrust, dire)
ser.write(utils.parityGenerate(
"$E;%d,%d#" % (thrust, dire)))
if thrust == 0 and dire == 0:
flag_finish = True
elif msg.startswith('&') and msg.endswith('#'):
datas = msg.replace('&', '').replace('#', '').split(';')
gz = float(datas[0])
except KeyboardInterrupt:
break
except Exception:
continue
ser.close()
print('Finish')