def setpoint():
with serial.Serial('/dev/ttyACM0', 115200, timeout=1) as ser:
try:
msg = pynmea2.parse(ser.readline().decode('ascii', errors='replace'))
if isinstance(msg, pynmea2.types.talker.GGA):
rover =[msg.latitude ,msg.longitude ,msg.altitude] #deg,deg,m
rover_enu = geodetic2enu(rover[0] ,rover[1] ,rover[2],basellh[0] ,basellh[1] ,basellh[2] )#m,m,m
else:
setpoint()
except:
print('nmea msg parse error')
return None,None
return rover_enu ,msg
menseki = 0
print("作業面積リセット")
time.sleep(2)
elif (key == 10): #距離リセット
kyori = 0
print("走行距離リセット")
time.sleep(2)
elif (key == 12): #shp属性を取る
shpdata = getshp()
try:
if shpdata != 0:
area = shpdata[3] #4番目に面積レコード
if float(shpdata[14]) != 0:
(aax, aay, aah) = geodetic2enu(float(shpdata[14]),
float(shpdata[15]), nh,
basellh[0], basellh[1],
basellh[2])
(bbx, bby, bbh) = geodetic2enu(float(shpdata[16]),
float(shpdata[17]), nh,
basellh[0], basellh[1],
basellh[2])
rrad = math.atan2((bby - aay), (bbx - aax))
base = True
c = wide / 2 - 30
print("Auto Set Line")
time.sleep(1)
else:
area = 0
time.sleep(2)
except:
print("Auto set error")
elif ( key == 10):#half
c += wide/2
print("Half Wide Offset")
time.sleep(1)
elif ( key == 12):#shp属性を取る
shpdata=getshp()
try:
if shpdata !=0:
area = shpdata[1] #2番目に面積レコード
if shpdata[5] != 0:
wide = WIDE
(aax,aay,aah) = (geodetic2enu(float(shpdata[5]) ,float(shpdata[6]) ,nowmsg["Height"]*0.001,basellh[0] ,basellh[1] ,basellh[2]))
(bbx,bby,bbh) = (geodetic2enu(float(shpdata[7]) ,float(shpdata[8]) ,nowmsg["Height"]*0.001,basellh[0] ,basellh[1] ,basellh[2]))
aax *= 100 #cm
aay *= 100
bbx *= 100
bby *= 100
rrad =math.atan2(( bby - aay ),( bbx - aax )) - addrad
AABBsin = math.sin(-rrad)
AABBcos = math.cos(-rrad)
base = True
c = -wide /2 -margin #枕+1工程から開始
menseki = 0
d = Direction
print("Auto Set Line")
time.sleep(1)
else :