def PingRespond(TargetID, TimeStamp):
# check to see if the target is valid
# a broadcast ping is not allowed
if TargetID <= 0 or TargetID > 255:
return -1
# get required data classes
PingPacket = CustMes.MESSAGE_FRAME()
PingData = CustMes.MESSAGE_PING()
# set up message frame
PingPacket.MessageID = 1
PingPacket.TargetID = TargetID
PingPacket.SystemID = GlobalVals.SYSTEM_ID
# set up payload
PingData.Intiator = False
PingData.TimeDiff = time.time() - TimeStamp
PingPacket.Payload = PingData.data_to_bytes()
# send the ping
sendPacket(PingPacket)
return 0
def ImaginaryBalloons():
# Balloons = [ImaginaryBalloon("Balloon_255.csv", 255), ImaginaryBalloon("Balloon_254.csv", 254), ImaginaryBalloon("Balloon_253.csv", 253)]
Balloons = [
ImaginaryBalloon("Balloon_254.csv", 254),
ImaginaryBalloon("Balloon_253.csv", 253)
]
break_condition = False
while True:
for Balloon in Balloons:
interpolated_tMercator = Balloon.get_tMercator(int(time.time()))
GPSdata = CustMes.MESSAGE_GPS()
GPSdata.Longitude = interpolated_tMercator.Longitude
GPSdata.Latitude = interpolated_tMercator.Latitude
GPSdata.GPSTime = interpolated_tMercator.Epoch
GPSdata.SystemID = Balloon.SystemID
GPSdata.Altitude = interpolated_tMercator.Altitude
#print(str(GPSdata.Latitude) + ', ' + str(GPSdata.Longitude) + ', ' + str(GPSdata.GPSTime) + ', ' + str(GPSdata.SystemID) + ', ' + str(GPSdata.Altitude))
with GlobalVals.GPS_DATA_BUFFER_MUTEX:
GlobalVals.GPS_DATA_BUFFER.append(GPSdata)
with GlobalVals.RECIEVED_GPS_LOCAL_DATA_MUTEX:
GlobalVals.RECIEVED_GPS_LOCAL_DATA = True
with GlobalVals.BREAK_IMAGINARY_BALLOONS_MUTEX:
if GlobalVals.BREAK_IMAGINARY_BALLOONS_THREAD:
break_condition = True
break
if break_condition:
break
time.sleep(1)
def main():
recievedPackets = False
# sendTime = int(time.time() + 1)
# this loop will wait for packets and then process them
while True:
# check if packets have been recived
with GlobalVals.RECIEVED_PACKETS_MUTEX:
if GlobalVals.RECIEVED_PACKETS:
recievedPackets = True
# if no packets have been recived then sleep and loop
if not recievedPackets:
time.sleep(0.1)
continue
else:
recievedPackets = False
# go through all the packets in the buffer
recievedPacket = CustMes.MESSAGE_FRAME()
with GlobalVals.PACKET_BUFFER_IN_MUTEX:
while len(GlobalVals.PACKET_BUFFER_IN) > 0:
recievedPacket = GlobalVals.PACKET_BUFFER_IN.pop(0)
# if the packet is a ping
if recievedPacket.MessageID == 1:
NetworkManager.PingRespond(recievedPacket.SystemID,
recievedPacket.Timestamp)
# if the packet is a GPS data packet
if recievedPacket.MessageID == 2:
# get the GPS data
GPSdata = CustMes.MESSAGE_GPS()
error = GPSdata.bytes_to_data(recievedPacket.Payload)
if error != 0:
print("Radio Network Main: GPS data error " +
str(error))
continue
# print("GPS Data from " + str(recievedPacket.SystemID) + ":")
# print("Lon:" + str(round(GPSdata.Longitude,3)) + ", Lat:" + str(round(GPSdata.Latitude,3)) + ", Alt:" + str(round(GPSdata.Altitude,2)) + ", Time:" + str(GPSdata.GPSTime) + "\n")
# set the system id for the GPS data
GPSdata.SystemID = recievedPacket.SystemID
if not GPSHandler.GPS_FormatCheck(GPSdata):
print(
"GPS message via RFD900 was broken. Discard it...")
continue
# print("GPS Data from " + str(recievedPacket.SystemID) + ":")
# print("Lon:" + str(round(GPSdata.Longitude,3)) + ", Lat:" + str(round(GPSdata.Latitude,3)) + ", Alt:" + str(round(GPSdata.Altitude,2)) + ", Time:" + str(GPSdata.GPSTime) + "\n")
# update GPS_Log
with GlobalVals.GPS_LOG_MUTEX:
update_GPS_Log(GPSdata)
distance = distanceMatrixCalculation(
GlobalVals.GPS_ALL, 0)
print(
"--------------------------------------------------------------------------------------------------"
)
print("GPS GPS GPS " + str(recievedPacket.SystemID) +
str(recievedPacket.SystemID) +
str(recievedPacket.SystemID) + ":" + " Lon:" +
str(round(GPSdata.Longitude, 3)) + ", Lat:" +
str(round(GPSdata.Latitude, 3)) + ", Alt:" +
str(round(GPSdata.Altitude, 1)) + ", Time:" +
str(round(GPSdata.GPSTime, 1)))
print('Distance from GPS [m]:\n', distance)
# put data into the buffer
# with GlobalVals.GPS_DATA_BUFFER_MUTEX:
# GlobalVals.GPS_DATA_BUFFER.append(GPSdata)
# set the flags for the buffer
# with GlobalVals.RECIEVED_GPS_RADIO_DATA_MUTEX:
# GlobalVals.RECIEVED_GPS_RADIO_DATA = True
continue
# if the packet is string message
if recievedPacket.MessageID == 3:
# get the string
StrData = CustMes.MESSAGE_STR()
error = StrData.bytes_to_data(recievedPacket.Payload)
if error != 0:
print("Packet error, packet will be discarded.\n")
continue
# print string
print(StrData.MessageStr)
continue
# if the packet is an EKF GPS data packet
if recievedPacket.MessageID == 5:
# get the GPS data
EKF_Data = CustMes.MESSAGE_EKF()
error = EKF_Data.bytes_to_data(recievedPacket.Payload)
if error != 0:
print("Radio Network Main: EKF data error " +
str(error))
continue
# set the system id for the GPS data
EKF_Data.SystemID = recievedPacket.SystemID
if not EKFHandler.EKF_FormatCheck(EKF_Data):
print(
"EKF message via RFD900 was broken. Discard it...")
continue
# update GPS_Log
with GlobalVals.EKF_LOG_MUTEX:
update_EKF_Log(EKF_Data)
distance = distanceEKF_MatrixCalculation(
GlobalVals.GPS_ALL,
0,
)
print(
"=================================================================================================="
)
print("EKF EKF EKF " + str(recievedPacket.SystemID) +
str(recievedPacket.SystemID) +
str(recievedPacket.SystemID) + ":" + " Lon:" +
str(round(EKF_Data.Longitude, 3)) + ", Lat:" +
str(round(EKF_Data.Latitude, 3)) + ", Alt:" +
str(round(EKF_Data.Altitude, 1)) + ", Time:" +
str(round(EKF_Data.Epoch, 1)))
print('Distance from EKF [m]:\n', distance)
continue
# if the packet is an temperature data packet
# Temperature
if recievedPacket.MessageID == 6:
# get the RSSI data
temperatureData = CustMes.MESSAGE_TEMP()
error = temperatureData.bytes_to_data(
recievedPacket.Payload)
if error != 0:
print("Radio Network Main: temperature data error " +
str(error))
continue
# set the system id for the GPS data
temperatureData.SystemID = recievedPacket.SystemID
if not TemperatureHandler.temperatureFormatCheck(
temperatureData):
print(
"Temperature message via RFD900 was broken. Discard it..."
)
continue
tempur = TEMPERATURE(temperatureData.SystemID,
temperatureData.Temperature,
temperatureData.Epoch)
with GlobalVals.TEMPERATURE_UPDATE_MUTEX:
update_temperature(tempur)
# print(" Temperature Data from " + str(recievedPacket.SystemID) + ":" + "Temperature:" + str(round(temperatureData.Temperature,1)))
# RSSI
if recievedPacket.MessageID == 7:
# get the RSSI data
RSSI_Data = CustMes.MESSAGE_RSSI()
error = RSSI_Data.bytes_to_data(recievedPacket.Payload)
if error != 0:
print("Radio Network Main: RSSI data error " +
str(error))
continue
# set the system id for the GPS data
RSSI_Data.SystemID = recievedPacket.SystemID
# print(RSSI_Data.SystemID)
# print(RSSI_Data.TargetPayloadID)
# print(GlobalVals.RSSI_ALLOCATION)
# Check if the message was sent correctly via the RFD900
if not RSSI_Handler.RSSI_FormatCheck(RSSI_Data):
print(
"RSSI message via RFD900 was broken. Discard it..."
)
continue
print("RSSI Data from " + str(recievedPacket.SystemID) +
": " + "RSSI Distance:" +
str(round(RSSI_Data.Distance, 1)) +
", Filtered RSSI: " +
str(round(RSSI_Data.FilteredRSSI, 1)) +
", TargetPayloadID: " +
str(RSSI_Data.TargetPayloadID) + ", Time: " +
str(RSSI_Data.Epoch) + ", SysID: " +
str(RSSI_Data.SystemID))
if GlobalVals.SYSTEM_ID == 1:
with GlobalVals.RSSI_ALLOCATION_MUTEX:
# print("UPDATE RSSI ALLOCATION FROM RADIO [",RSSI_Data.SystemID,"] !!!!")
# print(GlobalVals.RSSI_ALLOCATION)
GlobalVals.RSSI_ALLOCATION[RSSI_Data.SystemID - 1][
int(RSSI_Data.TargetPayloadID) - 1] = True
# print("check 32")
# print(GlobalVals.RSSI_ALLOCATION)
RSSI_Handler.getPairAllocation()
# put data into the buffer
with GlobalVals.RSSI_DATA_BUFFER_MUTEX:
GlobalVals.RSSI_DATA_BUFFER.append(RSSI_Data)
# set the flags for the buffer
with GlobalVals.RECIEVED_RSSI_RADIO_DATA_MUTEX:
GlobalVals.RECIEVED_RSSI_RADIO_DATA = True
continue
if recievedPacket.MessageID == 8:
# get the RSSI data
RSSI_AllocationData = CustMes.MESSAGE_RSSI_ALLOCATION()
error = RSSI_AllocationData.bytes_to_data(
recievedPacket.Payload)
if error != 0:
print(
"Radio Network Main: RSSI Allocation data error " +
str(error))
continue
# set the system id for the GPS data
RSSI_AllocationData.SystemID = recievedPacket.SystemID
if not RSSI_Handler.RSSI_AllocationFormatCheck(
RSSI_AllocationData):
print(
"RSSI Allocation message via RFD900 was broken. Discard it..."
)
continue
print("RSSI Allocation Data from " +
str(recievedPacket.SystemID) + ":" + "Pair:" +
str(int(RSSI_AllocationData.Pair)))
# put data into the buffer
with GlobalVals.RSSI_DATA_ALLOCATION_BUFFER_MUTEX:
# if len(GlobalVals.RSSI_DATA_ALLOCATION_BUFFER)>5:
# GlobalVals.RSSI_DATA_ALLOCATION_BUFFER.pop(0)
GlobalVals.RSSI_DATA_ALLOCATION_BUFFER.append(
int(RSSI_AllocationData.Pair))
# set the flags for the buffer
with GlobalVals.RECIEVED_RSSI_ALLOCATION_RADIO_DATA_MUTEX:
GlobalVals.RECIEVED_RSSI_ALLOCATION_RADIO_DATA = True
continue
with GlobalVals.PACKET_STATS_LOG_MUTEX:
packetStatsLogTmp = copy.deepcopy(GlobalVals.PACKET_STATS_LOG)
with GlobalVals.PACKET_STATS_AWS_MUTEX:
GlobalVals.PACKET_STATS_AWS = packetStatsLogTmp
# if radio GPS data has been recived record it
if GlobalVals.RECIEVED_GPS_RADIO_DATA:
logString = ""
with GlobalVals.GPS_DATA_BUFFER_MUTEX:
while len(GlobalVals.GPS_DATA_BUFFER) > 0:
# get the GPS data
GPSData = GlobalVals.GPS_DATA_BUFFER.pop(0)
Longitude = GPSData.Longitude
Latitude = GPSData.Latitude
Altitude = GPSData.Altitude
GPSTime = int(GPSData.GPSTime)
SystemID = GPSData.SystemID
# create message string
logString = logString + str(GPSTime) + "," + str(
SystemID) + "," + str(Longitude) + "," + str(
Latitude) + "," + str(Altitude) + "\n"
# write the log string to file
try:
fileObj = open(GlobalVals.GROUND_STATION_LOG_FILE, "a")
fileObj.write(logString)
fileObj.close()
except Exception as e:
print("Exception: " + str(e.__class__))
print(e)
print("Error using GPS data log file")
def EKFLoggerSocket():
# set up socket
while True:
try:
socket_logger = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
socket_logger.connect(
(GlobalVals.HOST, GlobalVals.EKF_LOGGER_SOCKET))
socket_logger.settimeout(GlobalVals.EKF_LOGGER_SOCKET_TIMEOUT)
except Exception as e:
if e.args[1] == 'Connection refused':
print('Retry connecting to EKF....')
time.sleep(1)
continue
else:
print("Exception: " + str(e.__class__))
print(
"There was an error starting the EKFlogger socket. This thread will now stop."
)
with GlobalVals.BREAK_EKF_LOGGER_THREAD_MUTEX:
GlobalVals.BREAK_EKF_LOGGER_THREAD = True
return
break
print('Connected to EKF!!!')
# intialize variables
bufferRead = 1024
breakMainThread = False
while True:
# if flag is set break the thread
with GlobalVals.BREAK_EKF_LOGGER_THREAD_MUTEX:
if GlobalVals.BREAK_EKF_LOGGER_THREAD:
break
# read the socket
while True:
try:
data_bytes = socket_logger.recv(bufferRead)
# print("EKF socket received")
break
except Exception as e:
if e.args[0] == 'timed out':
print("EKFLoggerSocket() Receive timed out. Retrying...")
time.sleep(0.1)
continue
else:
print("EKFLoggerSocket(): Receive Connection error.")
breakMainThread = True
break
if breakMainThread:
break
# print("EKF socket received2")
# if there is nothing in the socket then it has timed out
if len(data_bytes) == 0:
continue
data_str = data_bytes.decode('utf-8')
# print("EKF socket received3")
# print(data_str)
# print('---')
string_list = extract_str_btw_curly_brackets(data_str)
if len(string_list) > 0:
ekf_list = []
for string in string_list:
received, ekf_i = stringToEKF(string)
if received:
ekf_list.append(ekf_i)
idx = 0
# with GlobalVals.EKF_UPDATE_MUTEX:
while idx < len(ekf_list):
ekf_update(ekf_list[idx])
idx += 1
ekf = copy.deepcopy(GlobalVals.EKF_ALL)
EKF_Data = CustMes.MESSAGE_EKF()
EKF_Data.Longitude = ekf.lon
EKF_Data.Latitude = ekf.lat
EKF_Data.Altitude = ekf.alt
EKF_Data.Epoch = ekf.epoch
EKF_Data.SystemID = ekf.sysID
EKF_Data.PosX = ekf.posX
EKF_Data.PosY = ekf.posY
EKF_Data.P00 = ekf.p00
EKF_Data.P01 = ekf.p01
EKF_Data.P10 = ekf.p10
EKF_Data.P11 = ekf.p11
# add data to the gps buffer
# with GlobalVals.EKF_DATA_BUFFER_MUTEX:
# GlobalVals.EKF_DATA_BUFFER.append(EKF_Data)
# set the flag for the data
# with GlobalVals.RECIEVED_EKF_LOCAL_DATA_MUTEX:
# GlobalVals.RECIEVED_EKF_LOCAL_DATA = True
# send GPS data to other balloons
EKF_Packet = CustMes.MESSAGE_FRAME()
EKF_Packet.SystemID = GlobalVals.SYSTEM_ID
EKF_Packet.MessageID = 5
EKF_Packet.TargetID = 0
EKF_Packet.Payload = EKF_Data.data_to_bytes()
NetworkManager.sendPacket(EKF_Packet)
# print(EKF_Data.Longitude)
# print("***************************")
# pause a little bit so the mutexes are not getting called all the time
time.sleep(1)
socket_logger.close()
return
def RSSI_LoggerSocket(host,port,index):
# set up socket
while True:
try:
socket_logger = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
socket_logger.connect((host,port))
socket_logger.settimeout(GlobalVals.RSSI_LOGGER_SOCKET_TIMEOUT)
except Exception as e:
if e.args[1] == 'Connection refused':
print('Retry connecting to RSSI....')
time.sleep(1)
continue
else:
print("Exception: " + str(e.__class__))
print("There was an error starting the RSSI Logger socket. This thread will now stop.")
with GlobalVals.BREAK_RSSI_LOGGER_THREAD_MUTEX[index]:
GlobalVals.BREAK_RSSI_LOGGER_THREAD[index] = True
return
break
print('Connected to RSSI[',index,'], Port: ',port,'!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!')
# intialize variables
bufferRead = 1024
breakMainThread = False
while True:
# if flag is set break the thread
with GlobalVals.BREAK_RSSI_LOGGER_THREAD_MUTEX[index]:
if GlobalVals.BREAK_RSSI_LOGGER_THREAD[index]:
break
# read the socket
while True:
try:
data_bytes = socket_logger.recv(bufferRead)
# print("data received[",index,"]")
break
except Exception as e:
if e.args[0] == 'timed out':
print("RSSI_LoggerSocket Receive Time Out for Index [",index,"]. Retrying ...")
time.sleep(0.1)
continue
else:
print("RSSI_LoggerSocket Connection error.")
breakMainThread = True
break
if breakMainThread:
break
# if there is nothing in the socket then it has timed out
if len(data_bytes) == 0:
continue
data_str = data_bytes.decode('utf-8')
string_list = extract_str_btw_curly_brackets(data_str)
if len(string_list) > 0:
# print("len string: ", len(string_list))
rssi_list = []
for string in string_list:
received, rssi_i = stringToRSSI(string)
if received:
rssi_list.append(rssi_i)
idx = 0
# print("check lenstring 1")
with GlobalVals.RSSI_UPDATE_MUTEX[index]:
while idx < len(rssi_list):
# print("len rssi_list: ",len(rssi_list), "idx: ",idx)
rssi_update(rssi_list[idx])
idx += 1
# with GlobalVals.RSSI_UPDATE_MUTEX[index]:
rssi_i = copy.deepcopy(GlobalVals.RSSI_ALL)
# print(rssi_i)
# print("check lenstring 2")
RSSI_Data = CustMes.MESSAGE_RSSI()
RSSI_Data.FilteredRSSI = rssi_i.rssi_filtered
RSSI_Data.Distance = rssi_i.distance
RSSI_Data.Epoch = rssi_i.epoch
RSSI_Data.SystemID = GlobalVals.SYSTEM_ID
RSSI_Data.TargetPayloadID = int(findTargetPayloadID(index))
# print("check lenstring 3")
# add data to the gps buffer
with GlobalVals.RSSI_DATA_BUFFER_MUTEX:
GlobalVals.RSSI_DATA_BUFFER.append(RSSI_Data)
# print("check lenstring 4")
# # set the flag for the data
with GlobalVals.RECIEVED_RSSI_LOCAL_DATA_MUTEX: # 2 nodes?
GlobalVals.RECIEVED_RSSI_LOCAL_DATA = True
# print("check lenstring 5")
with GlobalVals.RSSI_ALLOCATION_MUTEX:
# print("UPDATE RSSI ALLOCATION MATRIX LOCALLYYYY")
# print(GlobalVals.RSSI_ALLOCATION)
GlobalVals.RSSI_ALLOCATION[GlobalVals.SYSTEM_ID-1][RSSI_Data.TargetPayloadID-1] = True
# print("check lenstring 6")
# send GPS data to other balloons
RSSI_Packet = CustMes.MESSAGE_FRAME()
RSSI_Packet.SystemID = GlobalVals.SYSTEM_ID
RSSI_Packet.MessageID = 7
RSSI_Packet.TargetID = 0
RSSI_Packet.Payload = RSSI_Data.data_to_bytes()
NetworkManager.sendPacket(RSSI_Packet)
# print(RSSI_Data)
# print(RSSI_Packet.Payload)
print("SENDING RSSI TO RFD900: "+", RSSI Distance:" + str(RSSI_Data.Distance) + ", Filtered RSSI: " + str(RSSI_Data.FilteredRSSI) + ", TargetPayloadID: " + str(RSSI_Data.TargetPayloadID) + ", SysID: " + str(RSSI_Data.SystemID))
# print("check lenstring 5")
# print(index)
# print(RSSI_Data)
# print("***************************")
# pause a little bit so the mutexes are not getting called all the time
time.sleep(0.1)
# while True:
# print("RSSI TO RFD900 SOCKET CLOSE!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
# time.sleep(0.5)
socket_logger.close()
return
def RSSI_AllocationDistributor():
# print("RSSI ALLOCATION DISTRIBUTOR 1")
Distro_Socket = [None]*len(GlobalVals.RSSI_ALLOCATION_DISTRO_SOCKET)
Distro_Connection = [None]*len(GlobalVals.RSSI_ALLOCATION_DISTRO_SOCKET)
for i in range(len(GlobalVals.RSSI_ALLOCATION_DISTRO_SOCKET)):
# start socket
Distro_Socket[i] = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
Distro_Socket[i].setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR,1)
Distro_Socket[i].bind((GlobalVals.HOST, GlobalVals.RSSI_ALLOCATION_DISTRO_SOCKET[i]))
Distro_Socket[i].settimeout(GlobalVals.RSSI_LOGGER_SOCKET_TIMEOUT)
# Wait for connection on the distro socket
try:
Distro_Socket[i].listen(1)
Distro_Connection[i], addr = Distro_Socket[i].accept()
Distro_Connection[i].settimeout(GlobalVals.RSSI_LOGGER_SOCKET_TIMEOUT)
print("RSSI Allocation Logger[",i,"] Connected to ", addr)
except Exception as e:
print("Exception: " + str(e.__class__))
print("Error in the RSSI_Allocation Distributor[",i,"] logger socket. Now closing thread.")
with GlobalVals.BREAK_RSSI_ALLOCATION_DISTRO_THREAD_MUTEX:
GlobalVals.BREAK_RSSI_ALLOCATION_DISTRO_THREAD = True
return 0
breakThread = False
nextPair = 1
while True:
# print("RSSI ALLOCATION DISTRIBUTOR 2")
if breakThread:
break
with GlobalVals.BREAK_RSSI_ALLOCATION_DISTRO_THREAD_MUTEX:
if GlobalVals.BREAK_RSSI_ALLOCATION_DISTRO_THREAD:
break
# print("RSSI ALLOCATION DISTRIBUTOR 3")
if GlobalVals.SYSTEM_ID == 1:
with GlobalVals.NEXT_PAIR_MUTEX:
nextPair = GlobalVals.NEXT_PAIR
else:
with GlobalVals.RSSI_DATA_ALLOCATION_BUFFER_MUTEX:
if len(GlobalVals.RSSI_DATA_ALLOCATION_BUFFER) > 0:
RSSI_DataAllocation = GlobalVals.RSSI_DATA_ALLOCATION_BUFFER.pop(0)
nextPair = RSSI_DataAllocation
messageStr = "{'pair': " + str(nextPair) +";}"
messageStr_bytes = messageStr.encode('utf-8')
# print("Send Pair Num to RFD900")
# print('Allocated pair: '+messageStr)
# send the message
for i in range(len(GlobalVals.RSSI_ALLOCATION_DISTRO_SOCKET)):
try:
Distro_Connection[i].sendall(messageStr_bytes)
except Exception as e:
print("Exception: " + str(e.__class__))
print("Error when sending to RSSI Allocation Distro_Connection[",i,"]. Now closing thread.")
breakThread = True
time.sleep(1)
break
# Send via RFD900 network
if GlobalVals.SYSTEM_ID == 1:
RSSI_Allocation = CustMes.MESSAGE_RSSI_ALLOCATION()
RSSI_Allocation.Pair = GlobalVals.NEXT_PAIR
RSSI_AllocationPacket = CustMes.MESSAGE_FRAME()
RSSI_AllocationPacket.SystemID = GlobalVals.SYSTEM_ID
RSSI_AllocationPacket.MessageID = 8
RSSI_AllocationPacket.TargetID = 0
RSSI_AllocationPacket.Payload = RSSI_Allocation.data_to_bytes()
NetworkManager.sendPacket(RSSI_AllocationPacket)
# print("sending RSSI ALLOCATOR (pair: ",RSSI_Allocation.Pair,") from 1...")
time.sleep(0.5)
# while True:
# print("Closing RSSI Allocation Distro !!!!!!!!!")
# time.sleep(0.5)
for i in range(GlobalVals.N_RSSI_NODE_PUBLISH):
Distro_Connection[i].close()
def GPSLoggerSocket():
# set up socket
try:
socket_logger = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
socket_logger.connect((GlobalVals.HOST, GlobalVals.GPS_LOGGER_SOCKET))
socket_logger.settimeout(GlobalVals.GPS_LOGGER_SOCKET_TIMEOUT)
except Exception as e:
print("Exception: " + str(e.__class__))
print("There was an error starting the GPSLoggerSocket logger socket. This thread will now stop.")
print("GPS_PORT: ",GlobalVals.GPS_LOGGER_SOCKET)
print("GPS_TIMEOUT: ",GlobalVals.GPS_LOGGER_SOCKET_TIMEOUT )
with GlobalVals.BREAK_GPS_LOGGER_THREAD_MUTEX:
GlobalVals.BREAK_GPS_LOGGER_THREAD = True
return
# intialize variables
synced = False
syncA = False
syncB = False
bufferRead = 1
breakMainThread = False
while True:
# if flag is set break the thread
with GlobalVals.BREAK_GPS_LOGGER_THREAD_MUTEX:
if GlobalVals.BREAK_GPS_LOGGER_THREAD:
break
# reset buffer read when not synced
if not synced:
bufferRead = 1
# read the socket
while True:
try:
data_bytes = socket_logger.recv(bufferRead)
break
except Exception as e:
if e.args[0] == 'timed out':
print("GPSLoggerSocket receive timed out. Retrying ...")
time.sleep(0.1)
continue
else:
print("GPSLoggerSocket: Receive Connection error.")
breakMainThread = True
break
if breakMainThread:
break
# if there is nothing in the socket then it has timed out
if len(data_bytes) == 0:
continue
# for the sync bytes (0xAA 0x55)
if not synced:
if data_bytes[0] == 0xAA and not syncA:
syncA = True
bufferRead = 1
continue
elif data_bytes[0] == 0x55 and syncA:
syncB = True
bufferRead = 32
else:
syncA = False
syncB = False
bufferRead = 1
continue
# if both bytes have been found in order then the socket buffer is synced
if syncA and syncB:
synced = True
syncA = False
syncB = False
continue
# once it is scyned read the rest of the data
if synced and bufferRead == 32:
# convert payload values back to double
LongitudeTuple = struct.unpack('!d',data_bytes[0:8])
LatitudeTuple = struct.unpack('!d',data_bytes[8:16])
AltitudeTuple = struct.unpack('!d',data_bytes[16:24])
GPSTimeTuple = struct.unpack('!d',data_bytes[24:32])
# store converted values
Longitude = LongitudeTuple[0]
Latitude = LatitudeTuple[0]
Altitude = AltitudeTuple[0]
GPSTime = GPSTimeTuple[0]
# Debug message
#print(str(GPSTime) + "," + str(Longitude) + "," + str(Latitude) + "," + str(Altitude) + "\n")
# use GPS message payload to store value
GPSData = CustMes.MESSAGE_GPS()
GPSData.Longitude = Longitude
GPSData.Latitude = Latitude
GPSData.Altitude = Altitude
GPSData.GPSTime = GPSTime
GPSData.SystemID = GlobalVals.SYSTEM_ID
# add data to the gps buffer
with GlobalVals.GPS_DATA_BUFFER_MUTEX:
GlobalVals.GPS_DATA_BUFFER.append(GPSData)
# set the flag for the data
with GlobalVals.RECIEVED_GPS_LOCAL_DATA_MUTEX:
GlobalVals.RECIEVED_GPS_LOCAL_DATA = True
# send GPS data to other balloons
GPSPacket = CustMes.MESSAGE_FRAME()
GPSPacket.SystemID = GlobalVals.SYSTEM_ID
GPSPacket.MessageID = 2
GPSPacket.TargetID = 0
GPSPacket.Payload = GPSData.data_to_bytes()
NetworkManager.sendPacket(GPSPacket)
# print("SEND GPS TO RFD900!!!!!!")
# reset
synced = False
# pause a little bit so the mutexes are not getting called all the time
time.sleep(0.01)
socket_logger.close()
return
def main():
recievedPackets = False
sendTime = int(time.time() + 1)
# this loop will wait for packets and then process them
while True:
curTime = int(time.time())
# send dummy GPS data once a second
if curTime >= sendTime:
sendTime = curTime + 1
GPSpacket = CustMes.MESSAGE_FRAME()
GPSdata = CustMes.MESSAGE_GPS()
# set up GPS packet
GPSpacket.MessageID = 0x02
GPSpacket.TargetID = 0
GPSpacket.SystemID = GlobalVals.SYSTEM_ID
# load dummy GPS data
GPSdata.Longitude = 77.77
GPSdata.Latitude = 88.88
GPSdata.Altitude = 99.99
GPSdata.GPSTime = 100.01
# send dummy GPS packet
GPSpacket.Payload = GPSdata.data_to_bytes()
NetworkManager.sendPacket(GPSpacket)
# check if packets have been recived
with GlobalVals.RECIEVED_PACKETS_MUTEX:
if GlobalVals.RECIEVED_PACKETS:
recievedPackets = True
# if no packets have been recived then sleep and loop
if not recievedPackets:
continue
else:
recievedPackets = False
# go through all the packets in the buffer
with GlobalVals.PACKET_BUFFER_IN_MUTEX:
while len(GlobalVals.PACKET_BUFFER_IN) > 0:
recievedPacket = GlobalVals.PACKET_BUFFER_IN.pop(0)
# if the packet is a ping
if recievedPacket.MessageID == 1:
NetworkManager.PingRespond(recievedPacket.SystemID,
recievedPacket.Timestamp)
# if the packet is a GPS data packet
if recievedPacket.MessageID == 2:
# get the GPS data
recData = CustMes.MESSAGE_GPS()
error = recData.bytes_to_data(recievedPacket.Payload)
if error != 0:
print("Dummy Radio 1: GPS data error " + str(error) +
".\n")
continue
print("GPS Data from " + str(recievedPacket.SystemID) +
":")
print("Lon:" + str(recData.Longitude) + ", Lat:" +
str(recData.Latitude) + ", Alt:" +
str(recData.Altitude) + ", Time:" +
str(recData.GPSTime) + "\n")
continue
# if the packet is string message
if recievedPacket.MessageID == 3:
# get the string
StrData = CustMes.MESSAGE_STR()
error = StrData.bytes_to_data(recievedPacket.Payload)
if error != 0:
print("Dummy Radio 2: Message error " + str(error) +
".\n")
continue
# print string
print(StrData.MessageStr)
continue
def SerialManagerThread():
# Initialise variables
readBytes = bytearray()
bufferRead = 1
reading = True
breakThread = False
connected = True
synced = False
# Connect to the serial port
try:
serial_port = serial.Serial(
port=GlobalVals.PORT,
baudrate=GlobalVals.BAUDRATE,
bytesize=serial.EIGHTBITS,
parity=serial.PARITY_NONE,
stopbits=serial.STOPBITS_ONE,
timeout=GlobalVals.TIMEOUT,
)
serial_port.reset_input_buffer()
serial_port.reset_output_buffer()
except Exception as e:
print(
"Network Manager: Unable to initiate serial port. Now breaking thread."
)
print("Network Manager: Exception: " + str(e.__class__))
connected = False
# Wait a second to let the port initialize
time.sleep(1)
# begin the loop used for reading and writing to the serial port
while connected and not breakThread:
# check if the thread needs to read or send data
with GlobalVals.SEND_PACKETS_MUTEX:
if not synced:
reading = not GlobalVals.SEND_PACKETS
# check if the thread needs to break
with GlobalVals.BREAK_NETWORK_THREAD_MUTEX:
breakThread = GlobalVals.BREAK_NETWORK_THREAD
if breakThread:
break
# if the thread needs to read data
if reading:
# reset buffer read when not synced
if not synced:
bufferRead = 1
# read some bytes out of the buffer
try:
comOut = serial_port.read(size=bufferRead)
except serial.SerialTimeoutException:
continue
except (OSError, serial.SerialException) as e:
print(
"Network Manager: Failed to read com port. Now breaking Thread."
)
print("Network Manager: Exception: " + str(e.__class__))
breakThread = True
continue
if len(comOut) == 0:
continue
# find the start bytes for the packet (0xFD) to sync the input buffer with the packet
if not synced:
readBytes.clear()
if comOut[0] == 0xFD:
synced = True
readBytes.append(comOut[0])
continue
else:
synced = False
continue
# Get the length of the packet once synced
if synced and bufferRead == 1:
bufferRead = comOut[0] # get the payload length
bufferRead = bufferRead + 6 # (payload length + header bytes + CRC) - already read bytes
readBytes.append(comOut[0])
continue
# form the rest of the packet
if bufferRead >= 2:
# join the bytes that have already been read with the new ones
for x in comOut:
readBytes.append(x)
# Put the packet data into a message frame
completePacket = CustMes.MESSAGE_FRAME()
error = completePacket.bytes_to_data(readBytes)
# increment packet counter
with GlobalVals.PACKET_COUNT_MUTEX:
if completePacket.SystemID - 1 in range(
GlobalVals.N_REAL_BALLOON):
GlobalVals.PACKET_COUNT[completePacket.SystemID -
1] = GlobalVals.PACKET_COUNT[
completePacket.SystemID -
1] + 1
else:
GlobalVals.PACKET_COUNT[
GlobalVals.
N_REAL_BALLOON] = GlobalVals.PACKET_COUNT[
GlobalVals.N_REAL_BALLOON] + 1
# if packet is corrupted
if error != 0:
GlobalVals.reportError(1, error, completePacket.SystemID)
print("Network Manager: Packet check error " + str(error) +
" from " + str(completePacket.SystemID) +
". Getting next packet.\n")
# look for next packet
synced = False
continue
# update sequence trackers
with GlobalVals.SEQ_TRACKERS_MUTEX:
TrackerLength = len(GlobalVals.SEQ_TRACKERS)
IsNewTracker = True
# if trackers exist
if TrackerLength > 0:
# find a tracker with a matching SystemID
trackerIndex = 0
for x in range(TrackerLength):
if GlobalVals.SEQ_TRACKERS[
x].SystemID == completePacket.SystemID:
IsNewTracker = False
trackerIndex = x
break
# if one does exist update the tarcker
if not IsNewTracker:
seqDiff = GlobalVals.SEQ_TRACKERS[
trackerIndex].NextNumber(
completePacket.SeqNumber)
# if the sequence has broken report the error
if seqDiff != 0:
GlobalVals.reportError(3, seqDiff,
completePacket.SystemID)
print("Network Manager: Packet from " +
str(completePacket.SystemID) +
" is out of sequence by " +
str(seqDiff) + "\n")
# increment packet counter with missing packets
with GlobalVals.PACKET_COUNT_MUTEX:
if completePacket.SystemID - 1 in range(
GlobalVals.N_REAL_BALLOON):
GlobalVals.PACKET_COUNT[
completePacket.SystemID -
1] = GlobalVals.PACKET_COUNT[
completePacket.SystemID -
1] + seqDiff
else:
GlobalVals.PACKET_COUNT[
GlobalVals.
N_REAL_BALLOON] = GlobalVals.PACKET_COUNT[
GlobalVals.
N_REAL_BALLOON] + seqDiff
else:
# if no trackers exist make a new one
IsNewTracker = True
# if a new tracker needs to be made make one here
if IsNewTracker:
newTracker = GlobalVals.sequenceTracker()
newTracker.SystemID = completePacket.SystemID
newTracker.CurrentNumber = completePacket.SeqNumber
GlobalVals.SEQ_TRACKERS.append(newTracker)
# check if the packet is meant for this system
if completePacket.TargetID != GlobalVals.SYSTEM_ID and completePacket.TargetID != 0:
# if it is not for this system reset values and look for next packet
synced = False
readBytes.clear()
continue
# timestamp packet
completePacket.Timestamp = time.time()
# if the packet is a ping from an initiator, respond and discard packet
#if completePacket.MessageID == 1 and completePacket.Payload[0] == 0x00:
# PingRespond(completePacket.SystemID,completePacket.Timestamp)
# if the packet is a ping response, put it in the ping buffer
if completePacket.MessageID == 1 and completePacket.Payload[
0] == 0xFF:
with GlobalVals.PACKET_PING_BUFFER_MUTEX:
while len(GlobalVals.PACKET_PING_BUFFER
) >= GlobalVals.PACKET_BUFFER_IN_MAX_SIZE:
GlobalVals.PACKET_PING_BUFFER.pop(0)
GlobalVals.PACKET_PING_BUFFER.append(completePacket)
# set the recieved flag
with GlobalVals.RECIEVED_PING_MUTEX:
GlobalVals.RECIEVED_PING = True
# Other packets are put into the standard packet buffer
else:
# debug check
if completePacket.MessageID == 2 and len(
completePacket.Payload) == 9:
print("FOUND IT!!!!")
# append the packet to the input buffer and remove old packets
with GlobalVals.PACKET_BUFFER_IN_MUTEX:
while len(GlobalVals.PACKET_BUFFER_IN
) >= GlobalVals.PACKET_BUFFER_IN_MAX_SIZE:
GlobalVals.PACKET_BUFFER_IN.pop(0)
GlobalVals.PACKET_BUFFER_IN.append(completePacket)
# set recieved packet flag
with GlobalVals.RECIEVED_PACKETS_MUTEX:
GlobalVals.RECIEVED_PACKETS = True
# reset values
synced = False
readBytes.clear()
continue
# to catch if something happened out of order, in which case reset everything
else:
synced = False
readBytes.clear()
continue
# if something needs to be written
else:
# reset send flag
with GlobalVals.SEND_PACKETS_MUTEX:
GlobalVals.SEND_PACKETS = False
#send each packet
with GlobalVals.PACKET_BUFFER_OUT_MUTEX:
while True:
if len(GlobalVals.PACKET_BUFFER_OUT) > 0:
# get the next packet to send, set the sequence number and convert to bytes
outPacket = GlobalVals.PACKET_BUFFER_OUT.pop(0)
outPacket.SeqNumber = getSeqNumber()
bytesToSend = outPacket.data_to_bytes()
try:
serial_port.write(bytesToSend)
serial_port.flush()
except Exception as e:
print(
"Network Manager: Failed to write to com port. Now breaking thread."
)
print("Network Manager: Exception: " +
str(e.__class__))
breakThread = True
break
else:
break
if connected:
try:
serial_port.close()
except Exception as e:
print("Network Manager: Exception: " + str(e.__class__))
return
def PingTarget(TargetID):
# check to see if the target is valid
# a broadcast ping is not allowed
if TargetID <= 0 or TargetID > 255:
return -1
# get required data classes
PingPacket = CustMes.MESSAGE_FRAME()
PingData = CustMes.MESSAGE_PING()
# set up message frame
PingPacket.MessageID = 1
PingPacket.TargetID = TargetID
PingPacket.SystemID = GlobalVals.SYSTEM_ID
# set up payload
PingData.Intiator = True
PingPacket.Payload = PingData.data_to_bytes()
# send the ping
TimeSent = time.time()
sendPacket(PingPacket)
# intialise some variables
recievedPacket = CustMes.MESSAGE_FRAME()
loopIndex = 0
# wait for respnse from ping
while True:
time.sleep(GlobalVals.PING_WAIT_TIME)
# if a packet hasn't been recieved yet
if not GlobalVals.RECIEVED_PING:
# check for time out and increment the loop counter
if loopIndex >= GlobalVals.PING_LOOP_LIMIT:
print("Network Manager: Ping Timeout Error.")
return -2
else:
loopIndex = loopIndex + 1
continue
# if a packet has been recieved
else:
# reset the flag
with GlobalVals.RECIEVED_PING_MUTEX:
GlobalVals.RECIEVED_PING = False
# get the packet
with GlobalVals.PACKET_PING_BUFFER_MUTEX:
found = False
x = 0
while x < len(GlobalVals.PACKET_PING_BUFFER):
if GlobalVals.PACKET_PING_BUFFER[x].SystemID == TargetID:
# remove old ping requests
if GlobalVals.PACKET_PING_BUFFER[
x].Timestamp < TimeSent:
GlobalVals.PACKET_PING_BUFFER.pop(x)
continue
# keep only the newest packet
else:
recievedPacket = GlobalVals.PACKET_PING_BUFFER.pop(
x)
found = True
x = x + 1
# if a ping packet hasn't been recieved loop back
if not found:
loopIndex = loopIndex + 1
continue
break
# get time stamp from packet and calculate the ping
TimeRec = recievedPacket.Timestamp
ping = int((TimeRec - TimeSent) * 1000)
# return the ping
return ping
def main():
recievedPackets = False
# sendTime = int(time.time() + 1)
# this loop will wait for packets and then process them
while True:
# curTime = int(time.time())
# # send dummy GPS data once a second
# if curTime >= sendTime:
# sendTime = curTime + 1
# GPSpacket = CustMes.MESSAGE_FRAME()
# GPSdata = CustMes.MESSAGE_GPS()
# # set up GPS packet
# GPSpacket.MessageID = 0x02
# GPSpacket.TargetID = 0
# GPSpacket.SystemID = GlobalVals.SYSTEM_ID
# # load dummy GPS data
# GPSdata.Longitude = 7.7
# GPSdata.Latitude = 8.8
# GPSdata.Altitude = 9.9
# GPSdata.GPSTime = 10.1
# # send dummy GPS packet
# GPSpacket.Payload = GPSdata.data_to_bytes()
# NetworkManager.sendPacket(GPSpacket)
# check if packets have been recived
with GlobalVals.RECIEVED_PACKETS_MUTEX:
if GlobalVals.RECIEVED_PACKETS:
recievedPackets = True
# if no packets have been recived then sleep and loop
if not recievedPackets:
time.sleep(0.1)
continue
else:
recievedPackets = False
# go through all the packets in the buffer
recievedPacket = CustMes.MESSAGE_FRAME()
with GlobalVals.PACKET_BUFFER_IN_MUTEX:
while len(GlobalVals.PACKET_BUFFER_IN) > 0:
recievedPacket = GlobalVals.PACKET_BUFFER_IN.pop(0)
# if the packet is a ping
if recievedPacket.MessageID == 1:
NetworkManager.PingRespond(recievedPacket.SystemID,
recievedPacket.Timestamp)
# if the packet is a GPS data packet
if recievedPacket.MessageID == 2:
# get the GPS data
GPSdata = CustMes.MESSAGE_GPS()
error = GPSdata.bytes_to_data(recievedPacket.Payload)
if error != 0:
print("Radio Network Main: GPS data error " +
str(error))
continue
# print("GPS Data from " + str(recievedPacket.SystemID) + ":" +"Lon:" + str(round(GPSdata.Longitude,3)) + ", Lat:" + str(round(GPSdata.Latitude,3)) + ", Alt:" + str(round(GPSdata.Altitude,1)) + ", Time:" + str(round(GPSdata.GPSTime,1)))
# print("Lon:" + str(GPSdata.Longitude) + ", Lat:" + str(GPSdata.Latitude) + ", Alt:" + str(GPSdata.Altitude) + ", Time:" + str(GPSdata.GPSTime) + "\n")
# set the system id for the GPS data
GPSdata.SystemID = recievedPacket.SystemID
if not GPSHandler.GPS_FormatCheck(GPSdata):
print(
"GPS message via RFD900 was broken. Discard it...")
continue
print("Lon:" + str(round(GPSdata.Longitude, 2)) +
", Lat:" + str(round(GPSdata.Latitude, 2)) +
", Alt:" + str(round(GPSdata.Altitude, 2)) +
", Time:" + str(GPSdata.GPSTime))
# print("RECEIVED GPS from ",GPSdata.SystemID,"!!")
# put data into the buffer
with GlobalVals.GPS_DATA_BUFFER_MUTEX:
GlobalVals.GPS_DATA_BUFFER.append(GPSdata)
# set the flags for the buffer
with GlobalVals.RECIEVED_GPS_RADIO_DATA_MUTEX:
GlobalVals.RECIEVED_GPS_RADIO_DATA = True
continue
# get the GPS data
# recData = CustMes.MESSAGE_GPS()
# error = recData.bytes_to_data(recievedPacket.Payload)
# if error != 0:
# print ("Radio Network Main: GPS data error " + str(error) )
# continue
# print("GPS Data from " + str(recievedPacket.SystemID) + ":")
# print("Lon:" + str(recData.Longitude) + ", Lat:" + str(recData.Latitude) + ", Alt:" + str(recData.Altitude) + ", Time:" + str(recData.GPSTime) + "\n")
# continue
# if the packet is string message
if recievedPacket.MessageID == 3:
# get the string
StrData = CustMes.MESSAGE_STR()
error = StrData.bytes_to_data(recievedPacket.Payload)
if error != 0:
print("Packet error, packet will be discarded.\n")
continue
# print string
print(StrData.MessageStr)
continue
if recievedPacket.MessageID == 4:
# get the GPS data
IMUdata = CustMes.MESSAGE_IMU()
error = IMUdata.bytes_to_data(recievedPacket.Payload)
if error != 0:
print("Radio Network Main: IMU data error " +
str(error))
continue
print("IMU Data from " + str(recievedPacket.SystemID) +
":")
print("Euler:" + str(IMUdata.Euler321_theta) + "\n")
# set the system id for the GPS data
IMUdata.SystemID = recievedPacket.SystemID
# put data into the buffer
with GlobalVals.IMU_DATA_BUFFER_MUTEX:
GlobalVals.IMU_DATA_BUFFER.append(GPSdata)
# set the flags for the buffer
with GlobalVals.RECIEVED_IMU_RADIO_DATA_MUTEX:
GlobalVals.RECIEVED_IMU_RADIO_DATA = True
continue
if recievedPacket.MessageID == 5:
# get the GPS data
EKF_Data = CustMes.MESSAGE_EKF()
error = EKF_Data.bytes_to_data(recievedPacket.Payload)
if error != 0:
print("Radio Network Main: EKF data error " +
str(error))
continue
# print("GPS Data from " + str(recievedPacket.SystemID) + ":" +"Lon:" + str(round(GPSdata.Longitude,3)) + ", Lat:" + str(round(GPSdata.Latitude,3)) + ", Alt:" + str(round(GPSdata.Altitude,1)) + ", Time:" + str(round(GPSdata.GPSTime,1)))
# print("Lon:" + str(GPSdata.Longitude) + ", Lat:" + str(GPSdata.Latitude) + ", Alt:" + str(GPSdata.Altitude) + ", Time:" + str(GPSdata.GPSTime) + "\n")
# set the system id for the GPS data
EKF_Data.SystemID = recievedPacket.SystemID
if not EKFHandler.EKF_FormatCheck(EKF_Data):
print(
"EKF message via RFD900 was broken. Discard it...")
continue
print("EKF EKF EKF Data from [", EKF_Data.SystemID,
"], Lat: ", round(EKF_Data.Latitude, 2), ", Lon: ",
round(EKF_Data.Longitude, 2), ", Alt: ",
round(EKF_Data.Altitude, 2))
# put data into the buffer
# with GlobalVals.EKF_DATA_BUFFER_MUTEX:
# GlobalVals.EKF_DATA_BUFFER.append(EKF_Data)
# set the flags for the buffer
# with GlobalVals.RECIEVED_EKF_RADIO_DATA_MUTEX:
# GlobalVals.RECIEVED_EKF_RADIO_DATA = True
continue
# Temperature
if recievedPacket.MessageID == 6:
# get the RSSI data
temperatureData = CustMes.MESSAGE_TEMP()
error = temperatureData.bytes_to_data(
recievedPacket.Payload)
if error != 0:
print("Radio Network Main: temperature data error " +
str(error))
continue
# set the system id for the GPS data
temperatureData.SystemID = recievedPacket.SystemID
if not TemperatureHandler.temperatureFormatCheck(
temperatureData):
print(
"Temperature message via RFD900 was broken. Discard it..."
)
continue
# print(" Temperature Data from " + str(recievedPacket.SystemID) + ":" + "Temperature:" + str(round(temperatureData.Temperature,1)))
# RSSI
if recievedPacket.MessageID == 7:
# get the RSSI data
RSSI_Data = CustMes.MESSAGE_RSSI()
error = RSSI_Data.bytes_to_data(recievedPacket.Payload)
if error != 0:
print("Radio Network Main: RSSI data error " +
str(error))
continue
# set the system id for the GPS data
RSSI_Data.SystemID = recievedPacket.SystemID
# print(RSSI_Data.SystemID)
# print(RSSI_Data.TargetPayloadID)
# print(GlobalVals.RSSI_ALLOCATION)
# Check if the message was sent correctly via the RFD900
if not RSSI_Handler.RSSI_FormatCheck(RSSI_Data):
print(
"RSSI message via RFD900 was broken. Discard it..."
)
continue
print("RSSI Data from " + str(recievedPacket.SystemID) +
": " + "RSSI Distance:" +
str(round(RSSI_Data.Distance, 1)) +
", Filtered RSSI: " +
str(round(RSSI_Data.FilteredRSSI, 1)) +
", TargetPayloadID: " +
str(RSSI_Data.TargetPayloadID) + ", Time: " +
str(RSSI_Data.Epoch) + ", SysID: " +
str(RSSI_Data.SystemID))
if GlobalVals.SYSTEM_ID == 1:
with GlobalVals.RSSI_ALLOCATION_MUTEX:
# print("UPDATE RSSI ALLOCATION FROM RADIO [",RSSI_Data.SystemID,"] !!!!")
# print(GlobalVals.RSSI_ALLOCATION)
GlobalVals.RSSI_ALLOCATION[RSSI_Data.SystemID - 1][
int(RSSI_Data.TargetPayloadID) - 1] = True
# print("check 32")
# print(GlobalVals.RSSI_ALLOCATION)
RSSI_Handler.getPairAllocation()
# put data into the buffer
with GlobalVals.RSSI_DATA_BUFFER_MUTEX:
GlobalVals.RSSI_DATA_BUFFER.append(RSSI_Data)
# set the flags for the buffer
with GlobalVals.RECIEVED_RSSI_RADIO_DATA_MUTEX:
GlobalVals.RECIEVED_RSSI_RADIO_DATA = True
continue
if recievedPacket.MessageID == 8:
# get the RSSI data
RSSI_AllocationData = CustMes.MESSAGE_RSSI_ALLOCATION()
error = RSSI_AllocationData.bytes_to_data(
recievedPacket.Payload)
if error != 0:
print(
"Radio Network Main: RSSI Allocation data error " +
str(error))
continue
# set the system id for the GPS data
RSSI_AllocationData.SystemID = recievedPacket.SystemID
if not RSSI_Handler.RSSI_AllocationFormatCheck(
RSSI_AllocationData):
print(
"RSSI Allocation message via RFD900 was broken. Discard it..."
)
continue
print(" RSSI Allocation Data from " +
str(recievedPacket.SystemID) + ":" + "Pair:" +
str(int(RSSI_AllocationData.Pair)))
# put data into the buffer
with GlobalVals.RSSI_DATA_ALLOCATION_BUFFER_MUTEX:
# if len(GlobalVals.RSSI_DATA_ALLOCATION_BUFFER)>5:
# GlobalVals.RSSI_DATA_ALLOCATION_BUFFER.pop(0)
GlobalVals.RSSI_DATA_ALLOCATION_BUFFER.append(
int(RSSI_AllocationData.Pair))
# set the flags for the buffer
with GlobalVals.RECIEVED_RSSI_ALLOCATION_RADIO_DATA_MUTEX:
GlobalVals.RECIEVED_RSSI_ALLOCATION_RADIO_DATA = True
continue
def TemperatureLoggerSocket():
try:
socket_logger = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
socket_logger.connect((GlobalVals.HOST, GlobalVals.TEMP_LOGGER_SOCKET))
socket_logger.settimeout(GlobalVals.TEMP_LOGGER_SOCKET_TIMEOUT)
except Exception as e:
print("Exception: " + str(e.__class__))
print(
"There was an error starting the TemperatureLoggerSocket logger socket. This thread will now stop."
)
with GlobalVals.BREAK_TEMP_LOGGER_THREAD_MUTEX:
GlobalVals.BREAK_TEMP_LOGGER_THREAD = True
return
bufferRead = 1024
breakMainThread = False
while True:
# print("TEMPERATURE SOCKET")
# if flag is set break the thread
with GlobalVals.BREAK_TEMP_LOGGER_THREAD_MUTEX:
if GlobalVals.BREAK_TEMP_LOGGER_THREAD:
break
# read the socket
while True:
try:
data_bytes = socket_logger.recv(bufferRead)
break
except Exception as e:
if e.args[0] == 'timed out':
print("temperature Socket receive timed out. Retrying...")
time.sleep(0.1)
continue
else:
print("Temperature Socket Connection error.")
breakMainThread = True
break
if breakMainThread:
break
# if there is nothing in the socket then it has timed out
if len(data_bytes) == 0:
continue
data_str = data_bytes.decode('utf-8')
string_list = extract_str_btw_curly_brackets(data_str)
if len(string_list) > 0:
temperature_list = []
for string in string_list:
received, temp_i = stringToTemperature(string)
if received:
temperature_list.append(temp_i)
idx = 0
while idx < len(temperature_list):
temperatureUpdate(temperature_list[idx])
idx += 1
temp_i = GlobalVals.TEMPERATURE_ALL
TempData = CustMes.MESSAGE_TEMP()
TempData.Temperature = temp_i.temperature
TempData.Epoch = temp_i.epoch
TempData.SystemID = temp_i.sysID
# add data to the gps buffer
# with GlobalVals.EKF_GPS_DATA_BUFFER_MUTEX:
# set the flag for the data
# with GlobalVals.RECIEVED_EKF_GPS_LOCAL_DATA_MUTEX:
# GlobalVals.RECIEVED_EKF_GPS_LOCAL_DATA = True
# send GPS data to other balloons
TempPacket = CustMes.MESSAGE_FRAME()
TempPacket.SystemID = GlobalVals.SYSTEM_ID
TempPacket.MessageID = 6
TempPacket.TargetID = 0
TempPacket.Payload = TempData.data_to_bytes()
NetworkManager.sendPacket(TempPacket)
# print(TempData)
# print("***************************")
# pause a little bit so the mutexes are not getting called all the time
time.sleep(3)
socket_logger.close()
return
def IMULoggerSocket():
# set up socket
try:
socket_logger = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
socket_logger.bind((GlobalVals.HOST, GlobalVals.IMU_LOGGER_SOCKET))
socket_logger.settimeout(GlobalVals.GPS_LOGGER_SOCKET_TIMEOUT)
except Exception as e:
print("Exception: " + str(e.__class__))
print(
"There was an error starting the IMU logger socket. This thread will now stop."
)
with GlobalVals.BREAK_IMU_LOGGER_THREAD_MUTEX:
GlobalVals.BREAK_IMU_LOGGER_THREAD = True
return
# intialize variables
synced = False
syncA = False
syncB = False
bufferRead = 1
while True:
# if flag is set break the thread
with GlobalVals.BREAK_IMU_LOGGER_THREAD_MUTEX:
if GlobalVals.BREAK_IMU_LOGGER_THREAD:
break
# reset buffer read when not synced
if not synced:
bufferRead = 1024
# read the socket
try:
data_bytes, server = socket_logger.recvfrom(bufferRead)
# print(data_bytes)
except:
print("IMULoggerSocket(): Receive Connection error.")
break
# if there is nothing in the socket then it has timed out
if len(data_bytes) == 0:
continue
raw_data = data_bytes.decode('utf-8')
# for the sync bytes (0xAA 0x55)
if not synced:
if raw_data.find("{") == -1 or raw_data.find("}") == -1:
synced = False
continue
else:
synced = True
raw_data = extract_string_data("{", "}", raw_data)
# once it is scyned read the rest of the data
if synced:
# store converted values
Epoch = float(extract_string_data("EPOCH:", ";", raw_data))
Acceleration_i = convert_to_array(
extract_string_data("ACCELERATION:", ";", raw_data))[0]
Acceleration_j = convert_to_array(
extract_string_data("ACCELERATION:", ";", raw_data))[1]
Acceleration_k = convert_to_array(
extract_string_data("ACCELERATION:", ";", raw_data))[2]
MagneticVector_i = convert_to_array(
extract_string_data("MAGNETIC_VECTOR:", ";", raw_data))[0]
MagneticVector_j = convert_to_array(
extract_string_data("MAGNETIC_VECTOR:", ";", raw_data))[1]
MagneticVector_k = convert_to_array(
extract_string_data("MAGNETIC_VECTOR:", ";", raw_data))[2]
RawQT_w = convert_to_array(
extract_string_data("RAW_QT:", ";", raw_data))[0]
RawQT_i = convert_to_array(
extract_string_data("RAW_QT:", ";", raw_data))[1]
RawQT_j = convert_to_array(
extract_string_data("RAW_QT:", ";", raw_data))[2]
RawQT_k = convert_to_array(
extract_string_data("RAW_QT:", ";", raw_data))[3]
Euler321_psi = convert_to_array(
extract_string_data("EULER_321:", "}", raw_data))[0]
Euler321_theta = convert_to_array(
extract_string_data("EULER_321:", "}", raw_data))[1]
Euler321_phi = convert_to_array(
extract_string_data("EULER_321:", "}", raw_data))[2]
Gyroscope_i = convert_to_array(
extract_string_data("GYRO:", ";", raw_data))[0]
Gyroscope_j = convert_to_array(
extract_string_data("GYRO:", ";", raw_data))[1]
Gyroscope_k = convert_to_array(
extract_string_data("GYRO:", ";", raw_data))[2]
# print(Euler321_phi)
# print("============================")
# Debug message
#print(str(GPSTime) + "," + str(Longitude) + "," + str(Latitude) + "," + str(Altitude) + "\n")
# use GPS message payload to store value
IMUData = CustMes.MESSAGE_GPS()
IMUData.EpochTuple = Epoch
IMUData.Acceleration_i = Acceleration_i
IMUData.Acceleration_j = Acceleration_j
IMUData.Acceleration_k = Acceleration_k
IMUData.MagneticVector_i = MagneticVector_i
IMUData.MagneticVector_j = MagneticVector_j
IMUData.MagneticVector_k = MagneticVector_k
IMUData.RawQT_w = RawQT_w
IMUData.RawQT_i = RawQT_i
IMUData.RawQT_j = RawQT_j
IMUData.RawQT_k = RawQT_k
IMUData.Euler321_psi = Euler321_psi
IMUData.Euler321_theta = Euler321_theta
IMUData.Euler321_phi = Euler321_phi
IMUData.Gyroscope_i = Gyroscope_i
IMUData.Gyroscope_j = Gyroscope_j
IMUData.Gyroscope_k = Gyroscope_k
IMUData.SystemID = GlobalVals.SYSTEM_ID
# add data to the gps buffer
with GlobalVals.IMU_DATA_BUFFER_MUTEX:
GlobalVals.IMU_DATA_BUFFER.append(IMUData)
# set the flag for the data
with GlobalVals.RECIEVED_IMU_LOCAL_DATA_MUTEX:
GlobalVals.RECIEVED_IMU_LOCAL_DATA = True
# send GPS data to other balloons
IMUPacket = CustMes.MESSAGE_FRAME()
IMUPacket.SystemID = GlobalVals.SYSTEM_ID
IMUPacket.MessageID = 4
IMUPacket.TargetID = 0
IMUPacket.Payload = IMUData.data_to_bytes()
NetworkManager.sendPacket(IMUPacket)
# reset
synced = False
# pause a little bit so the mutexes are not getting called all the time
time.sleep(0.01)
socket_logger.close()
return