# open the serial port between odroid and zibgee
myserial = serial.Serial('/dev/ttyUSB1', 115200, timeout=0.5) # read timeout is 0.5s
print myserial.portstr
if myserial.isOpen():
'''initialise datas'''
receivedDatas = ""
delt_T = 1.0 #1.0s
lastRecord = current_milli_time()
loop_cnt = 1
CONST_VX0 = 0.0
CONST_VY0 = 0.0
CONST_D12 = pow(0.1111, 2)
#leader object
leader = leaderControl(CONST_VX0, CONST_VY0, CONST_D12, vehicle.location.lat, vehicle.location.lon) #vx0 = 0.5, vy0 = 0.5, x0 = vehicle.location.lat, y0 = vehicle.location.lon
'''control loop'''
while not api.exit:
if v.mode.name != "GUIDED":
print "User has changed fight mode, aborting loop!"
break
if current_milli_time() - lastRecord >= 1000: #1000ms
lastRecord = current_milli_time()
print "[%s] current time is: %f" % (loop_cnt, lastRecord)
loop_cnt += 1
# read
receivedDatas = myserial.readline()
neighbourLon, neighbourLat = decode_position(receivedDatas)
print "neighbourLat: " + neighbourLat
tempos2 = positionList[1]
print "waiting for connecting..."
if tempos1 != 255.0 and tempos2 != 255.0:
print "connecting ok!"
break
arm_and_takeoff(3.5)
'''
After the vehicle reaches a target height, do other things
'''
''' main thread'''
#leader object
leader = leaderControl(CONST_VX0, CONST_VY0, CONST_D12, float(math.pi / 180)*R*vehicle.location.lat, float(math.pi / 180)*R*vehicle.location.lon) #vx0 = 0.5, vy0 = 0.5, x0 = vehicle.location.lat, y0 = vehicle.location.lon
'''control loop'''
while not api.exit:
not_received_flag = 0
if vehicle.mode.name != "GUIDED":
print "User has changed fight mode, aborting loop!"
break
if current_milli_time() - lastRecord >= delt_T: #500ms
lastRecord = current_milli_time()
print "[%s] current time is: %f" % (loop_cnt, lastRecord)
loop_cnt += 1
#write
myserial.write('o'+str("%.8f" % vehicle.location.lon))
'''
After the vehicle reaches a target height, do other things
'''
# open the serial port between odroid and zibgee
myserial = serial.Serial('/dev/ttyUSB1', 115200, timeout=0.5) # read timeout is 0.5s
print myserial.portstr
if myserial.isOpen():
'''initialise datas'''
receivedDatas = ""
delt_T = 1.0 #1.0s
lastRecord = current_milli_time()
#leader object
leader = leaderControl(0.5, 0.5, vehicle.location.lat, vehicle.location.lon) #vx0 = 0.5, vy0 = 0.5, x0 = vehicle.location.lat, y0 = vehicle.location.lon
loop_cnt = 1
'''control loop'''
while not api.exit:
if v.mode.name != "GUIDED":
print "User has changed fight mode, aborting loop!"
break
if current_milli_time() - lastRecord >= 1000: #1000ms
lastRecord = current_milli_time()
print "[%s] current time is: %f" % (loop_cnt, lastRecord)
loop_cnt += 1
# read
receivedDatas = myserial.readline()
neighbourLat, neighbourLon = decode_position(receivedDatas)
