'''A general class for redirecting I/O to this Text widget.'''

def __init__(self,UDP_server_Report):

self.UDP_server_Report = UDP_server_Report

'''A class for redirecting stdout to this Text widget.'''

def write(self, msg):

sys.__stdout__.write(msg)

if msg != '\n':

self.UDP_server_Report.send_report(str(msg))

def flush(self):

'''A class for redirecting stdout to this Text widget.'''

def write(self, msg):

sys.__stderr__.write(msg)

if msg != '\n':

self.UDP_server_Report.send_report(str(msg))

def flush(self):

def __init__(self, root, vehicle, sitl, vehicle_controll, close_all, *args, **kwargs):

self.vehicle = vehicle

self.sitl = sitl

self.vehicle_controll = vehicle_controll

self.close_all = close_all

print "Drone: GUI - Start"

tk.Frame.__init__(self, root, *args, **kwargs)

self.root = root

self.frame1 = tk.Frame(self.root)

self.root.title("drone GUI")

self.frame1.configure(background='white')

self.GUI_init(self.frame1)

self.frame1.grid(row=0, column=0)

self.root.protocol('WM_DELETE_WINDOW', self.on_window_close)

self.root.after(100,self.GUI_tick)

def GUI_init(self, frame):

print "Drone: GUI - Init objects"

#0

self.lbl_title = tk.Label(frame, text='Mission controllsky - drone side' ,font=('arial', 16, 'bold'), fg='red',bg='white')

self.lbl_title.grid(row=0, column=0, columnspan=6)

self.btn_close = tk.Button(frame, text='Close', width=25, command= self.on_btn_close)

self.btn_close.grid(row=0, column=6, columnspan=1)

#1

self.lbl_command = tk.Label(frame, text='Command:', fg='black',bg='white')

self.lbl_command.grid(row=1, column=0, columnspan=1)

self.ent_command = tk.Entry(frame)

self.ent_command.grid(row=1, column=1)

self.lbl_command_param1 = tk.Label(frame, text='param1:', fg='black',bg='white')

self.lbl_command_param1.grid(row=1, column=2, columnspan=1)

self.ent_command_param1 = tk.Entry(frame)

self.ent_command_param1.grid(row=1, column=3)

self.lbl_command_param2 = tk.Label(frame, text='param2:', fg='black',bg='white')

self.lbl_command_param2.grid(row=1, column=4, columnspan=1)

self.ent_command_param2 = tk.Entry(frame)

self.ent_command_param2.grid(row=1, column=5)

self.btn_send = tk.Button(frame, text='Send command', command=self.on_btn_send)

self.btn_send.grid(row=1, column=6, columnspan=1)

#2

self.lbl_sent = tk.Label(frame, fg='black', bg='white', text='')

self.lbl_sent.grid(row=2, column=0, columnspan=1)

#3

self.btn_listen_keys = tk.Button(frame, fg='black', activebackground='red', bg='red', text='Listen keys - NO', width=25, command= self.on_btn_listen_keys)

self.btn_listen_keys.grid(row=3, column=0, columnspan=1)

self.btn_send_position = tk.Button(frame, fg='black', activebackground='red', bg='red', text='Send position - NO', width=25, command= self.on_btn_send_position)

self.btn_send_position.grid(row=3, column=1, columnspan=1)

#4

#self.root.bind("<Key>", self.key_callback)

self.counter = 0

self.send_move_0 = 0

self.key_pressed = None

def on_btn_send(self):

command = self.ent_command.get()

command_param1 = self.ent_command_param1.get()

command_param2 = self.ent_command_param2.get()

sent_command = "Sending: {" + command + " " + command_param1 + " " + command_param2 +"}"

self.lbl_sent.config(text = sent_command)

self.vehicle_controll.send_command(command, command_param1, command_param2)

def on_window_close(self):

print "Drone: Close all - GUI window close"

self.close_all()

def on_btn_close(self):

print "Drone: Close all - GUI button Close"

self.close_all()

def on_btn_listen_keys(self):

if self.btn_listen_keys.cget('bg') == "green":

self.root.unbind("<Key>")

self.root.unbind("<KeyRelease>")

