from aux_functions import nav_transformations as nav import pynmea2 import serial import json import numpy as np from aux_functions import network_functions as nf from aux_functions import nav_transformations as nt from aux_functions import other_math as om from aux_functions import matrix_math as mm import time as tm import math from gpiozero import LED from smbus2 import SMBus from pymavlink import mavutil #comment line to verify that pull is working #Configure fuel light pin fuelLamp = LED(17) #Fuel tank I2C fuelI2Caddress = 0x48 #ADDR pin connected to GND #Read positon from auxillary GPS def get_aux_posn(): ser = serial.Serial('/dev/ttyAMA5', 9600, timeout=0.7) gps_posn = np.zeros(3) line = ser.readline().decode('utf-8') while (not(line.startswith("$GNGGA") or line.startswith("$GPGGA"))): line = ser.readline().decode('utf-8') msg = pynmea2.parse(line) gps_posn[0] = msg.latitude gps_posn[1] = msg.longitude gps_posn[2] = msg.altitude return gps_posn #Save parameter def param_to_file(str_state_name, state_val): if (str_state_name == "land_base"): fname = "land_base.txt" elif (str_state_name == "land_location"): fname = "land_location.txt" elif (str_state_name == "route_id"): fname = "route_id.txt" elif (str_state_name == "odid_data"): fname = "odid_data.txt" elif (str_state_name == "land_clearance"): fname = "land_clearance.txt" elif (str_state_name == "data2fc"): fname = "data2fc.txt" if ((str_state_name == "land_location") or (str_state_name == "odid_data") or (str_state_name == "data2fc")): state_val_list = state_val with open(fname, 'w', encoding="utf-8") as f_handle: json.dump(state_val_list, f_handle) if ((str_state_name == "route_id") or (str_state_name == "land_base") or (str_state_name == "land_clearance")): with open(fname, 'w', encoding="utf-8") as f_handle: json.dump(state_val, f_handle) if (str_state_name == "parsed_response"): parsed_wp_w = state_val['waypoints'] parsed_wp_list = parsed_wp_w parsed_wp_sz_w = state_val['waypoints_size'] parsed_wp_sz_list = parsed_wp_sz_w parsed_vt_w = state_val['velocities'] parsed_vt_list = repr(parsed_vt_w) parsed_vt_sz = state_val['velocities_size'] parsed_vt_sz_list = repr(parsed_vt_sz) with open('parsed_wp.txt', 'w', encoding="utf-8") as f_handle1: json.dump(parsed_wp_list, f_handle1) with open('parsed_wp_sz.txt', 'w', encoding="utf-8") as f_handle2: json.dump(parsed_wp_sz_list, f_handle2) with open('parsed_vt.txt', 'w', encoding="utf-8") as f_handle3: json.dump(parsed_vt_list, f_handle3) with open('parsed_vt_sz.txt', 'w', encoding="utf-8") as f_handle4: json.dump(parsed_vt_sz_list, f_handle4) #Read parameter def param_from_file(str_state_name): if (str_state_name == "land_base"): fname = "land_base.txt" elif (str_state_name == "land_location"): fname = "land_location.txt" elif (str_state_name == "route_id"): fname = "route_id.txt" elif (str_state_name == "odid_data"): fname = "odid_data.txt" elif (str_state_name == "land_clearance"): fname = "land_clearance.txt" elif (str_state_name == "data2fc"): fname = "data2fc.txt" if ((str_state_name == "route_id") or (str_state_name == "land_base") or (str_state_name == "odid_data") or (str_state_name == "land_clearance") or (str_state_name == "data2fc")): with open(fname, 'r', encoding="utf-8") as f_handle: result = json.load(f_handle) return result if (str_state_name == "land_location"): with open(fname, 'r', encoding="utf-8") as f_handle: result_i = json.load(f_handle) result = np.array(result_i) return result if (str_state_name == "parsed_response"): with open("parsed_wp.txt", 'r', encoding="utf-8") as f_handle1_r: parsed_wp_r = np.array(json.load(f_handle1_r)) with open("parsed_wp_sz.txt", 'r', encoding="utf-8") as f_handle2_r: parsed_wp_sz_r = np.array(json.load(f_handle2_r)) with open("parsed_vt.txt", 'r', encoding="utf-8") as f_handle3_r: parsed_vt_r = np.array(eval(json.load(f_handle3_r))) with open("parsed_vt_sz.txt", 'r', encoding="utf-8") as f_handle4_r: parsed_vt_sz_r = np.array(eval(json.load(f_handle4_r))) #result = {'waypoints': parsed_wp_r, 'waypoints_size': parsed_wp_sz_r, 'velocity_and_time': parsed_vt_r} result = {'waypoints': parsed_wp_r, 'waypoints_size': parsed_wp_sz_r, 'velocities': parsed_vt_r, 'velocities_size': parsed_vt_sz_r} return result return -1 #Calculate the relative poisition of the drone from the landing target def landing_target_posn_estimate(ekf_origin, ahrs_posn_estimate): #retrieve land location information land_base = param_from_file("land_base") filed_land_posn = param_from_file("land_location") if (land_base): planned_land_posn = nf.get_live_posn(land_base) aux_gps_posn = get_aux_posn() aux_gps_posn_NED = nav.lla2ned(aux_gps_posn, ekf_origin) ahrs_posn_NED = nav.lla2ned(ahrs_posn_estimate, ekf_origin) planned_land_posn_NED = nav.lla2ned(planned_land_posn, ekf_origin) aux_posn_err = planned_land_posn_NED - aux_gps_posn_NED true_land_posn_NED = aux_posn_err + ahrs_posn_NED else: planned_land_posn = filed_land_posn true_land_posn_NED = nav.lla2ned(planned_land_posn, ekf_origin) return true_land_posn_NED #parse logon response def parse_logon_response(logon_response): #waypoints waypoints = logon_response['waypoints'] num_waypoints = logon_response['num_waypoints'] velocities = logon_response['velocities'] velocities_size = [2, num_waypoints] waypoints_size = [4, num_waypoints] result = {'waypoints': waypoints, 'waypoints_size': waypoints_size, 'velocities': velocities, 'velocities_size': velocities_size} return result #Calculate reference state vector def calc_ref_velocity(parsed_logon_response): curr_time = tm.time() time_ref = curr_time velocities_mat = parsed_logon_response['velocities'] velocities_mat_size = parsed_logon_response['velocities_size'] if (time_ref > mm.select_element(velocities_mat, 1, (velocities_mat_size[1] - 1), velocities_mat_size[0])): velocities_ref = mm.select_element(velocities_mat, 0, (velocities_mat_size[1] - 1), velocities_mat_size[0]) else: #position reference for idx in range(velocities_mat_size[1]): if (mm.select_element(velocities_mat, 1, idx, velocities_mat_size[0]) > time_ref): velocityBefore = mm.select_array(velocities_mat, [0, 1], [(idx - 1), (idx - 1)], velocities_mat_size[0]) velocityAfter = mm.select_array(velocities_mat, [0, 1], [idx, idx], velocities_mat_size[0]) velocities_ref = om.interpolateBetweenPoints(velocityBefore[1], velocityBefore[0], velocityAfter[1], velocityAfter[0], time_ref) break return velocities_ref #Generate obstacle distance mavlink message def genObstacleDistance(drone_obj, odid_lla, time_stamp): #obtain drone position global_posn = drone_obj.recv_match(condition=None,type='GLOBAL_POSITION_INT') lla0 = [(global_posn.lat / (10 ** 7)), (global_posn.lon / (10 ** 7)), (global_posn.alt / (10 ** 3))] #compute distance and angle to intruder odid_ned = nav.lla2ned(odid_lla, lla0) angleFromDrone = math.degrees(math.atan2(odid_ned[1], odid_ned[0])) distFromDrone = math.floor(math.sqrt((odid_ned[0] ** 2) + (odid_ned[1] ** 2) + (odid_ned[2] ** 2))) #send info to drone distPosnInSequence = math.ceil(angleFromDrone / 5) distances = [0] * 72 distances[distPosnInSequence] = distFromDrone drone_obj.mav.obstacle_distance_send( time_usec = time_stamp, sensor_type = 4, #MAV_DISTANCE_SENSOR_UNKNOWN distances = distances, increment = 5, min_distance = 0, max_distance = 1000, increment_f = 5, angle_offset = 0, frame = 12, #MAV_FRAME_BODY_FRD force_mavlink1 = False ) #Process fuel tank readings def processFuelRaw(tankRaw): tankLevel = tankRaw return tankLevel #Take fuel readings from I2C device def takeFuelReading(): fuelBus = SMBus(1) #Create I2C object conversionRegVal = 0 #address pointer register value for conversion register configRegVal = 1 #addresss pointer register value for config register AN0Config0 = 193 AN1Config0 = 209 AN2Config0 = 225 AN3Config0 = 241 ANConfig1 = 131 #AN0 #Write to config register (begin conversion) data0 = [AN0Config0, ANConfig1] fuelBus.write_i2c_block_data(fuelI2Caddress, configRegVal, data0) #write to address pointer register (Select conversion register) fuelBus.write_byte_data(fuelI2Caddress, 0, conversionRegVal) #read conversion register tankSeq0 = fuelBus.read_i2c_block_data(fuelI2Caddress, 0, 2) tankRaw0 = 256 * tankSeq0[0] + tankSeq0[1] tankLevel0 = processFuelRaw(tankRaw0) #AN1 #Write to config register (begin conversion) data1 = [AN1Config0, ANConfig1] fuelBus.write_i2c_block_data(fuelI2Caddress, configRegVal, data1) #write to address pointer register (Select conversion register) fuelBus.write_byte_data(fuelI2Caddress, 0, conversionRegVal) #read conversion register tankSeq1 = fuelBus.read_i2c_block_data(fuelI2Caddress, 0, 2) tankRaw1 = 256 * tankSeq1[0] + tankSeq1[1] tankLevel1 = processFuelRaw(tankRaw1) #AN2 #Write to config register (begin conversion) data2 = [AN2Config0, ANConfig1] fuelBus.write_i2c_block_data(fuelI2Caddress, configRegVal, data2) #write to address pointer register (Select conversion register) fuelBus.write_byte_data(fuelI2Caddress, 0, conversionRegVal) #read conversion register tankSeq2 = fuelBus.read_i2c_block_data(fuelI2Caddress, 0, 2) tankRaw2 = 256 * tankSeq2[0] + tankSeq2[1] tankLevel2 = processFuelRaw(tankRaw2) #AN3 #Write to config register (begin conversion) data3 = [AN3Config0, ANConfig1] fuelBus.write_i2c_block_data(fuelI2Caddress, configRegVal, data3) #write to address pointer register (Select conversion register) fuelBus.write_byte_data(fuelI2Caddress, 0, conversionRegVal) #read conversion register tankSeq3 = fuelBus.read_i2c_block_data(fuelI2Caddress, 0, 2) tankRaw3 = 256 * tankSeq3[0] + tankSeq3[1] tankLevel3 = processFuelRaw(tankRaw3) tankLevels = [tankLevel0, tankLevel1, tankLevel2, tankLevel3] return tankLevels #Check fuel level def fuelCheck(): #turn on fuel light fuelLamp.on() #take readings tankLevel = min(takeFuelReading()) #turn off fuel lamp fuelLamp.off() #Log fuel quantity nf.log_fuel(tankLevel) print(tankLevel) #Send Mavlink message to flight controller def mavlinkCmdSend(mavlink_obj, cmdID, cmdParams): #encode the message message = mavlink_obj.mav.command_long_encode( mavlink_obj.target_system, # Target system ID mavlink_obj.target_component, # Target component ID cmdID, # ID of command to send 0, # Confirmation cmdParams[0], # param1 cmdParams[1], # param2 cmdParams[2], # param3 cmdParams[3], # param4 cmdParams[4], # param5 cmdParams[5], # param6 cmdParams[6] # param7 ) # Send the COMMAND_LONG mavlink_obj.mav.send(message) # Wait for a response (blocking) to the command response = mavlink_obj.recv_match(type='COMMAND_ACK', blocking=True) if response and response.command == cmdID and ((response.result == mavutil.mavlink.MAV_RESULT_ACCEPTED) or (response.result == mavutil.mavlink.MAV_RESULT_IN_PROGRESS)): return 0 else: return 1 #Create ADS-B message from traffic data and send to drone def traffic2adsb(droneObj, traffic_data): for trafficElement in traffic_data: droneObj.mav.adsb_vehicle_send( int(trafficElement["hex"], 16), #ICAO address int(trafficElement["lat"] * 1E7), int(trafficElement["lon"] * 1E7), 0, #altitude type (QNH) int(trafficElement["alt"] * 304.8), int(trafficElement["track"] * 100), int(trafficElement["gspeed"] * 51.4444), int(trafficElement["vspeed"] * 0.508), trafficElement["callsign"], 0, #emitter type (no info) 0, #time since last communication (1 | 2 | 4 | 8 | 16 | 32 | 128 | 256), int(trafficElement["squawk"], 8), )