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