def __init__(self, serialPort, readrate, myIDnum, Logmode):
threading.Thread.__init__(self)
#Create loggers and put them here
self.logger = logging.getLogger("BalloonReader:SensorThread")
self.logger.setLevel(Logmode)
myhandler = logging.StreamHandler()
self.logger.addHandler(myhandler)
self.logger.info("Sensor Thread has started")
#self.serialPortname = serialPort
if serialPort is None:
self.fn = None
else:
self.fn = serial.Serial(port=serialPort, timeout=1)
#check that this opened and throw a critical error otherwise
self.readrate = readrate
thisid = PyPacket.PacketID(PyPacket.PacketPlatform.AIRCRAFT, myIDnum)
self.MYID = str(thisid.getBytes())
fn = time.strftime("%Y%m%d-%H%M%S")
self.packet_log = PyPacketLogger.PyPacketLogger(fn)
self.packet_log.initFile()
self.logger.info("Logging Sensor Packets to: %s",
self.packet_log.logname)
self.PyPkt = PyPacket.PyPacket()
self.PyPkt.setDataType(PyPacket.PacketDataType.PKT_BALLOON_SENSOR_SET)
self.PyPkt.setID(thisid.getBytes())
def buildNodeHeartBeatPacket(subscribers, PacketNodeCounter):
#Create the ID information for the packet
pkt_id = PyPacket.PacketID(PyPacket.PacketPlatform.DUMMY, MYNUM)
pkt = PyPacket.PyPacket()
pkt.setDataType(PyPacket.PacketDataType.PKT_NODE_HEARTBEAT)
pkt.setID(pkt_id.getBytes())
#Define the basic components
msg = PyPackets_pb2.NodeHeartBeat()
msg.packetNum = PacketNodeCounter
msg.ID = str(pkt.getID())
msg.time = time.time()
#add all the subscriber information
c = 0
for n in subscribers:
new = msg.sub.add()
new.id = str(n.ID)
new.datatype = str(n.TYPE)
new.port = n.PORT
new.address = n.IP
new.msgfreq = n.FREQ
c += 1 #increment counter
#end loop
#serialize the data into packet
data_str = msg.SerializeToString()
pkt.setData(data_str)
#pkt.displayPacket()
del msg
return pkt.getPacket() #return the byte array msg
def WebReaderSide(l, QueList):
#QueList = [Q1,Q2,Q3,Q4]
#Create A set Number of Stream Reader Threads
print 'Created WebReaderSide'
#List of ports being used
ports = [10000, 10005, 10010, 10015]
#Create all the subscribers
#For Network Manager
#TODO! Update these parts
subNM = Subscriber.Subscriber(
PyPacket.PacketDataType.PKT_NETWORK_MANAGER_STATUS,
PyPacket.PacketID(PyPacket.PacketPlatform.GROUND_CONTROL_STATION,
00).getBytes(), ports[0], 'localhost', 1)
#For Aircraft State
subAS = Subscriber.Subscriber(
PyPacket.PacketDataType.PKT_AUTOPILOT_PIXHAWK,
PyPacket.PacketID(PyPacket.PacketPlatform.AIRCRAFT, 10).getBytes(),
ports[1], 'localhost', 1)
#For RF Data Msg1
subRF = Subscriber.Subscriber(
PyPacket.PacketDataType.PKT_RF_DATA_MSG,
PyPacket.PacketID(PyPacket.PacketPlatform.AIRCRAFT, 10).getBytes(),
ports[2], 'localhost', 1)
#For RF PL Map
subMP = Subscriber.Subscriber(
PyPacket.PacketDataType.PKT_RF_PL_MAP_MSG,
PyPacket.PacketID(PyPacket.PacketPlatform.AIRCRAFT, 10).getBytes(),
ports[3], 'localhost', 1)
StreamReaderThreadList = []
srt1 = Server_Reader.StreamReaderThread(subNM, QueList[0], shutdown_event)
StreamReaderThreadList.append(srt1)
srt2 = Server_Reader.StreamReaderThread(subAS, QueList[1], shutdown_event)
StreamReaderThreadList.append(srt2)
srt3 = Server_Reader.StreamReaderThread(subRF, QueList[2], shutdown_event)
StreamReaderThreadList.append(srt3)
srt4 = Server_Reader.StreamReaderThread(subMP, QueList[3], shutdown_event)
StreamReaderThreadList.append(srt4)
#loop through the stream readers and start each of them
srt1.start()
srt2.start()
srt3.start()
srt4.start()
while threading.active_count() > 1:
try:
l.acquire()
print 'Running with %s Readers' % len(StreamReaderThreadList)
l.release()
time.sleep(1)
except (KeyboardInterrupt, SystemExit):
shutdown_event.set()
sys.exit()
def run(self):
node_rate = 5
deltaT_node = 999
PacketCounterNode = 1
TestSubs = []
#Generate the subs here (hardcoded)
subA = Subscriber.Subscriber(
PyPacket.PacketDataType.PKT_DMY_MSG,
PyPacket.PacketID(PyPacket.PacketPlatform.DUMMY, 10).getBytes(),
PORT, 'localhost', 1)
subB = Subscriber.Subscriber(
PyPacket.PacketDataType.PKT_DMY_MSG,
PyPacket.PacketID(PyPacket.PacketPlatform.DUMMY, 20).getBytes(),
PORT, 'localhost', 1)
subC = Subscriber.Subscriber(
PyPacket.PacketDataType.PKT_DMY_MSG,
PyPacket.PacketID(PyPacket.PacketPlatform.DUMMY, 30).getBytes(),
PORT, 'localhost', 1)
subD = Subscriber.Subscriber(
PyPacket.PacketDataType.PKT_DMY_MSG,
PyPacket.PacketID(PyPacket.PacketPlatform.DUMMY, 40).getBytes(),
PORT, 'localhost', 1)
TestSubs.append(subA)
TestSubs.append(subB)
TestSubs.append(subC)
TestSubs.append(subD)
#Run loops
while not shutdown_event.is_set():
#check to send node info at desired frequency
if deltaT_node >= node_rate:
#create node message
m = buildNodeHeartBeatPacket(TestSubs, PacketCounterNode)
#add to queue
msg_que.put(m)
PacketCounterNode += 1
#update timer
tlast_node = time.time()
#Check to see if we have any messages in Que to send
try:
next_msg = msg_que.get_nowait()
except Queue.Empty:
empty_que = 1
else:
self.socket.sendto(next_msg, ('localhost', NM_PORT))
print 'sent message at %s' % (time.time())
#update deltaTs
deltaT_node = time.time() - tlast_node
#time.sleep(0.1)
#-------------
#End of While Loop
self.socket.close()
def buildNMHeartBeat(my_id, sublist, nmlist, packet_counter, my_ip, my_port):
# Create the packet
pkt = PyPacket.PyPacket()
pkt.setDataType(PyPacket.PacketDataType.PKT_NETWORK_MANAGER_HEARTBEAT)
pkt.setID(my_id.getBytes())
# Create the message
msg = PyPackets_pb2.NMHeartBeat()
msg.ID = str(my_id.getBytes())
msg.packetNum = packet_counter
msg.time = time.time()
# add local subscribers
for s in sublist:
new = msg.sub.add()
new.id = str(s.ID)
new.datatype = str(s.TYPE)
new.port = my_port # my port
new.address = my_ip # my IP
new.msgfreq = s.FREQ
# end loop
# add network managers
for nm in nmlist:
new = msg.nms.add()
new.IP = nm[0]
new.PORT = str(nm[1])
# end loop
# Serialize and store
data_str = msg.SerializeToString()
pkt.setData(data_str)
del msg
return pkt.getPacket()
def buildNMStatusMessage(my_id, sublist, packet_counter, total_msgs,
since_last_msgs, avg_delay, size_of_que):
# Create the packet
pkt = PyPacket.PyPacket()
pkt.setDataType(PyPacket.PacketDataType.PKT_NETWORK_MANAGER_STATUS)
pkt.setID(my_id.getBytes())
# Create the message
msg = PyPackets_pb2.NMStatus()
# Store the header information
msg.packetNum = packet_counter
msg.ID = str(pkt.getID())
msg.time = time.time()
# List of the local subscribers
for s in sublist:
new = msg.subs.add()
new.id = s.ID
new.datatype = s.TYPE
new.port = s.PORT
new.address = s.IP
new.msgfreq = s.FREQ
#Add in other information
msg.messagesInQue = size_of_que
msg.numberOfLocalSubscribers = len(sublist)
msg.numberOfMsgs = since_last_msgs
msg.totalMsgsRecv = total_msgs
msg.avgTimeDelay = avg_delay # Delay between received and sent
# Serialize and store
data_str = msg.SerializeToString()
pkt.setData(data_str)
del msg
return pkt.getPacket()
def buildNodeHeartBeat(my_id, subscribers, packet_counter):
# Create the packet
pkt = PyPacket.PyPacket()
pkt.setDataType(PyPacket.PacketDataType.PKT_NODE_HEARTBEAT)
pkt.setID(my_id.getBytes())
#Create the message
msg = PyPackets_pb2.NodeHeartBeat()
# Store the header information
msg.packetNum = packet_counter
msg.ID = str(pkt.getID())
msg.time = time.time()
#Add all the subscriber info
c = 0
for n in subscribers:
new = msg.sub.add()
new.id = str(n.ID)
new.datatype = str(n.TYPE)
new.port = n.PORT
new.address = n.IP
new.msgfreq = n.FREQ
c += 1
#END LOOP
# Serialize and store
data_str = msg.SerializeToString()
pkt.setData(data_str)
del msg
return pkt.getPacket()
def readPacketFromLog(self, readfile, location):
# with an open file, read x bytes
readfile.seek(location) #check the number of bytes left to read?
#inBytes = readfile.read(1)
#if struct.unpack('<b',inBytes)[0] == 0xAA:
# #start of the sequence; else we are miss aligned
# print "Packets are Aligned"
#else:
# print "Misaligned Packets"
# first read the 8 bytes for timestamp
inBytes = readfile.read(8)
#print len(inBytes)
ts = struct.unpack('<d', inBytes)[0]
# next read the 4 bytes for length of data
inBytes = readfile.read(4)
#print len(inBytes)
dsize = struct.unpack('<L', inBytes)[0]
#print dsize
# read the data & create packet
inBytes = readfile.read(dsize)
#print len(inBytes)
pkt = PyPacket.PyPacket()
pkt.setPacket(inBytes)
# add to the packet buffer
self.packetbuffer.addtolist(pkt, ts)
# calculate new location
# inBytes = readfile.read(1) #read teh last byte
newlocation = location + 4 + 8 + dsize #plus the header bytes
return newlocation
def buildDummyPacket(PacketDummyCounter):
#Create the id information for the packet
pkt_id = PyPacket.PacketID(PyPacket.PacketPlatform.DUMMY, MYNUM)
pkt = PyPacket.PyPacket()
pkt.setDataType(PyPacket.PacketDataType.PKT_DMY_MSG)
pkt.setID(pkt_id.getBytes())
#Define the basic components
msg = PyPackets_pb2.dummy_msg()
msg.packetNum = PacketDummyCounter
msg.ID = str(pkt.getID())
msg.time = time.time()
#Add the data
msg.s = MYSTRING
#Serialize the data
data_str = msg.SerializeToString()
pkt.setData(data_str)
#pkt.displayPacket()
del msg
return pkt.getPacket()
def WebReaderSide(l, QueList):
# QueList = [Q1,Q2,Q3,Q4]
# Create A set Number of Stream Reader Threads
print 'Created WebReaderSide'
# List of ports being used
ports = [10000, 10005, 10010, 10015]
# Create all the subscribers
# For Network Manager
subNM = Subscriber.Subscriber(
PyPacket.PacketDataType.PKT_NETWORK_MANAGER_STATUS,
PyPacket.PacketID(PyPacket.PacketPlatform.GROUND_CONTROL_STATION,
00).getBytes(), ports[0], 'localhost', 1)
# For Drifters
subBS = Subscriber.Subscriber(
PyPacket.PacketDataType.PKT_BALLOON_SENSOR_SET,
PyPacket.PacketID(PyPacket.PacketPlatform.AIRCRAFT, 90).getBytes(),
ports[1], 'localhost', 1)
#Add one for cars
#Add one for etc
StreamReaderThreadList = []
srt1 = Server_Reader.StreamReaderThread(subNM, QueList[0], shutdown_event)
StreamReaderThreadList.append(srt1)
srt2 = Server_Reader.StreamReaderThread(subBS, QueList[1], shutdown_event)
StreamReaderThreadList.append(srt2)
# loop through the stream readers and start each of them
srt1.start()
srt2.start()
while threading.active_count() > 1:
try:
l.acquire()
print 'Running with %s Readers' % len(StreamReaderThreadList)
l.release()
time.sleep(1)
except (KeyboardInterrupt, SystemExit):
shutdown_event.set()
sys.exit()
def run(self):
#pypacket
mhPacket = PyPacket.PyPacket()
newPacket.setDataType(PyPacket.PacketDataType.PKT_DMY_MSG)
newPacket.setID(self.MYID.getBytes())
#Protobuf message (just using dummy for strings right now)
mh_data_msg = PyPackets_pb2.dummy_msg()
mh_data_msg.ID = str(self.MYID.getBytes())
#measurement timing variables
last_measurement_time = 0
measurement_rate = 1 #in Seconds]
packet_counter = 1
#Sensor run loop
while not shutdown_event.is_set():
#is it time to measure
now_time = time.time()
delta_time = now_time - last_measurement_time
if delta_time > measurement_rate:
self.logger.debug("Logging a new measurement")
#Reset last time
last_measurement_time = now_time
#Get the response from radio
response = urllib2(self.request)
#INSERT WHATEVER YOU WANT TO GET FROM RADIO
mh_data_msg.s = response
#INSERT WHATEVER YOU WNAT FROM THE VEHICLE (Putting it into a json string
json_obj = {}
json_obj['lat'] = vehicle.location.global_frame.lat
json_obj['lon'] = vehicle.location.global_frame.lon
json_obj['alt'] = vehicle.location.global_relative_frame.alt
json_obj['airspeed'] = vehicle.airspeed
json_str = json.dumps(json_obj)
mh_data_msg.s2 = json_str
#store time of packet generation
mh_data_msg.time = now_time
#store and increment packet counter
mh_data_msg.packetNum = packet_counter
packet_counter += 1
#Parse protobuf message
data_str = mh_data_msg.SerializeToString()
#Put data into packet
mhPacket.setData(data_str)
#Log packet
self.packet_log.writePacketToLog(mhPacket)
#End while loop
self.logger.info("CLosing Sensor Task")
print('\t Microhard Sensor Task [Closed]')
def buildRFLearnCommand(my_id, packet_counter, node_list_to_learn,
node_list_to_plan, waypoint_flag, mode_string):
#Create the packet
pkt = PyPacket.PyPacket()
pkt.setDataType(PyPacket.PacketDataType.PKT_RF_LEARN_CMD)
pkt.setID(my_id.getBytes())
#Create the message
msg = PyPackets_pb2.RF_Learn_Cmd()
#Store the header information
msg.packetNum = packet_counter
msg.ID = str(pkt.getID())
msg.time = time.time()
#store message specific items
msg.NodesToLearn.extend(node_list_to_learn)
msg.NodesToPlan.extend(node_list_to_plan)
msg.calculateWaypointFlag = waypoint_flag
msg.mode = mode_string
#Serialize and store
data_str = msg.SerializeToString()
pkt.setData(data_str)
del msg
return pkt.getPacket()
def writeOutStates(self, states, waypoint):
#STATES: Pn, Pe, Chidot, Chi, Hdot, H, Va
#create packet
thisPacket = PyPacket.PyPacket()
# TODO! Update to be fed in or given the correct ID for this vehicle and packet information
pktid = PyPacket.PacketID(PyPacket.PacketPlatform.AIRCRAFT,10)
myidbytes = pktid.getBytes()
thisPacket.setDataType(PyPacket.PacketDataType.PKT_AUTOPILOT_PIXHAWK)
thisPacket.setID(pktid.getBytes())
#increment counter
self.packetCounter += 1
#TODO! Update to be fed in or given the correct ID for this vehicle and packet information
msg = PyPackets_pb2.AircraftPixhawkState()
msg.packetNum = self.packetCounter
msg.ID = "AC 10"
msg.time = time.time()
#convert NED to LLA
lla = assorted_lib.NED2LLA([states[0],states[1],states[5]],self.centerLLA)
msg.LLA_Pos.x = lla[0]#lat
msg.LLA_Pos.y = lla[1]#lon
msg.LLA_Pos.z = -lla[2]#Alt (ASL)
msg.velocity.x = math.cos(states[3])*states[6]
msg.velocity.y = math.sin(states[3])*states[6]
msg.velocity.z = states[4]
msg.attitude.x = 0
msg.attitude.y = 0
msg.attitude.z = states[3] #send in radians
msg.airspeed = states[6]
#optional
msg.omega.x = 0
msg.omega.y = 0
msg.omega.z = states[2]
msg.mode = "1A Model Sim"
#optional
msg.distance_to_mav = 0
#optional
msg.batteryStatus.voltage = 12.0
msg.batteryStatus.current = 0.0
#optional
msg.currentWaypoint.LLA_Pos.x = waypoint.lat#lat
msg.currentWaypoint.LLA_Pos.y = waypoint.lon#lon
msg.currentWaypoint.LLA_Pos.z = waypoint.asl#ASL
msg.currentWaypoint.cost = 0
#Serialize Msg
datastr = msg.SerializeToString()
thisPacket.setData(datastr)
#TODO! Add packet logger
#Add to Que
self.myQue.put(thisPacket.getPacket())
self.logger.debug('Added another message to the que; number %i', self.packetCounter)
home_lon = -105.00
base_altitude = 100 #meters
#Connection to Ardupilot
connection_string = '/dev/ttyS1' #Beaglebone setup
apmm = PyUAS_Ardupilot_Mission_Manager.ArduPilotMissionManager(
connection_string, "Testing Microhard Logging", base_altitude,
[home_lat, home_lon])
#Hand off the vehicle data stream
vehicle = apmm.vehicle
#Microhard string
mh_str = "http://192.168.168.150/cgi-bin/webif/getradio.sh" #Make sure 192.168.168.1## matches aircraft number
log_level = 10 #debug
#My id
my_id = PyPacket.PacketID(PyPacket.PacketPlatform.DUMMY, 0)
#start the microhard logging thread
mhlogging = MicroHardLogging(mh_str, my_id, log_level)
mhlogging.start()
while threading.active_count() > 1:
try:
time.sleep(0.1)
except (KeyboardInterrupt, SystemExit):
#Exit
shutdown_event.set()
#End while
print "Exiting"
sys.exit()