self.lbl_listen_keys.config(text = "Listen keys - NO", activebackground='red', bg='red')

elif self.btn_listen_keys.cget('bg') == "red":

self.root.bind("<Key>", self.key_callback)

self.root.bind("<KeyRelease>", self.key_release_callback)

self.btn_listen_keys.config(text = "Listen keys - YES", activebackground='green', bg='green')

def on_btn_send_position(self):

if self.btn_send_position.cget('bg') == "green":

self.send_move_0 = 0

self.btn_send_position.config(text = "Send position - NO", activebackground='red', bg='red')

elif self.btn_send_position.cget('bg') == "red":

self.send_move_0 = 1

self.btn_send_position.config(text = "Send position - YES", activebackground='green', bg='green')

def GUI_close(self):

self.root.destroy()

self.root.quit()

def GUI_tick(self):

if (self.key_pressed == 'w'):

self.vehicle_controll.send_command("forward", 1)

elif (self.key_pressed == 'a'):

self.vehicle_controll.send_command("left", 1)

elif (self.key_pressed == 's'):

self.vehicle_controll.send_command("backward", 1)

elif (self.key_pressed == 'd'):

self.vehicle_controll.send_command("right", 1)

elif (self.key_pressed == None):

if (self.send_move_0 == 1):

self.vehicle_controll.send_command("move_0")

else:

print "Drone: WTF am I doing here?"

self.root.after(100, self.GUI_tick)

def key_release_callback(self, event):

self.key_pressed = None

def key_callback(self, event):

if self.vehicle_controll is None:

print "Drone: GUI - Warning: vehicle controll is None. Run telem module first."

print "Drone: GUI - ", repr(event.char), " pressed"

return

#print "pressed", repr(event.char)

if (event.char=='z'):

self.vehicle_controll.send_command("arm", 10)

if (event.char=='w'):

self.key_pressed = event.char

#self.vehicle_controll.send_command("forward", 20, 1)

elif (event.char=='a'):

self.key_pressed = event.char

#self.vehicle_controll.send_command("left", 20, 1)

elif (event.char=='s'):

self.key_pressed = event.char

#self.vehicle_controll.send_command("backward", 20, 1)

elif (event.char=='d'):

self.key_pressed = event.char

#self.vehicle_controll.send_command("right", 20, 1)

elif (event.char=='q'):

print "Q pressed"

self.vehicle_controll.send_command("yaw_left", 10)

elif (event.char=='e'):

print "E pressed"

self.vehicle_controll.send_command("yaw_right", 10)

elif (event.char=='l'):

print "L pressed"

self.vehicle_controll.send_command("land", None)

elif (event.char=='t'):

print "T pressed"

self.vehicle_controll.send_command("triangle", None)

elif (event.char=='y'):

print "Y pressed"

self.vehicle_controll.send_command("triangle2", None)

elif (event.char=='u'):

print "U pressed"

self.vehicle_controll.send_command("square", None)

elif (event.char=='i'):

print "I pressed"

self.vehicle_controll.send_command("square2", None)

elif (event.char=='p'):

print "P pressed"

def __init__(self, GUI_enabled, Report_enabled):

try:

self.GUI_enabled = GUI_enabled

self.Report_enabled = Report_enabled

self.vehicle = None

self.sitl = None

self.GUI = None

self.vehicle_controll = None

self.UDP_server_Telem_Cmd = None

self.UDP_server_Report = None

#Set up option parsing to get connection string

self.parser = argparse.ArgumentParser(description='HUD module')

self.parser.add_argument('--connect', help="E.g. /dev/ttyACM0 or /dev/ttyUSB0,57600")

self.parser.add_argument('--gcs_ip', help="Mention the GCS ip to make a UDP connection with it")

self.parser.add_argument('--video_client_ip', help="Video transmit. Client's ip")

self.parser.add_argument('--video_client_port', help="Video transmit. Client's port")

self.parser.add_argument('--video_server_port', help="Video transmit. Server's port (sending video through this port)")

self.args = self.parser.parse_args()

print "Drone: Connect to FCU"

self.vehicle, self.sitl = self.connect2FCU()

if not self.args.gcs_ip:

# default - broadcast ip

self.gcs_ip = "255.255.255.255"

else:

