def main():
target_file = open("target_positions_gps.txt")
# Recieve GPS target position
line = target_file.readline()
if line != "":
t, lat, long = line.split(",")
target_loc = GPSCoord.from_nmea(lat.strip(), long.strip())
while True:
time.sleep(1)
# Recieve GPS target position
line = target_file.readline()
if line != "":
t, lat, long = line.split(",")
else:
target_file.close
break
prev_target_loc = target_loc
target_loc = GPSCoord.from_nmea(lat.strip(), long.strip())
dest = swarming_checks(drones, target_loc, prev_target_loc)
if dest != None:
print("target lat:{} long:{} dest lat:{} long:{}".format(
target_loc.lat, target_loc.long, dest.lat, dest.long))
def read_coordinate(self):
""" Reads the coordinates from the next line of the given file, and returns
it as a GPSCoord. Each line of the file contains time, latitude and
longitude in NMEA format.
"""
time, latitude, longitude = self.target_pos_file.readline().split(",")
return GPSCoord.from_nmea(latitude.strip(), longitude.strip())
def main():
# Socket to send readings to the client.
context = zmq.Context()
socket = context.socket(zmq.PUB)
socket.bind("tcp://*:{}".format(PORT))
storage = open("gps_storage.txt", "r")
gps_data = storage.readlines()
for line in gps_data:
# Change this to readings from the GPS module.
# time_sample = "Sat Aug 3 14:21:15 2019"
# latitude = "4331.23049S"
# longitude = "17234.98533E"
splitline = line.split(",")
time_sample = splitline[0]
latitude_str = splitline[1]
longitude_str = splitline[2]
coord = GPSCoord(float(latitude_str), float(longitude_str))
print_message = "{},{:.5f},{:.5f}".format(time_sample, coord.lat, coord.long)
message = "{},{:.5f},{:.5f}".format(time_sample, coord.lat, coord.long)
print("Sending: {}".format(print_message))
socket.send(message.encode('utf-8'))
time.sleep(1)
storage.close()
def destination(swarming_state, target_coords, est_centre):
""" Ouput destination depending fsm state """
if swarming_state == 0:
output_dest = target_coords
elif swarming_state == 1:
# desination average centre of the drones to reset formation
output_dest = est_centre
elif swarming_state == 2:
# If formation is critical stop the drones
# TODO: Set drone mode to hold (pixhawk)
output_dest = GPSCoord(-1, -1)
return output_dest
def main():
#Determine size of grid (metres), create grid (using smaller grid here for ease of testing)
xRange = 500
yRange = 500
grid = createGrid(xRange, yRange)
#Transmitter drone always centre of grid
Tx = (xRange / 2, yRange / 2)
#Set conditions for localisation testing
TxCoord = GPSCoord(-43.52051, 172.58310)
Rx1Coord = GPSCoord(-43.52046, 172.58305)
Rx2Coord = GPSCoord(-43.52046, 172.58315)
Rx3Coord = GPSCoord(-43.52056, 172.58305)
targetCoord = GPSCoord(-43.52059, 172.58315)
#Map from GPS to grid locations
Rx1 = (calcX(Rx1Coord, TxCoord, Tx), calcY(Rx1Coord, TxCoord, Tx))
Rx2 = (calcX(Rx2Coord, TxCoord, Tx), calcY(Rx2Coord, TxCoord, Tx))
Rx3 = (calcX(Rx3Coord, TxCoord, Tx), calcY(Rx3Coord, TxCoord, Tx))
target = (calcX(targetCoord, TxCoord, Tx), calcY(targetCoord, TxCoord, Tx))
grid = placeObjects(grid, Tx, Rx1, Rx2, Rx3, target)
#calculate true ranges, in practical test this will be given as the output of the SDR
r1 = findNorm(Tx, target) + findNorm(Rx1, target)
r2 = findNorm(Tx, target) + findNorm(Rx2, target)
r3 = findNorm(Tx, target) + findNorm(Rx3, target)
rangeArray = [r1, r2, r3, None, None]
RxCoordArray = [Rx1Coord, Rx2Coord, Rx3Coord, None, None]
#Perform Calculations
result = estimate_target_position(TxCoord, RxCoordArray, rangeArray, 3)
print(result)
print(grid)
def main():
#Determine size of grid (metres), create grid (using smaller grid here for ease of testing)
xRange = 500
yRange = 500
grid = createGrid(xRange, yRange)
#Transmitter drone always centre of grid
Tx = (xRange / 2, yRange / 2)
#Set conditions for localisation testing
TxCoord = GPSCoord(-43.52051, 172.58310)
Rx1Coord = GPSCoord(-43.52046, 172.58305)
Rx2Coord = GPSCoord(-43.52046, 172.58315)
Rx3Coord = GPSCoord(-43.52056, 172.58305)
targetCoord = GPSCoord(-43.52059, 172.58315)
#Map from GPS to grid locations
Rx1 = (calcX(Rx1Coord, TxCoord, Tx), calcY(Rx1Coord, TxCoord, Tx))
Rx2 = (calcX(Rx2Coord, TxCoord, Tx), calcY(Rx2Coord, TxCoord, Tx))
Rx3 = (calcX(Rx3Coord, TxCoord, Tx), calcY(Rx3Coord, TxCoord, Tx))
target = (calcX(targetCoord, TxCoord, Tx), calcY(targetCoord, TxCoord, Tx))
grid = placeObjects(grid, Tx, Rx1, Rx2, Rx3, target)
#calculate true ranges, in practical test this will be given as the output of the SDR
r1 = findNorm(Tx, target) + findNorm(Rx1, target)
r2 = findNorm(Tx, target) + findNorm(Rx2, target)
r3 = findNorm(Tx, target) + findNorm(Rx3, target)
result = bruteForce(grid, Tx, Rx1, Rx2, Rx3, r1, r2, r3)
#convert back to GPS coord
result = cartesianToLatLong(result, Tx, TxCoord)
print(result)
def main():
target_file = open("target_positions_gps.txt")
while True:
time.sleep(1)
# Recieve GPS target position
line = target_file.readline()
if line != "":
t, lat, long = line.split(",")
else:
target_file.close
break
# Output desired position
target_loc = GPSCoord.from_nmea(lat.strip(), long.strip())
desired_loc = swarming_logic.update_loc(target_loc, drone_num)
print("Target Loc: {} Desired Loc: {}".format(target_loc, desired_loc))
def gps_buffer(buffer, gps, prev_avg):
""" Given a list of GPSCoords, a new GPSCoord value, and the previous buffer average
the circular buffer is checked for errors and updated.
The updated gps average and buffer are outputed """
tot_lat = 0
tot_long = 0
value = gps_bound(gps, prev_avg)
if value == -1:
print("GPS_BOUNDARY_ERROR")
else:
buffer.append(value)
buffer.pop(0)
for gps in buffer:
tot_lat += gps.lat
tot_long += gps.long
gps_avg = GPSCoord(tot_lat / len(buffer), tot_long / len(buffer))
return gps_avg, buffer
def mean_centre(centres):
""" Returns the mean average GPSCoord of the centres list, and returns the
error; the grestest distance in m from the mean to a value in centres """
lat_sum = 0
long_sum = 0
error = 0
n = 0
for centre in centres:
lat_sum += centre.lat
long_sum += centre.long
n += 1
if lat_sum != 0 and long_sum != 0:
mean_centre = GPSCoord(lat_sum / n, long_sum / n)
# Find error in mean centre
for centre in centres:
if centre.distance(mean_centre) > error:
error = centre.distance(mean_centre)
return mean_centre, error
def main():
# Socket to send readings to the client.
socket = init_socket()
ser = serial.Serial('../../../../../../../dev/tty.usbserial',
4800,
timeout=5)
line = ser.readline(
) # Read remaining junk line so that next line is clean
while True:
# Change this to readings from the GPS module.
# time_sample = "Sat Aug 3 14:21:15 2019"
# latitude = "4331.23049S"
# longitude = "17234.98533E"
line = ser.readline().decode('utf-8')
splitline = line.split(",")
if splitline[0] == GPGGA:
date = time.localtime(time.time())
gps_time = "{}:{}:{}".format(splitline[1][:2], splitline[1][2:4],
splitline[1][4:-4])
time_sample = "{} {} {} {} {}".format(WEEKDAYS[date.tm_wday],
MONTHS[date.tm_mon - 1],
date.tm_mday, gps_time,
date.tm_year)
latitude_nmea = splitline[2] + splitline[3]
longitude_nmea = splitline[4] + splitline[5]
coord = GPSCoord.from_nmea(latitude_nmea, longitude_nmea)
print_message = "{},{:.5f},{:.5f}".format(time_sample, coord.lat,
coord.long)
message = "{},{:.5f},{:.5f}".format(time_sample, coord.lat,
coord.long)
storage = open("gps_storage.txt", "w")
storage.write(message + '\n')
print("Sending: {}".format(print_message))
# Send new gps data to NUC
socket.send(message.encode('utf-8'))
import swarming_logic from gps import GPSCoord from packets import RxUpdate, TxUpdate from mavros_offboard_posctl import MavrosOffboardPosctl # Must match the port numbers in rx.py TX_STARTUP_PORT = 5554 TX_RECEIVE_PORT = 5555 TX_SEND_PORT = 5556 # This must match the value in rx.py, and be compatible with the swarming logic # and the multilateration. NUM_RXS = 4 # Tx starting position. This value must match the one in rx.py TX_START_COORDS = GPSCoord(-43.520508, 172.583089) # Latitude and logtitude limits for the expected drone and target coordinates. # Used to define the boundaries of the plot. MIN_LAT = -43.520833 MAX_LAT = -43.520333 MIN_LONG = 172.582833 MAX_LONG = 172.583167 # The drone ID of the transmitter drone. TX_ID = 0 # How often the Tx should perform multilateration and send an update. # TODO: not currently used, instead the Tx just performs multilateration # whenever a new full group of readings has been received. # UPDATE_PERIOD_S = 1
from gps import GPSCoord
from packets import RxUpdate, TxUpdate
import swarming_logic
TX_IP_ADDRESS = "localhost"
# Must match the port numbers in tx.py
TX_STARTUP_PORT = 5554
TX_RECEIVE_PORT = 5555
TX_SEND_PORT = 5556
TARGET_POSITION_FILENAME = "target_positions_gps.txt"
# Tx starting position. This value must match the one in tx.py
TX_START_COORDS = GPSCoord(-43.520508, 172.583089)
# This must match the value in tx.py, and be compatible with the swarming logic
# and the multilateration.
NUM_RXS = 4
# Rx starting positions, length must be equal to NUM_RXs.
# The Rx with ID 1 is assigned the first position in the list, and so on.
RX_START_COORDS = [
GPSCoord(-43.520463, 172.583027),
GPSCoord(-43.520463, 172.583151),
GPSCoord(-43.520553, 172.583027),
GPSCoord(-43.520553, 172.583151)
]
# How often the Rx should send updates to the Tx.
def from_bytes(b):
target_lat, target_long, tx_lat, tx_long = struct.unpack("!dddd", b)
return TxUpdate(GPSCoord(target_lat, target_long),
GPSCoord(tx_lat, tx_long))
def from_bytes(b):
(rx_id, timestamp, seq_no, lat, lon, range_reading, target_lat,
target_long) = struct.unpack("!ididdddd", b)
return RxUpdate(rx_id, timestamp, seq_no, GPSCoord(lat, lon),
range_reading, GPSCoord(target_lat, target_long))
"""
tx_swarm.py
Transmitter drone swarming logic (mothership)
With the input of a target location (currently from target_positions_gps.txt),
outputs the desired drone location
"""
import time
import swarming_logic
from gps import GPSCoord
# assign the drone a number
drone_num = 0
drones = [
GPSCoord(-43.520508, 172.583089),
GPSCoord(-43.520363, 172.583027),
GPSCoord(-43.520463, 172.583151),
GPSCoord(-43.520553, 172.583027),
GPSCoord(-43.520553, 172.583151)
]
# Drone positions in formation about the first point in target_positions_gps.txt
#drones = [GPSCoord(-43.520508, 172.583089),
#GPSCoord(-43.520463, 172.583027),
#GPSCoord(-43.520463, 172.583151),
#GPSCoord(-43.520553, 172.583027),
#GPSCoord(-43.520553, 172.583151)]
def swarming_checks(drones, target_coords, previous_target_coords):
""" Return the desired centre position of the formation. If formation is