self.gcs_ip = self.args.gcs_ip

if (Report_enabled is True):

print "Drone: open Report socket"

self.UDP_server_Report = UDP.UDP(1, "Drone Report", "0.0.0.0", 5100, self.gcs_ip, 6101)

# Start redirecting stdout to UDP:

print "Drone: redirecting stdout to UDP"

sys.stdout = StdoutRedirector(self.UDP_server_Report)

print "Drone: redirecting stderr to UDP"

sys.stderr = StderrRedirector(self.UDP_server_Report)

print "Drone: open Telemetry, commands socket"

self.UDP_server_Telem_Cmd = UDP.UDP(1, "Telem/Cmd", "0.0.0.0", 5000, self.gcs_ip, 6001)

self.vehicle_controll = drone_controll.vehicle_controll(self.vehicle, self.UDP_server_Report, self.UDP_server_Telem_Cmd)

print "Drone: Start receive commands thread"

self.UDP_server_Telem_Cmd.receive_loop_cmd_thread(self.vehicle_controll)

print "Drone: Start to listen for telemetry from the drone"

self.vehicle.add_attribute_listener('*', self.wildcard_callback)

if self.args.video_client_ip:

self.video_client_ip = self.args.video_client_ip

self.video_client_port = self.args.video_client_port

self.video_server_port = self.args.video_server_port

self.run_Video(self.vehicle_controll)

if (GUI_enabled is True):

self.run_GUI()

else:

# if GUI disabled - here is the inf. loop

self.no_GUI()

except KeyboardInterrupt:

print "Drone: Close all - keyboard interrupt in main"

self.close_all()

except:

traceback.print_exc()

def run_GUI(self):

self.root = tk.Tk()

self.GUI = GUI_main(self.root, self.vehicle, self.sitl, self.vehicle_controll, self.close_all)

try:

print "Drone: GUI - Enter the mainloop"

self.root.mainloop()

except(KeyboardInterrupt):

print "Drone: Close all - keyboard interrupt in GUI"

self.close_all()

except:

traceback.print_exc()

def no_GUI(self):

try:

print "Drone: Enter the infinite loop"

while(1):

time.sleep(1)

except(KeyboardInterrupt):

print "Drone: Close all - keyboard interrupt in infinite loop"

self.close_all()

except:

traceback.print_exc()

def run_Video(self, vehicle_controll):

from video_server import Video_Server

print "Drone: Video - Start"

print "Drone: Video - Sending to the client's ip: " + str(self.video_client_ip)

print "Drone: Video - Sending to the client's port: " + str(self.video_client_port)

print "Drone: Video - Sending through port: " + str(self.video_server_port)

# Insert the video code here

self.video_server = Video_Server(self.vehicle_controll, self.video_client_port, self.video_client_ip)

def connect2FCU(self):

connection_string = self.args.connect

sitl=None

if not self.args.connect:

print "The connect string was not specified. Running SITL!"

from dronekit_sitl import SITL

sitl = SITL()

sitl.download('copter', '3.3', verbose=True)

sitl_args = ['-I0', '--model', 'quad', '--home=-35.363261,149.165230,584,353']

sitl.launch(sitl_args, await_ready=True, restart=True)

connection_string = 'tcp:127.0.0.1:5760'

# Connect to the Vehicle.

# Set `wait_ready=True` to ensure default attributes are populated before `connect()` returns.

print "\nConnecting to vehicle on: %s" % connection_string

vehicle = connect(connection_string, wait_ready=True)

vehicle.wait_ready('autopilot_version')

# Get all vehicle attributes (state)

print "======================================================="

print " Autopilot Firmware version: %s" % vehicle.version

print " Major version number: %s" % vehicle.version.major

print " Minor version number: %s" % vehicle.version.minor

print " Patch version number: %s" % vehicle.version.patch

print " Release type: %s" % vehicle.version.release_type()

print " Release version: %s" % vehicle.version.release_version()

print " Stable release?: %s" % vehicle.version.is_stable()

'''

print " Autopilot capabilities"

print " Supports MISSION_FLOAT message type: %s" % vehicle.capabilities.mission_float

print " Supports PARAM_FLOAT message type: %s" % vehicle.capabilities.param_float

print " Supports MISSION_INT message type: %s" % vehicle.capabilities.mission_int

print " Supports COMMAND_INT message type: %s" % vehicle.capabilities.command_int

print " Supports PARAM_UNION message type: %s" % vehicle.capabilities.param_union

print " Supports ftp for file transfers: %s" % vehicle.capabilities.ftp

print " Supports commanding attitude offboard: %s" % vehicle.capabilities.set_attitude_target

print " Supports commanding position and velocity targets in local NED frame: %s" % vehicle.capabilities.set_attitude_target_local_ned

print " Supports set position + velocity targets in global scaled integers: %s" % vehicle.capabilities.set_altitude_target_global_int

print " Supports terrain protocol / data handling: %s" % vehicle.capabilities.terrain

print " Supports direct actuator control: %s" % vehicle.capabilities.set_actuator_target

print " Supports the flight termination command: %s" % vehicle.capabilities.flight_termination

print " Supports mission_float message type: %s" % vehicle.capabilities.mission_float

print " Supports onboard compass calibration: %s" % vehicle.capabilities.compass_calibration

print " Global Location: %s" % vehicle.location.global_frame

print " Global Location (relative altitude): %s" % vehicle.location.global_relative_frame

print " Local Location: %s" % vehicle.location.local_frame

print " Attitude: %s" % vehicle.attitude

print " Velocity: %s" % vehicle.velocity

print " GPS: %s" % vehicle.gps_0

print " Gimbal status: %s" % vehicle.gimbal

print " Battery: %s" % vehicle.battery

print " EKF OK?: %s" % vehicle.ekf_ok

print " Last Heartbeat: %s" % vehicle.last_heartbeat

print " Rangefinder: %s" % vehicle.rangefinder

print " Rangefinder distance: %s" % vehicle.rangefinder.distance

print " Rangefinder voltage: %s" % vehicle.rangefinder.voltage

print " Heading: %s" % vehicle.heading

print " Is Armable?: %s" % vehicle.is_armable

print " System status: %s" % vehicle.system_status.state

print " Groundspeed: %s" % vehicle.groundspeed # settable

print " Airspeed: %s" % vehicle.airspeed # settable

print " Mode: %s" % vehicle.mode.name # settable

print " Armed: %s" % vehicle.armed # settable

'''

print "======================================================="

return vehicle, sitl

def wildcard_callback(self, vehicle, attr_name, value):

#print "(%s): %s" % (attr_name,value)

data = None

if attr_name=="attitude":

#print"roll, pitch, yaw = {} {} {}".format(round(value.roll,2),round(value.pitch,2),round(value.yaw,2))

data = {'roll': round(value.roll,2), 'pitch': round(value.pitch,2), 'yaw': round(value.yaw,2)}

elif attr_name=="velocity":

#print "Vx, Vy, Vz = {} {} {}".format(value[0], value[1], value[2])

data = {'vx': value[0], 'vy': value[1], 'vz': value[2]}

elif attr_name=="rangefinder":

#print "Lidar {}".format(round(value.distance,2))

data = {'rangefinder': round(value.distance,2)}

elif attr_name=="location.global_relative_frame":

#print "location.global_relative_frame lat, lon, alt = {} {} {}".format(value.lat, value.lon, value.alt)

data = {'frame_gl_rel_lat': value.lat, 'frame_gl_rel_lon': value.lon, 'frame_gl_rel_alt': value.alt}

elif attr_name=="location.global_frame":

#print "location.global_frame lat, lon, alt = {} {} {}".format(value.lat, value.lon, value.alt)

data = {'frame_gl_lat': value.lat, 'frame_gl_lon': value.lon, 'frame_gl_alt': value.alt}

elif attr_name=="location.local_frame":

#print "location.local_frame north, east, down = {} {} {}".format(value.north, value.east, value.down)

data = {'frame_loc_north': round(value.north,3), 'frame_loc_east': round(value.east,3), 'frame_loc_down': round(value.down,3)}

elif attr_name=="location":

#print "location_all {}".format(dir(value))

#print "location_all {}".format(value)

pass

elif attr_name=="heading":

#print "Heading: {}".format(value)

data = {'heading': value}

elif attr_name=="airspeed":

#print "Airspeed: {}".format(value)

data = {'airspeed': round(value,3)}

elif attr_name=="groundspeed":

#print "Groundspeed: {}".format(value)

data = {'groundspeed': round(value,3)}

elif attr_name=="channels":

#print "Channels. 1: {}, 2: {}, 3: {}, 4: {}, 5: {}, 6: {}, 7: {}, 8: {}".format(value['1'], value['2'], value['3'], value['4'], value['5'], value['6'], value['7'], value['8'], )

data = {'ch1': value[1], 'ch2': value[2], 'ch3': value[3], 'ch4': value[4], 'ch5': value[5], 'ch6': value[6], 'ch7': value[7], 'ch8': value[8]}

if (data['ch8'] > 1900) and (not self.vehicle_controll.in_panic):

# start to panic

self.vehicle_controll.panic()

elif (data['ch8'] < 1900) and (self.vehicle_controll.in_panic):

# stop panic

self.vehicle_controll.in_panic = False

else:

# or already in panic or no reason to panic

pass

elif attr_name=="last_heartbeat":

#print "last_heartbeat: {}".format(round(value,2))

data = {'last_heartbeat': round(value,2)}

elif attr_name=="gimbal":

#print "Gimbal. pitch: {} roll: {} yaw:{}".format(value.pitch, value.roll, value.yaw)

data = {'gimbal_roll': value.roll, 'gimbal_pitch': value.pitch, 'gimbal_yaw': value.yaw}

elif attr_name=="battery":

#print "Battery. {}".format(value.voltage)

data = {'battery': value.voltage}

elif attr_name=="gps_0":

#print "gps_0. eph: {} epv: {} fix_type: {} satellites_visible: {}".format(value.eph, value.epv, value.fix_type, value.satellites_visible)

data = {'gps_0_HDOP': value.eph, 'gps_0_VDOP': value.epv, 'gps_0_fix': value.fix_type, 'gps_0_satellites': value.satellites_visible}

elif attr_name=="ekf_ok":

#print "ekf_ok: {}".format(value)

data = {'ekf_ok': value}

elif attr_name=="mount":

#method is replaced by gimbal

#print "mount: {}".format(value)

pass

elif attr_name=="mode":

#print "mode: {}".format(value.name)

data = {'mode': value.name}

elif attr_name=="armed":

#print "armed: {}".format(value)

data = {'armed': str(value)}

elif attr_name=="is_armable":

#print "is_armable: {}".format(value)

print "### is_armable: {}".format(dir(value))

elif attr_name=="system_status":

#print "System status {}".format(value.state)

data = {'system_status': value.state}

else:

print "### NOT SENT:" + attr_name

if data:

#print str(data)

try:

self.UDP_server_Telem_Cmd.send_telem(data)

except:

traceback.print_exc()

def close_all(self):

if self.Report_enabled:

print "Drone: Stop redirecting stdout to UDP"

sys.stdout = sys.__stdout__

print "Drone: Stop redirecting stderr to UDP"

sys.stderr = sys.__stderr__

print "Drone: Stoped redirecting stdout to UDP"

if self.vehicle is not None:

print "Drone: Close all - Unbind Pixhawk telem callback"

self.vehicle.remove_attribute_listener('*', self.wildcard_callback)

print "Drone: Close all - Vehicle object"

self.vehicle.close()

if self.UDP_server_Telem_Cmd is not None:

print "Drone: Close all - UDP socket for Telem/Cmd"

self.UDP_server_Telem_Cmd.close_UDP()

else:

print "Drone: Close all - WARNING: UDP socket for Telem/Cmd does not exist. Nothing to close"

if self.UDP_server_Report is not None:

print "Drone: Close all - UDP socket for Reports"

self.UDP_server_Report.close_UDP()

else:

print "Drone: Close all - WARNING: UDP socket for Reports does not exist. Nothing to close"

if self.sitl is not None:

print "Drone: Close all - SITL"

self.sitl.stop()

if self.GUI_enabled:

print "Drone: Close all - GUI"

self.GUI.GUI_close()

if self.args.video_client_ip:

print "Drone: Close all - Video"

self.video_server.closeVideoServer()