├── .gitignore ├── Channels ├── DroneChannels.py └── __init__.py ├── Constant ├── CommandConstant.py ├── CommandConstant.pyc ├── ProjectConstant.py ├── ProjectConstant.pyc ├── Server.py ├── Server.pyc ├── User.py ├── User.pyc ├── __init__.py └── __init__.pyc ├── Databases ├── DbConstant.py ├── DbInitilize.py └── __init__.py ├── GoogleLatLng ├── LatLngCalculation.py ├── LatLngCalculation.pyc ├── __init__.py └── __init__.pyc ├── Mission ├── DroneMission.py ├── DroneMission.pyc ├── __init__.py └── __init__.pyc ├── Param ├── DroneParameter.py └── __init__.py ├── RC ├── RcController.py └── __init__.py ├── README.md ├── README_2.md ├── Utility ├── Delay.py ├── JsonBuilder.py ├── JsonBuilder.pyc ├── ProjectCommand.py ├── ProjectCommand.pyc ├── SdCard.py ├── SdCard.pyc ├── __init__.py └── __init__.pyc ├── main.py ├── mavlink.py ├── mavlink_lib.py ├── onesensor_01.py ├── run.sh ├── t2.py └── test.py /.gitignore: -------------------------------------------------------------------------------- 1 | .idea -------------------------------------------------------------------------------- /Channels/DroneChannels.py: -------------------------------------------------------------------------------- 1 | class DroneChannelsClass: 2 | @staticmethod 3 | def read_channels(vehicle): 4 | print("Read Channels individually:") 5 | print(" Ch1: %s" % vehicle.channels['1']) 6 | print(" Ch2: %s" % vehicle.channels['2']) 7 | print(" Ch3: %s" % vehicle.channels['3']) 8 | print(" Ch4: %s" % vehicle.channels['4']) 9 | print(" Ch5: %s" % vehicle.channels['5']) 10 | print(" Ch6: %s" % vehicle.channels['6']) 11 | print(" Ch7: %s" % vehicle.channels['7']) 12 | print(" Ch8: %s" % vehicle.channels['8']) 13 | print("Number of Channels: %s" % len(vehicle.channels)) -------------------------------------------------------------------------------- /Channels/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/w3iotarmy/drone_control/728950b8557bc35e073381535363ecb77bdac837/Channels/__init__.py -------------------------------------------------------------------------------- /Constant/CommandConstant.py: -------------------------------------------------------------------------------- 1 | class CommandConstantClass: 2 | str_user = "" 3 | str_eagleeye = "" 4 | 5 | def __init__(self): 6 | self.str_user = "borhan" 7 | self.str_eagleeye = "eagleeye" 8 | print ("ProjectConstantClass") 9 | 10 | @staticmethod 11 | def is_equal_start_drone(): 12 | return 'start' 13 | 14 | @staticmethod 15 | def is_equal_home_location(): 16 | return 'home_location' 17 | 18 | @staticmethod 19 | def is_equal_read_channels(): 20 | return 'read_channels' 21 | @staticmethod 22 | def is_equal_wp(): 23 | return 'wp' 24 | 25 | @staticmethod 26 | def is_equal_arm(): 27 | return 'arm' 28 | 29 | @staticmethod 30 | def is_equal_disarm(): 31 | return 'disarm' 32 | 33 | @staticmethod 34 | def is_equal_mode(): 35 | return 'mode' 36 | 37 | @staticmethod 38 | def is_equal_takeoff(): 39 | return 'takeoff' 40 | 41 | @staticmethod 42 | def is_equal_takeoff_land(): 43 | return 'takeoff_land' 44 | @staticmethod 45 | def is_equal_store_param(): 46 | return 'store_param' 47 | 48 | @staticmethod 49 | def is_equal_reboot(): 50 | return 'reboot' 51 | 52 | @staticmethod 53 | def is_equal_battery_info(): 54 | return 'battery' 55 | 56 | @staticmethod 57 | def is_equal_rc_03(): 58 | return 'rc_03' 59 | 60 | @staticmethod 61 | def is_equal_manual_fly(): 62 | return 'manual_fly' 63 | @staticmethod 64 | def get_wp_file_name(): 65 | return 'wp.txt' -------------------------------------------------------------------------------- /Constant/CommandConstant.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/w3iotarmy/drone_control/728950b8557bc35e073381535363ecb77bdac837/Constant/CommandConstant.pyc -------------------------------------------------------------------------------- /Constant/ProjectConstant.py: -------------------------------------------------------------------------------- 1 | class ProjectConstantClass: 2 | str_user ="" 3 | str_eagleeye="" 4 | def __init__(self): 5 | self.str_user="borhan" 6 | self.str_eagleeye="eagleeye" 7 | print ("ProjectConstantClass") 8 | @staticmethod 9 | def get_obstacle_enable_response(): 10 | return '{"username":"eagleeye","control_type":"1002","enable_disable":1,"variable_array":"Borhan"}' 11 | @staticmethod 12 | def get_obstacle_disable_response(): 13 | return '{"username":"eagleeye","control_type":"1003","enable_disable":1,"variable_array":"Borhan"}' 14 | @staticmethod 15 | def get_pwm_enable_response(): 16 | return '{"username":"eagleeye","control_type":"1001","enable_disable":1,"variable_array":"Borhan"}' 17 | @staticmethod 18 | def get_dialog_completed_response(): 19 | return '{"username":"eagleeye","control_type":"3001","enable_disable":1,"variable_array":"Borhan"}' 20 | @staticmethod 21 | def get_lat_lng_request(): 22 | return '{"username":"eagleeye","control_type":"4001","enable_disable":1,"variable_array":"Borhan"}' 23 | @staticmethod 24 | def object_dect(): 25 | return '{"username":"eagleeye","control_type":"5002","enable_disable":1,"variable_array":"Borhan","lat":0.00,"lng":0.00,"alt":100}' 26 | @staticmethod 27 | def get_host(): 28 | return '184.72.95.87' 29 | @staticmethod 30 | def get_port(): 31 | return 3000 32 | @staticmethod 33 | def get_secrer_key(): 34 | return 'secret!' 35 | @staticmethod 36 | def get_drone_control_array_type(): 37 | return 2001 38 | @staticmethod 39 | def get_defauld_mode_value(): 40 | return 470 41 | 42 | @staticmethod 43 | def get_drone_lat_lng_response(): 44 | return 4002 45 | 46 | @staticmethod 47 | def get_drone_object_detect(): 48 | return 5001 -------------------------------------------------------------------------------- /Constant/ProjectConstant.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/w3iotarmy/drone_control/728950b8557bc35e073381535363ecb77bdac837/Constant/ProjectConstant.pyc -------------------------------------------------------------------------------- /Constant/Server.py: -------------------------------------------------------------------------------- 1 | class ServerClass: 2 | str_user ="" 3 | str_eagleeye="" 4 | def __init__(self): 5 | self.str_user="borhan" 6 | self.str_eagleeye="eagleeye" 7 | print ("ServerClass") 8 | 9 | @staticmethod 10 | def get_server_secret_key(): 11 | return 'secret!' 12 | @staticmethod 13 | def get_server_ip(): 14 | return '184.72.95.87' 15 | @staticmethod 16 | def get_server_port(): 17 | return 3000 -------------------------------------------------------------------------------- /Constant/Server.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/w3iotarmy/drone_control/728950b8557bc35e073381535363ecb77bdac837/Constant/Server.pyc -------------------------------------------------------------------------------- /Constant/User.py: -------------------------------------------------------------------------------- 1 | class UserClass: 2 | @staticmethod 3 | def self_user(): 4 | return 'eagle' 5 | @staticmethod 6 | def ground_user(): 7 | return 'ground' 8 | -------------------------------------------------------------------------------- /Constant/User.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/w3iotarmy/drone_control/728950b8557bc35e073381535363ecb77bdac837/Constant/User.pyc -------------------------------------------------------------------------------- /Constant/__init__.py: -------------------------------------------------------------------------------- 1 | __version__ = "0.0.1" -------------------------------------------------------------------------------- /Constant/__init__.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/w3iotarmy/drone_control/728950b8557bc35e073381535363ecb77bdac837/Constant/__init__.pyc -------------------------------------------------------------------------------- /Databases/DbConstant.py: -------------------------------------------------------------------------------- 1 | class DbConstantClass: 2 | @staticmethod 3 | def get_table_name_wp_status(): 4 | return '' -------------------------------------------------------------------------------- /Databases/DbInitilize.py: -------------------------------------------------------------------------------- 1 | import sqlite3 2 | conn = sqlite3.connect('drone.db',check_same_thread=False) 3 | 4 | class DbInitializeClass: 5 | @staticmethod 6 | def dbInit(): 7 | a=0 8 | # c = conn.cursor() 9 | # #c.execute('''CREATE TABLE IF NOT EXISTS wp_status(id INTEGER PRIMARY KEY AUTOINCREMENT, uploaded int)''') 10 | # #c.execute("INSERT INTO wp_status VALUES (1,0)") 11 | # rows = [(0,1)] 12 | # c.executemany('insert into wp_status values (?,?)', rows) 13 | # conn.commit() 14 | # conn.close() 15 | @staticmethod 16 | def update_wp_status_false(): 17 | c = conn.cursor() 18 | c.execute("UPDATE wp_status set uploaded=0 WHERE id=0") 19 | conn.commit() 20 | #conn.close() 21 | 22 | @staticmethod 23 | def update_wp_status_true(): 24 | c = conn.cursor() 25 | c.execute("UPDATE wp_status set uploaded=1 WHERE id=0") 26 | conn.commit() 27 | #conn.close() 28 | 29 | @staticmethod 30 | def get_data_wp_status(): 31 | c = conn.cursor() 32 | c.execute("SELECT * FROM wp_status") 33 | rows= c.fetchall() 34 | for row in rows: 35 | print(row[1]) 36 | return row[1]; 37 | #conn.close() -------------------------------------------------------------------------------- /Databases/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/w3iotarmy/drone_control/728950b8557bc35e073381535363ecb77bdac837/Databases/__init__.py -------------------------------------------------------------------------------- /GoogleLatLng/LatLngCalculation.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import math 3 | 4 | a_constant = 110540 5 | b_constant = 111320 6 | 7 | class LatLngCalculationClass: 8 | def __init__(self): 9 | self.str_user="borhan" 10 | self.str_eagleeye="eagleeye" 11 | print ("LatLngCalculationClass") 12 | def get_def(self): 13 | return 5 14 | @staticmethod 15 | def get_desired_location(start_lat, start_lng, dist, angel): 16 | 17 | dx = float(float(dist) * float(math.cos(angel))) 18 | dy = float(float(dist) * float(math.sin(angel))) 19 | 20 | delta_lng = dx / (b_constant * (math.cos(float(start_lat)))) 21 | delta_lat = dy / a_constant 22 | 23 | lng = float(start_lng) + delta_lng 24 | lat = float(start_lat) + delta_lat 25 | return_dictionary = dict() 26 | return_dictionary['lat'] = lat 27 | return_dictionary['lng'] = lng 28 | return return_dictionary 29 | def get_home_location(self,vehicle): 30 | # Get Vehicle Home location - will be `None` until first set by autopilot 31 | while not vehicle.home_location: 32 | cmds = vehicle.commands 33 | cmds.download() 34 | cmds.wait_ready() 35 | if not vehicle.home_location: 36 | print " Waiting for home location ..." 37 | 38 | # We have a home location. 39 | print "\n Home location: %s" % vehicle.home_location 40 | return vehicle.home_location -------------------------------------------------------------------------------- /GoogleLatLng/LatLngCalculation.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/w3iotarmy/drone_control/728950b8557bc35e073381535363ecb77bdac837/GoogleLatLng/LatLngCalculation.pyc -------------------------------------------------------------------------------- /GoogleLatLng/__init__.py: -------------------------------------------------------------------------------- 1 | __version__ = "0.0.1" -------------------------------------------------------------------------------- /GoogleLatLng/__init__.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/w3iotarmy/drone_control/728950b8557bc35e073381535363ecb77bdac837/GoogleLatLng/__init__.pyc -------------------------------------------------------------------------------- /Mission/DroneMission.py: -------------------------------------------------------------------------------- 1 | from dronekit import Command 2 | 3 | 4 | def readmission(aFileName, vehicle): 5 | print "\nReading mission from file: %s" % aFileName 6 | cmds = vehicle.commands 7 | missionlist = [] 8 | with open(aFileName) as f: 9 | for i, line in enumerate(f): 10 | if i == 0: 11 | if not line.startswith('QGC WPL 110'): 12 | raise Exception('File is not supported WP version') 13 | else: 14 | linearray = line.split('\t') 15 | ln_index = int(linearray[0]) 16 | ln_currentwp = int(linearray[1]) 17 | ln_frame = int(linearray[2]) 18 | ln_command = int(linearray[3]) 19 | ln_param1 = float(linearray[4]) 20 | ln_param2 = float(linearray[5]) 21 | ln_param3 = float(linearray[6]) 22 | ln_param4 = float(linearray[7]) 23 | ln_param5 = float(linearray[8]) 24 | ln_param6 = float(linearray[9]) 25 | ln_param7 = float(linearray[10]) 26 | ln_autocontinue = int(linearray[11].strip()) 27 | cmd = Command(0, 0, 0, ln_frame, ln_command, ln_currentwp, ln_autocontinue, ln_param1, ln_param2, 28 | ln_param3, ln_param4, ln_param5, ln_param6, ln_param7) 29 | missionlist.append(cmd) 30 | return missionlist 31 | 32 | def download_mission(vehicle): 33 | print(" Download mission from vehicle") 34 | missionlist=[] 35 | cmds = vehicle.commands 36 | cmds.download() 37 | cmds.wait_ready() 38 | for cmd in cmds: 39 | missionlist.append(cmd) 40 | return missionlist 41 | 42 | def save_mission(aFileName,vehicle): 43 | missionlist = download_mission(vehicle) 44 | output='QGC WPL 110\n' 45 | home = vehicle.home_location 46 | output+="%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\n" % (0,1,0,16,0,0,0,0,home.lat,home.lon,home.alt,1) 47 | for cmd in missionlist: 48 | commandline="%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\t%s\n" % (cmd.seq,cmd.current,cmd.frame,cmd.command,cmd.param1,cmd.param2,cmd.param3,cmd.param4,cmd.x,cmd.y,cmd.z,cmd.autocontinue) 49 | output+=commandline 50 | with open(aFileName, 'w') as file_: 51 | print(" Write mission to file") 52 | file_.write(output) 53 | class DroneMissionClass: 54 | @staticmethod 55 | def upload_mission( aFileName,vehicle): 56 | missionlist = readmission(aFileName,vehicle) 57 | print ' Clear mission' 58 | cmds = vehicle.commands 59 | cmds.clear() 60 | # Add new mission to vehicle 61 | for command in missionlist: 62 | cmds.add(command) 63 | print ' Upload mission' 64 | vehicle.commands.upload() 65 | return True -------------------------------------------------------------------------------- /Mission/DroneMission.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/w3iotarmy/drone_control/728950b8557bc35e073381535363ecb77bdac837/Mission/DroneMission.pyc -------------------------------------------------------------------------------- /Mission/__init__.py: -------------------------------------------------------------------------------- 1 | __version__ = "0.0.1" -------------------------------------------------------------------------------- /Mission/__init__.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/w3iotarmy/drone_control/728950b8557bc35e073381535363ecb77bdac837/Mission/__init__.pyc -------------------------------------------------------------------------------- /Param/DroneParameter.py: -------------------------------------------------------------------------------- 1 | class DroneParameterClass: 2 | print "" -------------------------------------------------------------------------------- /Param/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/w3iotarmy/drone_control/728950b8557bc35e073381535363ecb77bdac837/Param/__init__.py -------------------------------------------------------------------------------- /RC/RcController.py: -------------------------------------------------------------------------------- 1 | import Adafruit_PCA9685 2 | def reconnect_io_func(): 3 | try: 4 | pwm = Adafruit_PCA9685.PCA9685() 5 | return pwm 6 | except Exception as error: 7 | if "Remote I/O error" in (error): 8 | reconnect_io = True 9 | while reconnect_io: 10 | try: 11 | print("while Error: "+str(error)) 12 | pwm = Adafruit_PCA9685.PCA9685() 13 | # print(pwm) 14 | reconnect_io = False 15 | return pwm 16 | except Exception as error: 17 | # print((error)) 18 | reconnect_io = True 19 | pwm = reconnect_io_func() 20 | pwm.set_pwm_freq(60) 21 | class RcControllerClass: 22 | @staticmethod 23 | def send_rc_command(rc_0,rc_1,rc_2,rc_3,rc_4,rc_5,rc_6,rc_7): 24 | print ("RC val RC Controller", rc_0, " ", rc_1, " ", rc_2, " ", rc_3, " ", rc_4, " ", rc_5, " ", rc_6, " ", rc_7) 25 | pwm.set_pwm(0, 0, int(rc_0)) 26 | pwm.set_pwm(1, 0, int(rc_1)) 27 | pwm.set_pwm(2, 0, int(rc_2)) 28 | pwm.set_pwm(3, 0, int(rc_3)) 29 | pwm.set_pwm(4, 0, int(rc_4)) 30 | pwm.set_pwm(5, 0, int(rc_5)) 31 | pwm.set_pwm(6, 0, int(rc_6)) 32 | pwm.set_pwm(7, 0, int(rc_7)) -------------------------------------------------------------------------------- /RC/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/w3iotarmy/drone_control/728950b8557bc35e073381535363ecb77bdac837/RC/__init__.py -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Drone Control Python File 2 | ## Dependency 3 | RPi.GPIO 4 | Adafruit_PCA9685 5 | flask 6 | flask_socketio 7 | json 8 | #### Install 9 | **Install Adafruit PCA9685 I2C device with raspberry pi via I2C Bus** 10 | 11 | sudo apt-get install python-smbus 12 | sudo apt-get install i2c-tools 13 | # Test is connected device 14 | sudo i2cdetect -y 0 15 | # OR 16 | sudo i2cdetect -y 1 17 | sudo apt-get install git build-essential python-dev 18 | cd ~ git clone https://github.com/adafruit/Adafruit_Python_PCA9685.git 19 | cd Adafruit_Python_PCA9685 20 | sudo python setup.py install 21 | # if you have python3 installed: 22 | sudo python3 setup.py install 23 | **Flask install** 24 | 25 | pip uninstall gevent 26 | sudo pip install socketIO_client 27 | pip install flask 28 | pip install flask-socketio 29 | ### Download from git 30 | sudo git clone https://code.leftofthedot.com/borhanreo/drone_control.git 31 | cd drone_control 32 | sudo python main.py 33 | 34 | 35 | 36 | ### socket io install issue 37 | **If you get any problem then need to uninstall gevent** 38 | 39 | pip2 freeze | grep socket 40 | sudo pip2 uninstall gevent-socketio 41 | sudo pip2 uninstall gevent-python 42 | sudo pip2 install python-socketio 43 | sudo pip2 install socketIO-client 44 | sudo pip2 install websocket-client 45 | 46 | # Install mavlink 47 | ### Install 48 | sudo apt-get update 49 | sudo apt-get install screen python-wxgtk2.8 python-matplotlib python-opencv python-pip python-numpy python-dev libxml2-dev libxslt-dev python-lxml 50 | sudo pip install future 51 | sudo pip install pymavlink 52 | sudo pip install mavproxy 53 | 54 | ### Show list similar usb port identity 55 | python -m serial.tools.list_ports 56 | /dev/ttyAMA0 57 | /dev/ttyUSB0 58 | /dev/ttyUSB1 59 | 3 ports found 60 | ### Need to remember for pip is python2 or python3 here we need pip (python 2) 61 | pip --version 62 | ### Connect Ardupilot 63 | bash run.sh 64 | ##### Or 65 | python mavlink_lib.py --master=/dev/ttyACM0 --baudrate 115200 --aircraft MyCopter 66 | ##### OR 67 | bash run.sh 68 | ### Useful mavproxy command 69 | ##### show available mod 70 | mode 71 | ##### Guided Mode 72 | mode guided 73 | ##### Arm 74 | arm throttle 75 | ##### takeoff 76 | takeoff 40 77 | ##### Parameter load 78 | param load ..\Tools\autotest\default_params\copter.parm 79 | ##### Circle mode 80 | mode circle 81 | param set circle_radius 2000 82 | ##### Target altitude 83 | **Write guided then desire altitude guided ALTITUDE** 84 | 85 | guided 100 86 | 87 | **Write guided then desire LAT LNG ALT guided ALTITUDE** 88 | 89 | guided 22.376666 -121.54464 120 90 | ##### save parameter 91 | param save ./myparams.parm 92 | http://ardupilot.org/dev/docs/using-sitl-for-ardupilot-testing.html 93 | https://ardupilot.github.io/MAVProxy/html/modules/cmdlong.html 94 | #### GPS Data 95 | master=mpstate.master() 96 | lat = master.field('GLOBAL_POSITION_INT', 'lat', 0) * 1.0e-7 97 | lng = master.field('GLOBAL_POSITION_INT', 'lon', 0) * 1.0e-7 98 | #### Auto open a terminal 99 | **To auto-start the terminal on boot, open this file with nano:** 100 | 101 | nano ~/.config/lxsession/LXDE-pi/autostart 102 | **Add this line to the end of the file:** 103 | 104 | @lxterminal 105 | **Close, save and reboot** 106 | 107 | sudo reboot 108 | 109 | 110 | ##### Motor can not sync 111 | --https://youtu.be/Y8G3tua0ezI 112 | 113 | Power on rpi and run.. **Drone power will be off** 114 | 115 | All power shuld be off 116 | cd /home/pi/development/drone_control 117 | python python obsAI.py 118 | RC 3 HIGH / THROTTLE 100% MAXIMUM** 119 | POWER ON/ CONNECT BATTERY DRONE** 120 | AFTER BEEF COMPLETED then again drone battery power OFF** 121 | power on or Plug battery again 122 | After beef completer then throttle going to minimum** 123 | unplug again 124 | 125 | 126 | ####Reference 127 | ##### Dronekit 128 | http://ardupilot.org/copter/docs/common-lightware-sf40c-objectavoidance.html 129 | ##### OpencV target set 130 | https://www.pyimagesearch.com/2015/05/04/target-acquired-finding-targets-in-drone-and-quadcopter-video-streams-using-python-and-opencv/ 131 | ##### Face 132 | https://www.pyimagesearch.com/2018/06/18/face-recognition-with-opencv-python-and-deep-learning/ 133 | -------------------------------------------------------------------------------- /README_2.md: -------------------------------------------------------------------------------- 1 | #### Control 2 | python mavlink.py --master=/dev/ttyACM0 --baudrate 115200 --aircraft MyCopter 3 | #### start json 4 | {"u":"ground","action":"start"} 5 | -------------------------------------------------------------------------------- /Utility/Delay.py: -------------------------------------------------------------------------------- 1 | class Delay: 2 | @staticmethod 3 | def delay_in_ms(): 4 | print "delay" 5 | -------------------------------------------------------------------------------- /Utility/JsonBuilder.py: -------------------------------------------------------------------------------- 1 | import json 2 | class JsonBuilderClass: 3 | @staticmethod 4 | def get_start_information(vehicle): 5 | start_data = {} 6 | start_data['u'] = 'eagle' 7 | start_data['action'] = 'gps' 8 | start_data['lat'] = vehicle.location.global_frame.lat 9 | start_data['lon'] = vehicle.location.global_frame.lon 10 | start_data['mode'] = vehicle.mode.name 11 | json_data = json.dumps(start_data) 12 | return json_data 13 | 14 | @staticmethod 15 | def get_is_arm(): 16 | start_data = {} 17 | start_data['u'] = 'eagle' 18 | start_data['action'] = 'arm' 19 | json_data = json.dumps(start_data) 20 | return json_data 21 | 22 | @staticmethod 23 | def get_is_takeoff(): 24 | start_data = {} 25 | start_data['u'] = 'eagle' 26 | start_data['action'] = 'takeoff' 27 | json_data = json.dumps(start_data) 28 | return json_data 29 | 30 | @staticmethod 31 | def get_is_no_waypoint(): 32 | start_data = {} 33 | start_data['u'] = 'eagle' 34 | start_data['action'] = 'no_waypoint' 35 | json_data = json.dumps(start_data) 36 | return json_data 37 | @staticmethod 38 | def get_location_information(lat,lon,alt): 39 | start_data = {} 40 | start_data['u'] = 'eagle' 41 | start_data['action'] = 'location' 42 | start_data['lat'] = lat 43 | start_data['lon'] = lon 44 | start_data['alt'] = alt 45 | json_data = json.dumps(start_data) 46 | return json_data 47 | 48 | @staticmethod 49 | def get_battery_information(voltage,current,level): 50 | start_data = {} 51 | start_data['u'] = 'eagle' 52 | start_data['action'] = 'battery' 53 | start_data['voltage'] = voltage 54 | start_data['current'] = current 55 | start_data['level'] = level 56 | json_data = json.dumps(start_data) 57 | return json_data 58 | 59 | @staticmethod 60 | def get_all_information(b_voltage, b_current, b_level,gps_fix,gps_num_sat,gps_lat,gps_lon,gps_alt): 61 | start_data = {} 62 | start_data['u'] = 'eagle' 63 | start_data['action'] = 'all_info' 64 | start_data['b_voltage'] = b_voltage 65 | start_data['b_current'] = b_current 66 | start_data['b_level'] = b_level 67 | start_data['gps_fix'] = gps_fix 68 | start_data['gps_num_sat'] = gps_num_sat 69 | start_data['gps_lat'] = gps_lat 70 | start_data['gps_lon'] = gps_lon 71 | start_data['gps_alt'] = gps_alt 72 | json_data = json.dumps(start_data) 73 | return json_data 74 | 75 | @staticmethod 76 | def get_mode_information(mode): 77 | start_data = {} 78 | start_data['u'] = 'eagle' 79 | start_data['action'] = 'mode' 80 | start_data['data'] = mode 81 | json_data = json.dumps(start_data) 82 | return json_data 83 | 84 | @staticmethod 85 | def get_gps_inf_information(fix,sat): 86 | start_data = {} 87 | start_data['u'] = 'eagle' 88 | start_data['action'] = 'gps_info' 89 | start_data['fix'] = fix 90 | start_data['sat_num'] = sat 91 | json_data = json.dumps(start_data) 92 | return json_data 93 | 94 | @staticmethod 95 | def get_waypoint_received_response(): 96 | start_data = {} 97 | start_data['u'] = 'eagle' 98 | start_data['action'] = 'wp_res' 99 | start_data['value'] = True 100 | json_data = json.dumps(start_data) 101 | return json_data -------------------------------------------------------------------------------- /Utility/JsonBuilder.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/w3iotarmy/drone_control/728950b8557bc35e073381535363ecb77bdac837/Utility/JsonBuilder.pyc -------------------------------------------------------------------------------- /Utility/ProjectCommand.py: -------------------------------------------------------------------------------- 1 | class CommandClass: 2 | str_user = "" 3 | str_eagleeye = "" 4 | 5 | def __init__(self): 6 | self.str_user = "borhan" 7 | self.str_eagleeye = "eagleeye" 8 | print ("CommandClass") 9 | @staticmethod 10 | def get_arm_string(value): 11 | if(value==1): 12 | return 'arm throttle' 13 | elif value==0: 14 | return '' 15 | @staticmethod 16 | def get_mode_stabilize_string(): 17 | return 'mode stabilize' 18 | 19 | @staticmethod 20 | def get_mode_land_string(): 21 | return 'mode land' 22 | 23 | @staticmethod 24 | def get_mode_loiter_string(): 25 | return 'mode loiter' 26 | 27 | @staticmethod 28 | def get_mode_guided_string(): 29 | return 'mode guided' 30 | 31 | @staticmethod 32 | def get_mode_rtl_string(): 33 | return 'mode rtl' 34 | 35 | @staticmethod 36 | def get_mode_althold_string(): 37 | return 'mode alt_hold' -------------------------------------------------------------------------------- /Utility/ProjectCommand.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/w3iotarmy/drone_control/728950b8557bc35e073381535363ecb77bdac837/Utility/ProjectCommand.pyc -------------------------------------------------------------------------------- /Utility/SdCard.py: -------------------------------------------------------------------------------- 1 | import os 2 | from Constant import CommandConstant 3 | 4 | class SdCardClass: 5 | @staticmethod 6 | def file_write(wp_data): 7 | # if(os.path.isfile(CommandConstant.CommandConstantClass.get_wp_file_name())): 8 | # print ("File exit") 9 | # os.remove(CommandConstant.CommandConstantClass.get_wp_file_name()) 10 | # open(CommandConstant.CommandConstantClass.get_wp_file_name(), "w+") 11 | # with open(CommandConstant.CommandConstantClass.get_wp_file_name(), 'a') as the_file: 12 | # the_file.write(wp_data) 13 | # else: 14 | # print("file not exit") 15 | # open(CommandConstant.CommandConstantClass.get_wp_file_name(), "w+") 16 | # with open(CommandConstant.CommandConstantClass.get_wp_file_name(), 'a') as the_file: 17 | # the_file.write(wp_data) 18 | try: 19 | #print(data) 20 | temp = wp_data 21 | file = open('wp.txt', 'w') 22 | for letter in temp: 23 | if (letter == '*'): 24 | file.write(letter.replace('*', '\n')) 25 | elif (letter == ','): 26 | file.write(letter.replace(',', '\t')) 27 | else: 28 | file.write(letter) 29 | file.close() 30 | except Exception as ex: 31 | print(ex) 32 | @staticmethod 33 | def param_write(data): 34 | try: 35 | #print(data) 36 | temp = data 37 | file = open('param.txt', 'w') 38 | file.write(temp) 39 | file.close() 40 | except Exception as ex: 41 | print(ex) 42 | -------------------------------------------------------------------------------- /Utility/SdCard.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/w3iotarmy/drone_control/728950b8557bc35e073381535363ecb77bdac837/Utility/SdCard.pyc -------------------------------------------------------------------------------- /Utility/__init__.py: -------------------------------------------------------------------------------- 1 | __version__ = "0.0.1" -------------------------------------------------------------------------------- /Utility/__init__.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/w3iotarmy/drone_control/728950b8557bc35e073381535363ecb77bdac837/Utility/__init__.pyc -------------------------------------------------------------------------------- /main.py: -------------------------------------------------------------------------------- 1 | #mmcblk0p2 2 | import threading 3 | import RPi.GPIO as GPIO 4 | import time 5 | import Adafruit_PCA9685 6 | from time import sleep 7 | from flask import Flask, render_template 8 | from flask_socketio import SocketIO 9 | import json 10 | import time 11 | from Constant import ProjectConstant 12 | from GoogleLatLng import LatLngCalculation 13 | obstacle_diable_response = ProjectConstant.ProjectConstantClass.get_obstacle_disable_response() 14 | obstacle_enable_response = ProjectConstant.ProjectConstantClass.get_obstacle_enable_response() 15 | pwm_enable_response = ProjectConstant.ProjectConstantClass.get_pwm_enable_response() 16 | print (obstacle_diable_response," ", obstacle_enable_response) 17 | 18 | drone_control_array_type=ProjectConstant.ProjectConstantClass.get_drone_control_array_type() 19 | modeValue=ProjectConstant.ProjectConstantClass.get_defauld_mode_value() 20 | sensor_enable=False 21 | lat_lng_request = ProjectConstant.ProjectConstantClass.get_lat_lng_request() 22 | 23 | # allow the camera to warmup 24 | time.sleep(0.1) 25 | pwm_enabled_code=2001 26 | 27 | left_or_right=0 28 | control_pwm=420 29 | control_time=1 30 | 31 | 32 | app = Flask(__name__) 33 | app.config['SECRET_KEY'] = ProjectConstant.ProjectConstantClass.get_secrer_key() 34 | socketio = SocketIO(app) 35 | 36 | from socketIO_client import SocketIO as client_socketio, BaseNamespace 37 | 38 | my_client = client_socketio(ProjectConstant.ProjectConstantClass.get_host(), ProjectConstant.ProjectConstantClass.get_port()) 39 | fl_mode_stabilize=1 40 | fl_mode_loiter=2 41 | fl_mode_rtl=3 42 | fl_mode_land=4 43 | fl_mode_alt_hold=5 44 | fl_mode_auto=6 45 | def send_message(send_msg): 46 | my_client_send = client_socketio(ProjectConstant.ProjectConstantClass.get_host(), ProjectConstant.ProjectConstantClass.get_port()) 47 | my_client_send.emit('chat message', send_msg) 48 | #print(str) 49 | 50 | 51 | GPIO.setmode(GPIO.BCM) 52 | TRIG = 19 53 | ECHO = 26 54 | pulse_end_01 =0.00 55 | pulse_start_01 = 0.00 56 | pulse_duration =0.00 57 | alt_holt_code=456 58 | GPIO.setup(TRIG, GPIO.OUT) 59 | GPIO.setup(ECHO, GPIO.IN) 60 | 61 | 62 | def doSomething(data): 63 | print (data) 64 | obj_data = json.loads(data) 65 | val = int(obj_data['enable_disable']) 66 | control_type = int(obj_data['control_type']) 67 | 68 | if control_type==1: 69 | if val == 1: 70 | global sensor_enable 71 | sensor_enable = True 72 | # obstacle enabled 73 | str = obstacle_enable_response 74 | send_message(str) 75 | elif val == 2: 76 | global sensor_enable 77 | sensor_enable = False 78 | # obstacle disable 79 | str = obstacle_diable_response 80 | send_message(str) 81 | # print (fruits_list) 82 | elif control_type==2: 83 | pwm.set_pwm(0, 0, 420) 84 | pwm.set_pwm(1, 0, 420) 85 | pwm.set_pwm(2, 0, 307) 86 | pwm.set_pwm(3, 0, 420) 87 | pwm.set_pwm(4, 0, 307) 88 | pwm.set_pwm(5, 0, 307) 89 | pwm.set_pwm(6, 0, 307) 90 | pwm.set_pwm(7, 0, 307) 91 | pwm.set_pwm(8, 0, 0) 92 | #pwn signal enabled 93 | str=pwm_enable_response 94 | send_message(str) 95 | elif control_type == 3: 96 | global modeValue 97 | modeValue=470 98 | elif control_type==4: 99 | global modeValue 100 | modeValue=348 101 | elif control_type== ProjectConstant.ProjectConstantClass.get_drone_lat_lng_response(): 102 | receive_lat = obj_data['lat'] 103 | receive_lng= obj_data['lng'] 104 | desire_lat_lng =LatLngCalculation.LatLngCalculationClass.get_desired_location(receive_lat,receive_lng,500,135) 105 | print ("Response data ", receive_lat," ",receive_lng) 106 | print ("desired data ", desire_lat_lng['lat'],",",desire_lat_lng['lng']) 107 | ###################### object code here and send to target lat lng alt 108 | elif control_type == ProjectConstant.ProjectConstantClass.get_drone_object_detect(): 109 | print "Object detect" 110 | send_message(ProjectConstant.ProjectConstantClass.object_dect()) 111 | elif control_type==drone_control_array_type: 112 | obj_data_obstacle=obj_data['variable_array'] 113 | spilit_data = obj_data_obstacle.split(",") 114 | global control_time 115 | global control_pwm 116 | control_pwm=spilit_data[1] 117 | control_time=spilit_data[2] 118 | str = ProjectConstant.ProjectConstantClass.get_dialog_completed_response() 119 | send_message(str) 120 | 121 | def reconnect_io_func(): 122 | try: 123 | pwm = Adafruit_PCA9685.PCA9685() 124 | return pwm 125 | except Exception as error: 126 | if "Remote I/O error" in (error): 127 | reconnect_io = True 128 | while reconnect_io: 129 | try: 130 | # print("while Error: "+str(error)) 131 | pwm = Adafruit_PCA9685.PCA9685() 132 | # print(pwm) 133 | reconnect_io = False 134 | return pwm 135 | except Exception as error: 136 | # print((error)) 137 | reconnect_io = True 138 | 139 | 140 | def going_stabilize(): 141 | modeLoiter = 307 142 | pwm.set_pwm(4, 0, modeLoiter) 143 | def going_automode(): 144 | modeLoiter = 496 145 | pwm.set_pwm(4, 0, modeLoiter) 146 | def going_loiter(): 147 | modeLoiter = 348 148 | pwm.set_pwm(4, 0, modeLoiter) 149 | def going_alt_hold_mode(): 150 | mode = 456 151 | pwm.set_pwm(4, 0, mode) 152 | def goingDrone(pinNumber,value): 153 | pwm.set_pwm(pinNumber, 0, value) 154 | def random_drone_control(): 155 | print ("Going Throttle up 70% ") 156 | #465 is the throttle up 70 % 157 | pwm.set_pwm(2, 0, 465) 158 | sleep(2) 159 | print ("Going to mode ") 160 | pwm.set_pwm(4, 0, modeValue) 161 | print ("Going to sleep 5 sec ") 162 | sleep(3) 163 | print ("Going to left/right PWM=", control_pwm," TIME=", control_time) 164 | pwm.set_pwm(0, 0, int(control_pwm)) 165 | sleep(int(control_time)) 166 | pwm.set_pwm(0, 0, 420) 167 | sleep(int(control_time)) 168 | pwm.set_pwm(0, 0, int(control_pwm)) 169 | sleep(int(control_time)) 170 | pwm.set_pwm(0, 0, 420) 171 | sleep(int(control_time)) 172 | pwm.set_pwm(4, 0, modeValue) 173 | sleep(5) 174 | # Going to Auto mode 175 | pwm.set_pwm(4, 0, 500) 176 | global sensor_enable 177 | sensor_enable = False 178 | def drone_tast_start(): 179 | print ("Going Throttle up 70% ") 180 | # 465 is the throttle up 70 % 181 | pwm.set_pwm(2, 0, 465) 182 | ##Going to LOITER mode for 183 | print ("Going to mode LOITER") 184 | pwm.set_pwm(4, 0, 348) 185 | 186 | str = ProjectConstant.ProjectConstantClass.get_lat_lng_request() 187 | send_message(str) 188 | global sensor_enable 189 | sensor_enable = False 190 | def drone_final_task(lat,lng): 191 | print(lat," ",lng) 192 | #it will work when drone going to ALT_HOLD Mode 193 | def throttle_up(): 194 | pwm.set_pwm(2, 0, 507) 195 | def object_dectec_01(): 196 | print ("obs Detect") 197 | pwm.set_pwm(4, 0, modeValue) 198 | sleep(5) 199 | print("set pwm ", int(control_pwm)) 200 | pwm.set_pwm(0, 0, int(control_pwm)) 201 | print("set time delay ", int(control_time)) 202 | sleep(int(control_time)) 203 | # strait and wait 5 second 204 | pwm.set_pwm(0, 0, 420) 205 | pwm.set_pwm(4, 0, modeValue) 206 | sleep(5) 207 | # Going to Auto mode 208 | pwm.set_pwm(4, 0, 500) 209 | 210 | 211 | def object_dectec(): 212 | print ("obs Detect") 213 | # going_stabilize() 214 | 215 | # going to Loiter Mode 216 | #pwm.set_pwm(4, 0, 350) 217 | # going alt_hold mode 218 | print ("") 219 | pwm.set_pwm(4, 0, modeValue) 220 | sleep(5) 221 | # turn left until 0.5 second 222 | sendValueBACKWARD = 490 223 | pwm.set_pwm(0, 0, sendValueBACKWARD) 224 | sleep(0.5) 225 | # strait and wait 5 second 226 | pwm.set_pwm(0, 0, 420) 227 | pwm.set_pwm(4, 0, modeValue) 228 | sleep(5) 229 | # Going to Auto mode 230 | pwm.set_pwm(4, 0, 500) 231 | 232 | # if fleft==False: 233 | # sleep(2) 234 | # fleft=True 235 | # sendValueBACKWARD = 455 236 | # pwm.set_pwm(0, 0, sendValueBACKWARD) 237 | def obs_test(): 238 | cc='{"username":"orhan","control_type":"1","enable_disable":1,"variable_array":"Borhan"}' 239 | msg= '{"username":"orhan","control_type":"1","enable_disable":1,"variable_array":"Borhan","name":"John","age":30,"cars":"ABC"}' 240 | send_message(msg) 241 | def read_sensor(): 242 | fleft=False 243 | while True: 244 | if sensor_enable: 245 | # print ("dfsfdsf") 246 | GPIO.output(TRIG, False) 247 | time.sleep(0.1) 248 | GPIO.output(TRIG, True) 249 | time.sleep(0.00001) 250 | GPIO.output(TRIG, False) 251 | GPIO.setwarnings(False) 252 | 253 | while GPIO.input(ECHO) == 0: 254 | global pulse_start_01 255 | pulse_start_01 = time.time() 256 | # print ("Start ", pulse_start) 257 | while GPIO.input(ECHO) == 1: 258 | global pulse_end_01 259 | pulse_end_01 = time.time() 260 | pulse_duration = pulse_end_01 - pulse_start_01 261 | distance = pulse_duration * 17150 262 | distance = round(distance, 2) 263 | 264 | if distance > 10 and distance < 400: 265 | if (distance < 100): 266 | #print ("Distance obs:", distance - 0.5, "cm", obsCounter, " ", modeValue) 267 | #global obsCounter 268 | #obsCounter=obsCounter+1 269 | object_dectec_01() 270 | #obs_test() 271 | #print ("Distance:", distance - 0.5, "cm", obsCounter," ", modeValue) 272 | print ("Distance:", distance - 0.5, "cm"," ", modeValue, control_pwm, " ", control_time) 273 | else: 274 | if (fleft): 275 | # pwm.set_pwm(4, 0, 500) 276 | # sendValueBACKWARD = 420 277 | # pwm.set_pwm(0, 0, sendValueBACKWARD) 278 | fleft = False 279 | 280 | def run_thread(): 281 | while True: 282 | if sensor_enable: 283 | #random_drone_control() 284 | drone_tast_start() 285 | print ("finish and waiting for new command running....") 286 | global pwm 287 | pwm = reconnect_io_func() 288 | 289 | #thread = threading.Thread(target=read_sensor) 290 | thread = threading.Thread(target=run_thread) 291 | thread.daemon = True 292 | thread.start() 293 | 294 | servo_min = 150 # Min pulse length out of 4096 295 | servo_max = 600 # Max pulse length out of 4096 296 | 297 | pwm.set_pwm_freq(60) 298 | my_client.on('chat message', doSomething) 299 | my_client.wait() 300 | 301 | 302 | @socketio.on('chat message') 303 | def create_json_array(): 304 | str = '{"username":"orhan","control_type":"1","enable_disable":1,"variable_array":"Borhan"}' 305 | my_client.emit('chat message', str) 306 | print(str) 307 | 308 | if __name__ == '__main__': 309 | send_message() 310 | socketio.run(app) 311 | pwm.set_pwm(0, 0, 420) 312 | pwm.set_pwm(1, 0, 420) 313 | pwm.set_pwm(2, 0, 307) 314 | pwm.set_pwm(3, 0, 420) 315 | pwm.set_pwm(4, 0, 307) 316 | pwm.set_pwm(5, 0, 307) 317 | pwm.set_pwm(6, 0, 307) 318 | pwm.set_pwm(7, 0, 307) 319 | pwm.set_pwm(8, 0, 0) 320 | 321 | create_json_array() 322 | #array = '{"fruits": ["apple", "banana", "orange"]}' 323 | #create_json_array(array) 324 | #read_sensor() 325 | 326 | 327 | 328 | -------------------------------------------------------------------------------- /mavlink.py: -------------------------------------------------------------------------------- 1 | import threading 2 | 3 | import time 4 | 5 | import sys 6 | 7 | import datetime 8 | from droneapi.lib import Location 9 | from dronekit import connect, VehicleMode, LocationGlobalRelative, Battery, GPSInfo, Command 10 | from pymavlink import mavutil 11 | from Databases import DbInitilize, DbConstant 12 | from Constant import Server,CommandConstant,User,ProjectConstant 13 | from Mission import DroneMission 14 | from Utility import ProjectCommand,SdCard,JsonBuilder 15 | from GoogleLatLng import LatLngCalculation 16 | from flask import Flask, render_template 17 | from RC import RcController 18 | from flask_socketio import SocketIO 19 | app = Flask(__name__) 20 | app.config['SECRET_KEY'] = Server.ServerClass.get_server_secret_key() 21 | socketio = SocketIO(app) 22 | from socketIO_client import SocketIO as client_socketio, BaseNamespace 23 | import json 24 | from Channels import DroneChannels 25 | 26 | 27 | vehicle = connect('/dev/ttyACM0', wait_ready=False) 28 | 29 | battery_voltage = 0.00 30 | battery_current = 0.00 31 | battery_level = 0 32 | gps_fix = 0 33 | gps_num_sat = 0 34 | gps_lat = 0.00 35 | gps_lon = 0.00 36 | gps_alt = 0 37 | mode ="" 38 | 39 | 40 | def init_sender_socket(): 41 | global my_client_send 42 | my_client_send = client_socketio(ProjectConstant.ProjectConstantClass.get_host(),ProjectConstant.ProjectConstantClass.get_port()) 43 | def send_message(send_msg): 44 | my_client_send.emit('chat message', send_msg) 45 | #BATTERY INFO 46 | def receive_battery_data(): 47 | # Demonstrate getting callback on any attribute change 48 | def wildcard_callback(self, attr_name, value): 49 | #print(" CALLBACK_BATTERY: (%s): %s" % (attr_name, value)) 50 | send_message(JsonBuilder.JsonBuilderClass.get_battery_information(value.voltage,value.current,value.level)) 51 | print("\nAdd attribute callback detecting ANY attribute change L") 52 | vehicle.add_attribute_listener('battery', wildcard_callback) 53 | previous_milli = 0 54 | #LOCATION INFO 55 | def receive_location_data(): 56 | # Demonstrate getting callback on any attribute change 57 | def wildcard_callback(self, attr_name, value): 58 | #print(" CALLBACK_LOCATION: (%s): %s" % (attr_name, value)) 59 | send_message(JsonBuilder.JsonBuilderClass.get_location_information(value.lat,value.lon,value.alt)) 60 | vehicle.add_attribute_listener('location.global_relative_frame', wildcard_callback) 61 | #GPS INFO 62 | def receive_gps_info(): 63 | # Demonstrate getting callback on any attribute change 64 | def wildcard_callback(self, attr_name, value): 65 | #print(" CALLBACK_GPS_INFO: (%s): %s" % (attr_name, value)) 66 | send_message(JsonBuilder.JsonBuilderClass.get_gps_inf_information(value.fix_type,value.satellites_visible)) 67 | vehicle.add_attribute_listener('gps_0', wildcard_callback) 68 | #MODE CHANGE 69 | def receive_mode_data(): 70 | @vehicle.on_attribute('mode') 71 | def wildcard_callback(self, attr_name, value): 72 | #print(" CALLBACK: Mode changed to (%s), (%s)", value, attr_name) 73 | send_message(JsonBuilder.JsonBuilderClass.get_mode_information(str(vehicle.mode.name))) 74 | vehicle.add_attribute_listener('mode', wildcard_callback) 75 | def receive_all_data(): 76 | def wildcard_callback(self, attr_name, value): 77 | cbv=0 78 | #print(" All data CALLBACK_ALL: (%s): %s" % (attr_name, value)) 79 | vehicle.add_attribute_listener('*', wildcard_callback) 80 | 81 | def all_information(): 82 | previous_milli = 0 83 | while True: 84 | current_milli = time.mktime(datetime.datetime.now().timetuple()) * 1000 85 | if current_milli-previous_milli>=1000: 86 | print " every 1 second" 87 | send_message(JsonBuilder.JsonBuilderClass.get_all_information(battery_voltage,battery_current,battery_level,gps_fix,gps_num_sat,gps_lat,gps_lon,gps_alt)) 88 | previous_milli = current_milli 89 | 90 | def init_socket(): 91 | my_client = client_socketio(Server.ServerClass.get_server_ip(), Server.ServerClass.get_server_port()) 92 | @socketio.on('chat message') 93 | def handle_message(message): 94 | print('received message init: ' + message) 95 | 96 | def socket_receiver(data): 97 | try: 98 | #print (data) 99 | #vehicle = mpstate.get_vehicles()[0] 100 | obj_data = json.loads(data.replace('\r\n', '\\r\\n'),strict=False) 101 | #obj_data = json.loads(data,strict=False) 102 | #obj_data = json.loads(data) 103 | sender_user = obj_data['u'] 104 | if(sender_user==User.UserClass.ground_user()): 105 | val = obj_data['action'] 106 | if (val == CommandConstant.CommandConstantClass.is_equal_wp()): 107 | #DbInitilize.DbInitializeClass.dbInit() 108 | wp_data = obj_data['data'] 109 | SdCard.SdCardClass.file_write(wp_data) 110 | time.sleep(1) 111 | if(DroneMission.DroneMissionClass.upload_mission(CommandConstant.CommandConstantClass.get_wp_file_name(),vehicle)): 112 | DbInitilize.DbInitializeClass.update_wp_status_true() 113 | send_message(JsonBuilder.JsonBuilderClass.get_waypoint_received_response()) 114 | elif (val == CommandConstant.CommandConstantClass.is_equal_start_drone()): ## Start 115 | init_aircraft() 116 | send_message(JsonBuilder.JsonBuilderClass.get_start_information(vehicle)) 117 | elif (val == CommandConstant.CommandConstantClass.is_equal_arm()): 118 | vehicle.armed=True 119 | send_message(JsonBuilder.JsonBuilderClass.get_is_arm()) 120 | elif (val == CommandConstant.CommandConstantClass.is_equal_disarm()): 121 | vehicle.armed = False 122 | elif (val == CommandConstant.CommandConstantClass.is_equal_mode()): 123 | mode_str = obj_data['data'] 124 | vehicle.mode = VehicleMode(mode_str) 125 | elif (val == CommandConstant.CommandConstantClass.is_equal_takeoff()): 126 | aTargetAltitude = obj_data['data'] 127 | send_message(JsonBuilder.JsonBuilderClass.get_is_takeoff()) 128 | arm_and_takeoff(aTargetAltitude,True) 129 | elif (val == CommandConstant.CommandConstantClass.is_equal_takeoff_land()): 130 | aTargetAltitude = obj_data['data'] 131 | print "takeoff land" 132 | DbInitilize.DbInitializeClass.get_data_wp_status() 133 | if(DbInitilize.DbInitializeClass.get_data_wp_status()==0): 134 | send_message(JsonBuilder.JsonBuilderClass.get_is_no_waypoint()) 135 | elif(DbInitilize.DbInitializeClass.get_data_wp_status()==1): 136 | DbInitilize.DbInitializeClass.update_wp_status_false() 137 | send_message(JsonBuilder.JsonBuilderClass.get_is_takeoff()) 138 | #arm_and_takeoff(aTargetAltitude,False) 139 | elif (val == CommandConstant.CommandConstantClass.is_equal_rc_03()): 140 | rc_value = int(obj_data['data']) 141 | vehicle.channels.overrides['3'] = rc_value 142 | elif (val == CommandConstant.CommandConstantClass.is_equal_battery_info()): 143 | try: 144 | voltage=vehicle.battery.voltage 145 | current=vehicle.battery.current 146 | level=vehicle.battery.level 147 | except: 148 | print('An error occurred.') 149 | voltage = 0.00 150 | current = 0.00 151 | level = 0 152 | send_message(JsonBuilder.JsonBuilderClass.get_battery_information(voltage,current,level)) 153 | elif (val == CommandConstant.CommandConstantClass.is_equal_reboot()): 154 | vehicle.reboot() 155 | time.sleep(1) 156 | elif (val == CommandConstant.CommandConstantClass.is_equal_store_param()): 157 | #print "\nPrint all parameters (iterate `vehicle.parameters`):" 158 | append_string = "" 159 | for key, value in vehicle.parameters.iteritems(): 160 | #print " Key:%s Value:%s" % (key, value) 161 | append_string = append_string + str(key)+":"+str(value)+"\n" 162 | SdCard.SdCardClass.param_write(append_string) 163 | print append_string 164 | elif (val == CommandConstant.CommandConstantClass.is_equal_home_location()): 165 | #home_lat,home_lng = LatLngCalculation.LatLngCalculationClass.get_home_location(vehicle) 166 | #print " ",home_lat," ",home_lng 167 | init_aircraft() 168 | elif (val == CommandConstant.CommandConstantClass.is_equal_read_channels()): 169 | DroneChannels.DroneChannelsClass.read_channels(vehicle) 170 | elif (val == CommandConstant.CommandConstantClass.is_equal_manual_fly()): 171 | rc_0 = int(obj_data['channel_0']) 172 | rc_1 = int(obj_data['channel_1']) 173 | rc_2 = int(obj_data['channel_2']) 174 | rc_3 = int(obj_data['channel_3']) 175 | rc_4 = int(obj_data['channel_4']) 176 | rc_5 = int(obj_data['channel_5']) 177 | rc_6 = int(obj_data['channel_6']) 178 | rc_7 = int(obj_data['channel_7']) 179 | RcController.RcControllerClass.send_rc_command(rc_0, rc_1, rc_2, rc_3, rc_4, rc_5, rc_6, rc_7) 180 | elif sender_user==User.UserClass.self_user(): 181 | cvb=0 182 | #print ("self message") 183 | except Exception as error: 184 | print (error) 185 | #set_command(data) 186 | my_client.on('chat message', socket_receiver) 187 | my_client.wait() 188 | #{"action":"wp","data":""} 189 | def cmddd(): 190 | # add a takeoff command 191 | cmds = vehicle.commands 192 | altitude = 100 # target altitude 193 | pitch = 45 # take off pitch. Need to check if degrees or radians, and what is a reasonable valued. 194 | cmd = Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, 195 | mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 196 | pitch, 0, 0, 0, 0, 0, altitude) 197 | cmds.add(cmd) 198 | vehicle.commands.upload() 199 | def arm_and_takeoff(aTargetAltitude,test_type): 200 | print("Basic pre-arm checks", aTargetAltitude) 201 | # Don't try to arm until autopilot is ready 202 | # while not vehicle.is_armable: 203 | # print(" Waiting for vehicle to initialise...") 204 | # time.sleep(1) 205 | print("Arming motors") 206 | vehicle.mode = VehicleMode("GUIDED") 207 | vehicle.armed = True 208 | while not vehicle.armed: 209 | print(" Waiting for arming...") 210 | time.sleep(1) 211 | print("Taking off!") 212 | vehicle.simple_takeoff(aTargetAltitude) 213 | while True: 214 | print(" Altitude: ", vehicle.location.global_relative_frame.alt) 215 | if vehicle.location.global_relative_frame.alt >= aTargetAltitude * 0.95: 216 | print("Reached target altitude") 217 | break 218 | time.sleep(1) 219 | if test_type==True: 220 | vehicle.mode = VehicleMode("AUTO") 221 | elif test_type==False: 222 | vehicle.mode = VehicleMode("LAND") 223 | def ai_receiver(): 224 | print "AI" 225 | def init_aircraft(): 226 | # api = local_connect() 227 | # vehicle = api.get_vehicles()[0] 228 | print "Global Location: %s" % vehicle.location.global_frame 229 | print "Global Location (relative altitude): %s" % vehicle.location.global_relative_frame 230 | print "Local Location: %s" % vehicle.location.local_frame 231 | print "Home Location: %s" % vehicle.home_location 232 | print "Home Location: %s" % vehicle.home_location 233 | print "Mode %s" % vehicle 234 | print "Battery %s" % vehicle.battery 235 | print "Mode %s" % vehicle.mode 236 | 237 | def run_thread(): 238 | thread_apm = threading.Thread(target=receive_battery_data) 239 | thread_apm.daemon = True 240 | thread_apm.start() 241 | 242 | thread_apm_location = threading.Thread(target=receive_location_data) 243 | thread_apm_location.daemon = True 244 | thread_apm_location.start() 245 | 246 | thread_receiver = threading.Thread(target=init_socket) 247 | thread_receiver.daemon = True 248 | thread_receiver.start() 249 | 250 | thread_sender = threading.Thread(target=init_sender_socket) 251 | thread_sender.daemon = True 252 | thread_sender.start() 253 | 254 | thread_mode_data_response = threading.Thread(target=receive_mode_data) 255 | thread_mode_data_response.daemon=True 256 | thread_mode_data_response.start() 257 | 258 | thread_mode_data_response = threading.Thread(target=receive_gps_info) 259 | thread_mode_data_response.daemon=True 260 | thread_mode_data_response.start() 261 | 262 | thread_all_data = threading.Thread(target=receive_all_data) 263 | thread_all_data.daemon = True 264 | thread_all_data.start() 265 | 266 | thread_all_data = threading.Thread(target=receive_all_data) 267 | thread_all_data.daemon = True 268 | thread_all_data.start() 269 | 270 | thread_ai = threading.Thread(target=ai_receiver) 271 | thread_ai.daemon = True 272 | thread_ai.start() 273 | if __name__ == '__main__': 274 | DbInitilize.DbInitializeClass.update_wp_status_false() 275 | run_thread() 276 | socketio.run(app) 277 | 278 | 279 | -------------------------------------------------------------------------------- /mavlink_lib.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | ''' 3 | mavproxy - a MAVLink proxy program 4 | Copyright Andrew Tridgell 2011 5 | Released under the GNU GPL version 3 or later 6 | ''' 7 | 8 | import sys, os, time, socket, signal 9 | import fnmatch, errno, threading 10 | import serial, Queue, select 11 | import traceback 12 | import select 13 | import shlex 14 | import platform 15 | import json 16 | 17 | from MAVProxy.modules.lib import textconsole 18 | from MAVProxy.modules.lib import rline 19 | from MAVProxy.modules.lib import mp_module 20 | from MAVProxy.modules.lib import dumpstacks 21 | 22 | # adding all this allows pyinstaller to build a working windows executable 23 | # note that using --hidden-import does not work for these modules 24 | 25 | from Constant import Server,CommandConstant 26 | from Utility import ProjectCommand,SdCard 27 | 28 | from flask import Flask, render_template 29 | from flask_socketio import SocketIO 30 | app = Flask(__name__) 31 | app.config['SECRET_KEY'] = Server.ServerClass.get_server_secret_key() 32 | socketio = SocketIO(app) 33 | from socketIO_client import SocketIO as client_socketio, BaseNamespace 34 | 35 | try: 36 | from multiprocessing import freeze_support 37 | from pymavlink import mavwp, mavutil 38 | import matplotlib, HTMLParser 39 | 40 | try: 41 | import readline 42 | except ImportError: 43 | import pyreadline as readline 44 | except Exception: 45 | pass 46 | 47 | if __name__ == '__main__': 48 | 49 | freeze_support() 50 | 51 | # The MAVLink version being used (None, "1.0", "2.0") 52 | mavversion = None 53 | 54 | 55 | class MPStatus(object): 56 | '''hold status information about the mavproxy''' 57 | 58 | def __init__(self): 59 | self.gps = None 60 | self.msgs = {} 61 | self.msg_count = {} 62 | self.counters = {'MasterIn': [], 'MasterOut': 0, 'FGearIn': 0, 'FGearOut': 0, 'Slave': 0} 63 | self.setup_mode = opts.setup 64 | self.mav_error = 0 65 | self.altitude = 0 66 | self.last_distance_announce = 0.0 67 | self.exit = False 68 | self.flightmode = 'MAV' 69 | self.last_mode_announce = 0 70 | self.last_mode_announced = 'MAV' 71 | self.logdir = None 72 | self.last_heartbeat = 0 73 | self.last_message = 0 74 | self.heartbeat_error = False 75 | self.last_apm_msg = None 76 | self.last_apm_msg_time = 0 77 | self.highest_msec = 0 78 | self.have_gps_lock = False 79 | self.lost_gps_lock = False 80 | self.last_gps_lock = 0 81 | self.watch = None 82 | self.last_streamrate1 = -1 83 | self.last_streamrate2 = -1 84 | self.last_seq = 0 85 | self.armed = False 86 | 87 | def show(self, f, pattern=None): 88 | '''write status to status.txt''' 89 | if pattern is None: 90 | f.write('Counters: ') 91 | for c in self.counters: 92 | f.write('%s:%s ' % (c, self.counters[c])) 93 | f.write('\n') 94 | f.write('MAV Errors: %u\n' % self.mav_error) 95 | f.write(str(self.gps) + '\n') 96 | for m in sorted(self.msgs.keys()): 97 | if pattern is not None and not fnmatch.fnmatch(str(m).upper(), pattern.upper()): 98 | continue 99 | f.write("%u: %s\n" % (self.msg_count[m], str(self.msgs[m]))) 100 | 101 | def write(self): 102 | '''write status to status.txt''' 103 | f = open('status.txt', mode='w') 104 | self.show(f) 105 | f.close() 106 | 107 | 108 | def say_text(text, priority='important'): 109 | '''text output - default function for say()''' 110 | mpstate.console.writeln(text) 111 | 112 | 113 | def say(text, priority='important'): 114 | '''text and/or speech output''' 115 | mpstate.functions.say(text, priority) 116 | 117 | 118 | def add_input(cmd, immediate=False): 119 | '''add some command input to be processed''' 120 | if immediate: 121 | process_stdin(cmd) 122 | else: 123 | mpstate.input_queue.put(cmd) 124 | 125 | 126 | class MAVFunctions(object): 127 | '''core functions available in modules''' 128 | 129 | def __init__(self): 130 | self.process_stdin = add_input 131 | self.param_set = param_set 132 | self.get_mav_param = get_mav_param 133 | self.say = say_text 134 | # input handler can be overridden by a module 135 | self.input_handler = None 136 | 137 | 138 | class MPState(object): 139 | '''holds state of mavproxy''' 140 | ################### Socket Server Start############### 141 | 142 | 143 | ################### Socket Server End############### 144 | 145 | def __init__(self): 146 | self.console = textconsole.SimpleConsole() 147 | self.map = None 148 | self.map_functions = {} 149 | self.vehicle_type = None 150 | self.vehicle_name = None 151 | from MAVProxy.modules.lib.mp_settings import MPSettings, MPSetting 152 | self.settings = MPSettings( 153 | [MPSetting('link', int, 1, 'Primary Link', tab='Link', range=(0, 4), increment=1), 154 | MPSetting('streamrate', int, 4, 'Stream rate link1', range=(-1, 100), increment=1), 155 | MPSetting('streamrate2', int, 4, 'Stream rate link2', range=(-1, 100), increment=1), 156 | MPSetting('heartbeat', int, 1, 'Heartbeat rate', range=(0, 5), increment=1), 157 | MPSetting('mavfwd', bool, True, 'Allow forwarded control'), 158 | MPSetting('mavfwd_rate', bool, False, 'Allow forwarded rate control'), 159 | MPSetting('shownoise', bool, True, 'Show non-MAVLink data'), 160 | MPSetting('baudrate', int, opts.baudrate, 'baudrate for new links', range=(0, 10000000), increment=1), 161 | MPSetting('rtscts', bool, opts.rtscts, 'enable flow control'), 162 | MPSetting('select_timeout', float, 0.01, 'select timeout'), 163 | 164 | MPSetting('altreadout', int, 10, 'Altitude Readout', 165 | range=(0, 100), increment=1, tab='Announcements'), 166 | MPSetting('distreadout', int, 200, 'Distance Readout', range=(0, 10000), increment=1), 167 | 168 | MPSetting('moddebug', int, opts.moddebug, 'Module Debug Level', range=(0, 3), increment=1, tab='Debug'), 169 | MPSetting('compdebug', int, 0, 'Computation Debug Mask', range=(0, 3), tab='Debug'), 170 | MPSetting('flushlogs', bool, False, 'Flush logs on every packet'), 171 | MPSetting('requireexit', bool, False, 'Require exit command'), 172 | MPSetting('wpupdates', bool, True, 'Announce waypoint updates'), 173 | 174 | MPSetting('basealt', int, 0, 'Base Altitude', range=(0, 30000), increment=1, tab='Altitude'), 175 | MPSetting('wpalt', int, 100, 'Default WP Altitude', range=(0, 10000), increment=1), 176 | MPSetting('rallyalt', int, 90, 'Default Rally Altitude', range=(0, 10000), increment=1), 177 | MPSetting('terrainalt', str, 'Auto', 'Use terrain altitudes', choice=['Auto', 'True', 'False']), 178 | MPSetting('rally_breakalt', int, 40, 'Default Rally Break Altitude', range=(0, 10000), increment=1), 179 | MPSetting('rally_flags', int, 0, 'Default Rally Flags', range=(0, 10000), increment=1), 180 | 181 | MPSetting('source_system', int, 255, 'MAVLink Source system', range=(0, 255), increment=1, tab='MAVLink'), 182 | MPSetting('source_component', int, 0, 'MAVLink Source component', range=(0, 255), increment=1), 183 | MPSetting('target_system', int, 0, 'MAVLink target system', range=(0, 255), increment=1), 184 | MPSetting('target_component', int, 0, 'MAVLink target component', range=(0, 255), increment=1), 185 | MPSetting('state_basedir', str, None, 'base directory for logs and aircraft directories'), 186 | MPSetting('allow_unsigned', bool, True, 'whether unsigned packets will be accepted'), 187 | 188 | MPSetting('dist_unit', str, 'm', 'distance unit', choice=['m', 'nm', 'miles'], tab='Units'), 189 | MPSetting('height_unit', str, 'm', 'height unit', choice=['m', 'feet']), 190 | MPSetting('speed_unit', str, 'm/s', 'height unit', choice=['m/s', 'knots', 'mph']), 191 | 192 | MPSetting('vehicle_name', str, '', 'Vehicle Name', tab='Vehicle'), 193 | ]) 194 | 195 | self.completions = { 196 | "script": ["(FILENAME)"], 197 | "set": ["(SETTING)"], 198 | "status": ["(VARIABLE)"], 199 | "module": ["list", 200 | "load (AVAILMODULES)", 201 | " (LOADEDMODULES)"] 202 | } 203 | 204 | self.status = MPStatus() 205 | 206 | # master mavlink device 207 | self.mav_master = None 208 | 209 | # mavlink outputs 210 | self.mav_outputs = [] 211 | self.sysid_outputs = {} 212 | 213 | # SITL output 214 | self.sitl_output = None 215 | 216 | self.mav_param = mavparm.MAVParmDict() 217 | self.modules = [] 218 | self.public_modules = {} 219 | self.functions = MAVFunctions() 220 | self.select_extra = {} 221 | self.continue_mode = False 222 | self.aliases = {} 223 | import platform 224 | self.system = platform.system() 225 | 226 | def module(self, name): 227 | '''Find a public module (most modules are private)''' 228 | if name in self.public_modules: 229 | return self.public_modules[name] 230 | return None 231 | 232 | def master(self): 233 | '''return the currently chosen mavlink master object''' 234 | if len(self.mav_master) == 0: 235 | return None 236 | if self.settings.link > len(self.mav_master): 237 | self.settings.link = 1 238 | 239 | # try to use one with no link error 240 | if not self.mav_master[self.settings.link - 1].linkerror: 241 | return self.mav_master[self.settings.link - 1] 242 | for m in self.mav_master: 243 | if not m.linkerror: 244 | return m 245 | return self.mav_master[self.settings.link - 1] 246 | 247 | 248 | def get_mav_param(param, default=None): 249 | '''return a EEPROM parameter value''' 250 | return mpstate.mav_param.get(param, default) 251 | 252 | 253 | def param_set(name, value, retries=3): 254 | '''set a parameter''' 255 | name = name.upper() 256 | return mpstate.mav_param.mavset(mpstate.master(), name, value, retries=retries) 257 | 258 | 259 | def cmd_script(args): 260 | '''run a script''' 261 | if len(args) < 1: 262 | print("usage: script ") 263 | return 264 | 265 | run_script(args[0]) 266 | 267 | 268 | def cmd_set(args): 269 | '''control mavproxy options''' 270 | mpstate.settings.command(args) 271 | 272 | 273 | def cmd_status(args): 274 | '''show status''' 275 | if len(args) == 0: 276 | mpstate.status.show(sys.stdout, pattern=None) 277 | else: 278 | for pattern in args: 279 | mpstate.status.show(sys.stdout, pattern=pattern) 280 | 281 | 282 | def cmd_setup(args): 283 | mpstate.status.setup_mode = True 284 | mpstate.rl.set_prompt("") 285 | 286 | 287 | def cmd_reset(args): 288 | print("Resetting master") 289 | mpstate.master().reset() 290 | 291 | 292 | def cmd_watch(args): 293 | '''watch a mavlink packet pattern''' 294 | if len(args) == 0: 295 | mpstate.status.watch = None 296 | return 297 | mpstate.status.watch = args[0] 298 | print("Watching %s" % mpstate.status.watch) 299 | 300 | 301 | def generate_kwargs(args): 302 | kwargs = {} 303 | module_components = args.split(":{", 1) 304 | module_name = module_components[0] 305 | if (len(module_components) == 2 and module_components[1].endswith("}")): 306 | # assume json 307 | try: 308 | module_args = "{" + module_components[1] 309 | kwargs = json.loads(module_args) 310 | except ValueError as e: 311 | print('Invalid JSON argument: {0} ({1})'.format(module_args, 312 | repr(e))) 313 | return (module_name, kwargs) 314 | 315 | 316 | def load_module(modname, quiet=False, **kwargs): 317 | '''load a module''' 318 | modpaths = ['MAVProxy.modules.mavproxy_%s' % modname, modname] 319 | for (m, pm) in mpstate.modules: 320 | if m.name == modname: 321 | if not quiet: 322 | print("module %s already loaded" % modname) 323 | return False 324 | for modpath in modpaths: 325 | try: 326 | m = import_package(modpath) 327 | reload(m) 328 | module = m.init(mpstate, **kwargs) 329 | if isinstance(module, mp_module.MPModule): 330 | mpstate.modules.append((module, m)) 331 | if not quiet: 332 | if kwargs: 333 | print("Loaded module %s with kwargs = %s" % (modname, kwargs)) 334 | else: 335 | print("Loaded module %s" % (modname,)) 336 | return True 337 | else: 338 | ex = "%s.init did not return a MPModule instance" % modname 339 | break 340 | except ImportError as msg: 341 | ex = msg 342 | if mpstate.settings.moddebug > 1: 343 | import traceback 344 | print(traceback.format_exc()) 345 | print("Failed to load module: %s. Use 'set moddebug 3' in the MAVProxy console to enable traceback" % ex) 346 | return False 347 | 348 | 349 | def unload_module(modname): 350 | '''unload a module''' 351 | for (m, pm) in mpstate.modules: 352 | if m.name == modname: 353 | if hasattr(m, 'unload'): 354 | m.unload() 355 | mpstate.modules.remove((m, pm)) 356 | print("Unloaded module %s" % modname) 357 | return True 358 | print("Unable to find module %s" % modname) 359 | return False 360 | 361 | 362 | def cmd_module(args): 363 | '''module commands''' 364 | usage = "usage: module " 365 | if len(args) < 1: 366 | print(usage) 367 | return 368 | if args[0] == "list": 369 | for (m, pm) in mpstate.modules: 370 | print("%s: %s" % (m.name, m.description)) 371 | elif args[0] == "load": 372 | if len(args) < 2: 373 | print("usage: module load ") 374 | return 375 | (modname, kwargs) = generate_kwargs(args[1]) 376 | try: 377 | load_module(modname, **kwargs) 378 | except TypeError: 379 | print("%s module does not support keyword arguments" % modname) 380 | return 381 | elif args[0] == "reload": 382 | if len(args) < 2: 383 | print("usage: module reload ") 384 | return 385 | (modname, kwargs) = generate_kwargs(args[1]) 386 | pmodule = None 387 | for (m, pm) in mpstate.modules: 388 | if m.name == modname: 389 | pmodule = pm 390 | if pmodule is None: 391 | print("Module %s not loaded" % modname) 392 | return 393 | if unload_module(modname): 394 | import zipimport 395 | try: 396 | reload(pmodule) 397 | except ImportError: 398 | clear_zipimport_cache() 399 | reload(pmodule) 400 | try: 401 | if load_module(modname, quiet=True, **kwargs): 402 | print("Reloaded module %s" % modname) 403 | except TypeError: 404 | print("%s module does not support keyword arguments" % modname) 405 | elif args[0] == "unload": 406 | if len(args) < 2: 407 | print("usage: module unload ") 408 | return 409 | modname = os.path.basename(args[1]) 410 | unload_module(modname) 411 | else: 412 | print(usage) 413 | 414 | 415 | def cmd_alias(args): 416 | '''alias commands''' 417 | usage = "usage: alias " 418 | if len(args) < 1 or args[0] == "list": 419 | if len(args) >= 2: 420 | wildcard = args[1].upper() 421 | else: 422 | wildcard = '*' 423 | for a in sorted(mpstate.aliases.keys()): 424 | if fnmatch.fnmatch(a.upper(), wildcard): 425 | print("%-15s : %s" % (a, mpstate.aliases[a])) 426 | elif args[0] == "add": 427 | if len(args) < 3: 428 | print(usage) 429 | return 430 | a = args[1] 431 | mpstate.aliases[a] = ' '.join(args[2:]) 432 | elif args[0] == "remove": 433 | if len(args) != 2: 434 | print(usage) 435 | return 436 | a = args[1] 437 | if a in mpstate.aliases: 438 | mpstate.aliases.pop(a) 439 | else: 440 | print("no alias %s" % a) 441 | else: 442 | print(usage) 443 | return 444 | 445 | 446 | def clear_zipimport_cache(): 447 | """Clear out cached entries from _zip_directory_cache. 448 | See http://www.digi.com/wiki/developer/index.php/Error_messages""" 449 | import sys, zipimport 450 | syspath_backup = list(sys.path) 451 | zipimport._zip_directory_cache.clear() 452 | 453 | # load back items onto sys.path 454 | sys.path = syspath_backup 455 | # add this too: see https://mail.python.org/pipermail/python-list/2005-May/353229.html 456 | sys.path_importer_cache.clear() 457 | 458 | 459 | # http://stackoverflow.com/questions/211100/pythons-import-doesnt-work-as-expected 460 | # has info on why this is necessary. 461 | 462 | def import_package(name): 463 | """Given a package name like 'foo.bar.quux', imports the package 464 | and returns the desired module.""" 465 | import zipimport 466 | try: 467 | mod = __import__(name) 468 | except ImportError: 469 | clear_zipimport_cache() 470 | mod = __import__(name) 471 | 472 | components = name.split('.') 473 | for comp in components[1:]: 474 | mod = getattr(mod, comp) 475 | return mod 476 | 477 | 478 | command_map = { 479 | 'script': (cmd_script, 'run a script of MAVProxy commands'), 480 | 'setup': (cmd_setup, 'go into setup mode'), 481 | 'reset': (cmd_reset, 'reopen the connection to the MAVLink master'), 482 | 'status': (cmd_status, 'show status'), 483 | 'set': (cmd_set, 'mavproxy settings'), 484 | 'watch': (cmd_watch, 'watch a MAVLink pattern'), 485 | 'module': (cmd_module, 'module commands'), 486 | 'alias': (cmd_alias, 'command aliases') 487 | } 488 | 489 | 490 | def shlex_quotes(value): 491 | '''see http://stackoverflow.com/questions/6868382/python-shlex-split-ignore-single-quotes''' 492 | lex = shlex.shlex(value) 493 | lex.quotes = '"' 494 | lex.whitespace_split = True 495 | lex.commenters = '' 496 | return list(lex) 497 | 498 | 499 | def process_stdin(line): 500 | '''handle commands from user''' 501 | if line is None: 502 | sys.exit(0) 503 | 504 | # allow for modules to override input handling 505 | if mpstate.functions.input_handler is not None: 506 | mpstate.functions.input_handler(line) 507 | return 508 | 509 | line = line.strip() 510 | 511 | if mpstate.status.setup_mode: 512 | # in setup mode we send strings straight to the master 513 | if line == '.': 514 | mpstate.status.setup_mode = False 515 | mpstate.status.flightmode = "MAV" 516 | mpstate.rl.set_prompt("MAV> ") 517 | return 518 | if line != '+++': 519 | line += '\r' 520 | for c in line: 521 | time.sleep(0.01) 522 | mpstate.master().write(c) 523 | return 524 | 525 | if not line: 526 | return 527 | 528 | args = shlex_quotes(line) 529 | cmd = args[0] 530 | while cmd in mpstate.aliases: 531 | line = mpstate.aliases[cmd] 532 | args = shlex.split(line) + args[1:] 533 | cmd = args[0] 534 | 535 | if cmd == 'help': 536 | k = command_map.keys() 537 | k.sort() 538 | for cmd in k: 539 | (fn, help) = command_map[cmd] 540 | print("%-15s : %s" % (cmd, help)) 541 | return 542 | if cmd == 'exit' and mpstate.settings.requireexit: 543 | mpstate.status.exit = True 544 | return 545 | 546 | if not cmd in command_map: 547 | for (m, pm) in mpstate.modules: 548 | if hasattr(m, 'unknown_command'): 549 | try: 550 | if m.unknown_command(args): 551 | return 552 | except Exception as e: 553 | print("ERROR in command: %s" % str(e)) 554 | print("Unknown command '%s'" % line) 555 | return 556 | (fn, help) = command_map[cmd] 557 | try: 558 | fn(args[1:]) 559 | except Exception as e: 560 | print("ERROR in command %s: %s" % (args[1:], str(e))) 561 | if mpstate.settings.moddebug > 1: 562 | traceback.print_exc() 563 | 564 | 565 | def process_master(m): 566 | '''process packets from the MAVLink master''' 567 | try: 568 | s = m.recv(16 * 1024) 569 | except Exception: 570 | time.sleep(0.1) 571 | return 572 | # prevent a dead serial port from causing the CPU to spin. The user hitting enter will 573 | # cause it to try and reconnect 574 | if len(s) == 0: 575 | time.sleep(0.1) 576 | return 577 | 578 | if (mpstate.settings.compdebug & 1) != 0: 579 | return 580 | 581 | if mpstate.logqueue_raw: 582 | mpstate.logqueue_raw.put(str(s)) 583 | 584 | if mpstate.status.setup_mode: 585 | if mpstate.system == 'Windows': 586 | # strip nsh ansi codes 587 | s = s.replace("\033[K", "") 588 | sys.stdout.write(str(s)) 589 | sys.stdout.flush() 590 | return 591 | 592 | global mavversion 593 | if m.first_byte and mavversion == None: 594 | m.auto_mavlink_version(s) 595 | msgs = m.mav.parse_buffer(s) 596 | if msgs: 597 | for msg in msgs: 598 | sysid = msg.get_srcSystem() 599 | if sysid in mpstate.sysid_outputs: 600 | # the message has been handled by a specialised handler for this system 601 | continue 602 | if getattr(m, '_timestamp', None) is None: 603 | m.post_message(msg) 604 | if msg.get_type() == "BAD_DATA": 605 | if opts.show_errors: 606 | mpstate.console.writeln("MAV error: %s" % msg) 607 | mpstate.status.mav_error += 1 608 | 609 | 610 | def process_mavlink(slave): 611 | '''process packets from MAVLink slaves, forwarding to the master''' 612 | try: 613 | buf = slave.recv() 614 | except socket.error: 615 | return 616 | try: 617 | global mavversion 618 | if slave.first_byte and mavversion == None: 619 | slave.auto_mavlink_version(buf) 620 | msgs = slave.mav.parse_buffer(buf) 621 | except mavutil.mavlink.MAVError as e: 622 | mpstate.console.error("Bad MAVLink slave message from %s: %s" % (slave.address, e.message)) 623 | return 624 | if msgs is None: 625 | return 626 | if mpstate.settings.mavfwd and not mpstate.status.setup_mode: 627 | for m in msgs: 628 | if mpstate.status.watch is not None: 629 | if fnmatch.fnmatch(m.get_type().upper(), mpstate.status.watch.upper()): 630 | mpstate.console.writeln('> ' + str(m)) 631 | mpstate.master().write(m.get_msgbuf()) 632 | mpstate.status.counters['Slave'] += 1 633 | 634 | 635 | def mkdir_p(dir): 636 | '''like mkdir -p''' 637 | if not dir: 638 | return 639 | if dir.endswith("/"): 640 | mkdir_p(dir[:-1]) 641 | return 642 | if os.path.isdir(dir): 643 | return 644 | mkdir_p(os.path.dirname(dir)) 645 | os.mkdir(dir) 646 | 647 | 648 | def log_writer(): 649 | '''log writing thread''' 650 | while True: 651 | mpstate.logfile_raw.write(mpstate.logqueue_raw.get()) 652 | timeout = time.time() + 10 653 | while not mpstate.logqueue_raw.empty() and time.time() < timeout: 654 | mpstate.logfile_raw.write(mpstate.logqueue_raw.get()) 655 | while not mpstate.logqueue.empty() and time.time() < timeout: 656 | mpstate.logfile.write(mpstate.logqueue.get()) 657 | if mpstate.settings.flushlogs or time.time() >= timeout: 658 | mpstate.logfile.flush() 659 | mpstate.logfile_raw.flush() 660 | 661 | 662 | # If state_basedir is NOT set then paths for logs and aircraft 663 | # directories are relative to mavproxy's cwd 664 | def log_paths(): 665 | '''Returns tuple (logdir, telemetry_log_filepath, raw_telemetry_log_filepath)''' 666 | if opts.aircraft is not None: 667 | dirname = "" 668 | if opts.mission is not None: 669 | print(opts.mission) 670 | dirname += "%s/logs/%s/Mission%s" % (opts.aircraft, time.strftime("%Y-%m-%d"), opts.mission) 671 | else: 672 | dirname += "%s/logs/%s" % (opts.aircraft, time.strftime("%Y-%m-%d")) 673 | # dirname is currently relative. Possibly add state_basedir: 674 | if mpstate.settings.state_basedir is not None: 675 | dirname = os.path.join(mpstate.settings.state_basedir, dirname) 676 | mkdir_p(dirname) 677 | highest = None 678 | for i in range(1, 10000): 679 | fdir = os.path.join(dirname, 'flight%u' % i) 680 | if not os.path.exists(fdir): 681 | break 682 | highest = fdir 683 | if mpstate.continue_mode and highest is not None: 684 | fdir = highest 685 | elif os.path.exists(fdir): 686 | print("Flight logs full") 687 | sys.exit(1) 688 | logname = 'flight.tlog' 689 | logdir = fdir 690 | else: 691 | logname = os.path.basename(opts.logfile) 692 | dir_path = os.path.dirname(opts.logfile) 693 | if not os.path.isabs(dir_path) and mpstate.settings.state_basedir is not None: 694 | dir_path = os.path.join(mpstate.settings.state_basedir, dir_path) 695 | 696 | logdir = dir_path 697 | 698 | mkdir_p(logdir) 699 | return (logdir, 700 | os.path.join(logdir, logname), 701 | os.path.join(logdir, logname + '.raw')) 702 | 703 | 704 | def open_telemetry_logs(logpath_telem, logpath_telem_raw): 705 | '''open log files''' 706 | if opts.append_log or opts.continue_mode: 707 | mode = 'a' 708 | else: 709 | mode = 'w' 710 | 711 | try: 712 | mpstate.logfile = open(logpath_telem, mode=mode) 713 | mpstate.logfile_raw = open(logpath_telem_raw, mode=mode) 714 | print("Log Directory: %s" % mpstate.status.logdir) 715 | print("Telemetry log: %s" % logpath_telem) 716 | 717 | # make sure there's enough free disk space for the logfile (>200Mb) 718 | # statvfs doesn't work in Windows 719 | if platform.system() != 'Windows': 720 | stat = os.statvfs(logpath_telem) 721 | if stat.f_bfree * stat.f_bsize < 209715200: 722 | print("ERROR: Not enough free disk space for logfile") 723 | mpstate.status.exit = True 724 | return 725 | 726 | # use a separate thread for writing to the logfile to prevent 727 | # delays during disk writes (important as delays can be long if camera 728 | # app is running) 729 | t = threading.Thread(target=log_writer, name='log_writer') 730 | t.daemon = True 731 | t.start() 732 | except Exception as e: 733 | print("ERROR: opening log file for writing: %s" % e) 734 | mpstate.status.exit = True 735 | return 736 | 737 | 738 | def set_stream_rates(): 739 | '''set mavlink stream rates''' 740 | if (not msg_period.trigger() and 741 | mpstate.status.last_streamrate1 == mpstate.settings.streamrate and 742 | mpstate.status.last_streamrate2 == mpstate.settings.streamrate2): 743 | return 744 | mpstate.status.last_streamrate1 = mpstate.settings.streamrate 745 | mpstate.status.last_streamrate2 = mpstate.settings.streamrate2 746 | for master in mpstate.mav_master: 747 | if master.linknum == 0: 748 | rate = mpstate.settings.streamrate 749 | else: 750 | rate = mpstate.settings.streamrate2 751 | if rate != -1: 752 | master.mav.request_data_stream_send(mpstate.settings.target_system, mpstate.settings.target_component, 753 | mavutil.mavlink.MAV_DATA_STREAM_ALL, 754 | rate, 1) 755 | 756 | 757 | def check_link_status(): 758 | '''check status of master links''' 759 | tnow = time.time() 760 | if mpstate.status.last_message != 0 and tnow > mpstate.status.last_message + 5: 761 | say("no link") 762 | mpstate.status.heartbeat_error = True 763 | for master in mpstate.mav_master: 764 | if not master.linkerror and (tnow > master.last_message + 5 or master.portdead): 765 | say("link %s down" % (mp_module.MPModule.link_label(master))) 766 | master.linkerror = True 767 | 768 | 769 | def send_heartbeat(master): 770 | if master.mavlink10(): 771 | master.mav.heartbeat_send(mavutil.mavlink.MAV_TYPE_GCS, mavutil.mavlink.MAV_AUTOPILOT_INVALID, 772 | 0, 0, 0) 773 | else: 774 | MAV_GROUND = 5 775 | MAV_AUTOPILOT_NONE = 4 776 | master.mav.heartbeat_send(MAV_GROUND, MAV_AUTOPILOT_NONE) 777 | 778 | 779 | def periodic_tasks(): 780 | '''run periodic checks''' 781 | if mpstate.status.setup_mode: 782 | return 783 | 784 | if (mpstate.settings.compdebug & 2) != 0: 785 | return 786 | 787 | if mpstate.settings.heartbeat != 0: 788 | heartbeat_period.frequency = mpstate.settings.heartbeat 789 | 790 | if heartbeat_period.trigger() and mpstate.settings.heartbeat != 0: 791 | mpstate.status.counters['MasterOut'] += 1 792 | for master in mpstate.mav_master: 793 | send_heartbeat(master) 794 | 795 | if heartbeat_check_period.trigger(): 796 | check_link_status() 797 | 798 | set_stream_rates() 799 | 800 | # call optional module idle tasks. These are called at several hundred Hz 801 | for (m, pm) in mpstate.modules: 802 | if hasattr(m, 'idle_task'): 803 | try: 804 | m.idle_task() 805 | except Exception as msg: 806 | if mpstate.settings.moddebug == 1: 807 | print(msg) 808 | elif mpstate.settings.moddebug > 1: 809 | exc_type, exc_value, exc_traceback = sys.exc_info() 810 | traceback.print_exception(exc_type, exc_value, exc_traceback, 811 | limit=2, file=sys.stdout) 812 | 813 | # also see if the module should be unloaded: 814 | if m.needs_unloading: 815 | unload_module(m.name) 816 | 817 | 818 | def main_loop(): 819 | '''main processing loop''' 820 | if not mpstate.status.setup_mode and not opts.nowait: 821 | for master in mpstate.mav_master: 822 | send_heartbeat(master) 823 | if master.linknum == 0: 824 | print("Waiting for heartbeat from %s" % master.address) 825 | master.wait_heartbeat() 826 | set_stream_rates() 827 | 828 | while True: 829 | if mpstate is None or mpstate.status.exit: 830 | return 831 | while not mpstate.input_queue.empty(): 832 | line = mpstate.input_queue.get() 833 | mpstate.input_count += 1 834 | cmds = line.split(';') 835 | if len(cmds) == 1 and cmds[0] == "": 836 | mpstate.empty_input_count += 1 837 | for c in cmds: 838 | process_stdin(c) 839 | 840 | for master in mpstate.mav_master: 841 | if master.fd is None: 842 | if master.port.inWaiting() > 0: 843 | process_master(master) 844 | 845 | periodic_tasks() 846 | 847 | rin = [] 848 | for master in mpstate.mav_master: 849 | if master.fd is not None and not master.portdead: 850 | rin.append(master.fd) 851 | for m in mpstate.mav_outputs: 852 | rin.append(m.fd) 853 | for sysid in mpstate.sysid_outputs: 854 | m = mpstate.sysid_outputs[sysid] 855 | rin.append(m.fd) 856 | if rin == []: 857 | time.sleep(0.0001) 858 | continue 859 | 860 | for fd in mpstate.select_extra: 861 | rin.append(fd) 862 | try: 863 | (rin, win, xin) = select.select(rin, [], [], mpstate.settings.select_timeout) 864 | except select.error: 865 | continue 866 | 867 | if mpstate is None: 868 | return 869 | 870 | for fd in rin: 871 | if mpstate is None: 872 | return 873 | for master in mpstate.mav_master: 874 | if fd == master.fd: 875 | process_master(master) 876 | if mpstate is None: 877 | return 878 | continue 879 | for m in mpstate.mav_outputs: 880 | if fd == m.fd: 881 | process_mavlink(m) 882 | if mpstate is None: 883 | return 884 | continue 885 | 886 | for sysid in mpstate.sysid_outputs: 887 | m = mpstate.sysid_outputs[sysid] 888 | if fd == m.fd: 889 | process_mavlink(m) 890 | if mpstate is None: 891 | return 892 | continue 893 | 894 | # this allow modules to register their own file descriptors 895 | # for the main select loop 896 | if fd in mpstate.select_extra: 897 | try: 898 | # call the registered read function 899 | (fn, args) = mpstate.select_extra[fd] 900 | fn(args) 901 | except Exception as msg: 902 | if mpstate.settings.moddebug == 1: 903 | print(msg) 904 | # on an exception, remove it from the select list 905 | mpstate.select_extra.pop(fd) 906 | 907 | 908 | def input_loop(): 909 | '''wait for user input''' 910 | while mpstate.status.exit != True: 911 | try: 912 | if mpstate.status.exit != True: 913 | line = raw_input(mpstate.rl.prompt) 914 | #print ("Borhan ",mpstate.rl.prompt) 915 | except EOFError: 916 | mpstate.status.exit = True 917 | sys.exit(1) 918 | #print("borhan user input", line) 919 | mpstate.input_queue.put(line) 920 | 921 | def set_command(command): 922 | mpstate.input_queue.put(command) 923 | def run_script(scriptfile): 924 | '''run a script file''' 925 | try: 926 | f = open(scriptfile, mode='r') 927 | except Exception: 928 | return 929 | mpstate.console.writeln("Running script %s" % scriptfile) 930 | for line in f: 931 | line = line.strip() 932 | if line == "" or line.startswith('#'): 933 | continue 934 | if line.startswith('@'): 935 | line = line[1:] 936 | else: 937 | mpstate.console.writeln("-> %s" % line) 938 | process_stdin(line) 939 | f.close() 940 | 941 | 942 | def set_mav_version(mav10, mav20, autoProtocol, mavversionArg): 943 | '''Set the Mavlink version based on commandline options''' 944 | # if(mav10 == True or mav20 == True or autoProtocol == True): 945 | # print("Warning: Using deprecated --mav10, --mav20 or --auto-protocol options. Use --mavversion instead") 946 | 947 | # sanity check the options 948 | if (mav10 == True or mav20 == True) and autoProtocol == True: 949 | print("Error: Can't have [--mav10, --mav20] and --auto-protocol both True") 950 | sys.exit(1) 951 | if mav10 == True and mav20 == True: 952 | print("Error: Can't have --mav10 and --mav20 both True") 953 | sys.exit(1) 954 | if mavversionArg is not None and (mav10 == True or mav20 == True or autoProtocol == True): 955 | print("Error: Can't use --mavversion with legacy (--mav10, --mav20 or --auto-protocol) options") 956 | sys.exit(1) 957 | 958 | # and set the specific mavlink version (False = autodetect) 959 | global mavversion 960 | if mavversionArg == "1.0" or mav10 == True: 961 | os.environ['MAVLINK09'] = '1' 962 | mavversion = "1" 963 | elif mavversionArg == "2.0" or mav20 == True: 964 | os.environ['MAVLINK20'] = '1' 965 | mavversion = "2" 966 | else: 967 | mavversion = None 968 | def init_socket(): 969 | my_client = client_socketio(Server.ServerClass.get_server_ip(), Server.ServerClass.get_server_port()) 970 | @socketio.on('chat message') 971 | def handle_message(message): 972 | print('received message init: ' + message) 973 | 974 | def socker_receiver(data): 975 | try: 976 | print (data) 977 | #vehicle = mpstate.get_vehicles()[0] 978 | obj_data = json.loads(data.replace('\r\n', '\\r\\n'),strict=False) 979 | #obj_data = json.loads(data,strict=False) 980 | #obj_data = json.loads(data) 981 | sender_user = obj_data['u'] 982 | if(sender_user=='ground'): 983 | val = obj_data['action'] 984 | if (val == CommandConstant.CommandConstantClass.is_equal_wp()): 985 | master = mpstate.master() 986 | lat = master.field('GLOBAL_POSITION_INT', 'lat', 0) * 1.0e-7 987 | lng = master.field('GLOBAL_POSITION_INT', 'lon', 0) * 1.0e-7 988 | curr_alt = mpstate.status.altitude 989 | print ("current alt ",curr_alt) 990 | print("this is way point", lat, " ", lng) 991 | wp_data = obj_data['data'] 992 | SdCard.SdCardClass.file_write(wp_data) 993 | 994 | elif (val == CommandConstant.CommandConstantClass.is_equal_arm()): 995 | print ('command arm') 996 | set_command(ProjectCommand.CommandClass.get_arm_string(1)) 997 | elif sender_user=='eagle': 998 | print ("self message") 999 | 1000 | 1001 | except Exception as error: 1002 | print (error) 1003 | #set_command(data) 1004 | my_client.on('chat message', socker_receiver) 1005 | my_client.wait() 1006 | #{"action":"wp","data":""} 1007 | if __name__ == '__main__': 1008 | thread = threading.Thread(target=init_socket) 1009 | thread.daemon = True 1010 | thread.start() 1011 | from optparse import OptionParser 1012 | parser = OptionParser("mavproxy.py [options]") 1013 | 1014 | parser.add_option("--master", dest="master", action='append', 1015 | metavar="DEVICE[,BAUD]", help="MAVLink master port and optional baud rate", 1016 | default=[]) 1017 | parser.add_option("--out", dest="output", action='append', 1018 | metavar="DEVICE[,BAUD]", help="MAVLink output port and optional baud rate", 1019 | default=[]) 1020 | parser.add_option("--baudrate", dest="baudrate", type='int', 1021 | help="default serial baud rate", default=57600) 1022 | parser.add_option("--sitl", dest="sitl", default=None, help="SITL output port") 1023 | parser.add_option("--streamrate", dest="streamrate", default=4, type='int', 1024 | help="MAVLink stream rate") 1025 | parser.add_option("--source-system", dest='SOURCE_SYSTEM', type='int', 1026 | default=255, help='MAVLink source system for this GCS') 1027 | parser.add_option("--source-component", dest='SOURCE_COMPONENT', type='int', 1028 | default=0, help='MAVLink source component for this GCS') 1029 | parser.add_option("--target-system", dest='TARGET_SYSTEM', type='int', 1030 | default=0, help='MAVLink target master system') 1031 | parser.add_option("--target-component", dest='TARGET_COMPONENT', type='int', 1032 | default=0, help='MAVLink target master component') 1033 | parser.add_option("--logfile", dest="logfile", help="MAVLink master logfile", 1034 | default='mav.tlog') 1035 | parser.add_option("-a", "--append-log", dest="append_log", help="Append to log files", 1036 | action='store_true', default=False) 1037 | parser.add_option("--quadcopter", dest="quadcopter", help="use quadcopter controls", 1038 | action='store_true', default=False) 1039 | parser.add_option("--setup", dest="setup", help="start in setup mode", 1040 | action='store_true', default=False) 1041 | parser.add_option("--nodtr", dest="nodtr", help="disable DTR drop on close", 1042 | action='store_true', default=False) 1043 | parser.add_option("--show-errors", dest="show_errors", help="show MAVLink error packets", 1044 | action='store_true', default=False) 1045 | parser.add_option("--speech", dest="speech", help="use text to speech", 1046 | action='store_true', default=False) 1047 | parser.add_option("--aircraft", dest="aircraft", help="aircraft name", default=None) 1048 | parser.add_option("--cmd", dest="cmd", help="initial commands", default=None, action='append') 1049 | parser.add_option("--console", action='store_true', help="use GUI console") 1050 | parser.add_option("--map", action='store_true', help="load map module") 1051 | parser.add_option( 1052 | '--load-module', 1053 | action='append', 1054 | default=[], 1055 | help='Load the specified module. Can be used multiple times, or with a comma separated list') 1056 | parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0") 1057 | parser.add_option("--mav20", action='store_true', default=False, help="Use MAVLink protocol 2.0") 1058 | parser.add_option("--auto-protocol", action='store_true', default=False, 1059 | help="Auto detect MAVLink protocol version") 1060 | parser.add_option("--mavversion", type='choice', choices=['1.0', '2.0'], 1061 | help="Force MAVLink Version (1.0, 2.0). Otherwise autodetect version") 1062 | parser.add_option("--nowait", action='store_true', default=False, help="don't wait for HEARTBEAT on startup") 1063 | parser.add_option("-c", "--continue", dest='continue_mode', action='store_true', default=False, 1064 | help="continue logs") 1065 | parser.add_option("--dialect", default="ardupilotmega", help="MAVLink dialect") 1066 | parser.add_option("--rtscts", action='store_true', help="enable hardware RTS/CTS flow control") 1067 | parser.add_option("--moddebug", type=int, help="module debug level", default=0) 1068 | parser.add_option("--mission", dest="mission", help="mission name", default=None) 1069 | parser.add_option("--daemon", action='store_true', help="run in daemon mode, do not start interactive shell") 1070 | parser.add_option("--non-interactive", action='store_true', help="do not start interactive shell") 1071 | parser.add_option("--profile", action='store_true', help="run the Yappi python profiler") 1072 | parser.add_option("--state-basedir", default=None, help="base directory for logs and aircraft directories") 1073 | parser.add_option("--version", action='store_true', help="version information") 1074 | parser.add_option("--default-modules", 1075 | default="log,signing,wp,rally,fence,param,relay,tuneopt,arm,mode,calibration,RC,auxopt,misc,cmdlong,battery,terrain,output,adsb", 1076 | help='default module list') 1077 | 1078 | (opts, args) = parser.parse_args() 1079 | if len(args) != 0: 1080 | print("ERROR: mavproxy takes no position arguments; got (%s)" % str(args)) 1081 | sys.exit(1) 1082 | 1083 | # warn people about ModemManager which interferes badly with APM and Pixhawk 1084 | if os.path.exists("/usr/sbin/ModemManager"): 1085 | print("WARNING: You should uninstall ModemManager as it conflicts with APM and Pixhawk") 1086 | 1087 | # set the Mavlink version, if required 1088 | set_mav_version(opts.mav10, opts.mav20, opts.auto_protocol, opts.mavversion) 1089 | 1090 | from pymavlink import mavutil, mavparm 1091 | 1092 | mavutil.set_dialect(opts.dialect) 1093 | 1094 | # version information 1095 | if opts.version: 1096 | # pkg_resources doesn't work in the windows exe build, so read the version file 1097 | try: 1098 | import pkg_resources 1099 | 1100 | version = pkg_resources.require("mavproxy")[0].version 1101 | except: 1102 | start_script = os.path.join(os.environ['LOCALAPPDATA'], "MAVProxy", "version.txt") 1103 | f = open(start_script, 'r') 1104 | version = f.readline() 1105 | 1106 | print("MAVProxy is a modular ground station using the mavlink protocol") 1107 | print("MAVProxy Version: " + version) 1108 | sys.exit(1) 1109 | 1110 | # global mavproxy state 1111 | mpstate = MPState() 1112 | mpstate.status.exit = False 1113 | mpstate.command_map = command_map 1114 | mpstate.continue_mode = opts.continue_mode 1115 | # queues for logging 1116 | mpstate.logqueue = Queue.Queue() 1117 | mpstate.logqueue_raw = Queue.Queue() 1118 | 1119 | if opts.speech: 1120 | # start the speech-dispatcher early, so it doesn't inherit any ports from 1121 | # modules/mavutil 1122 | load_module('speech') 1123 | 1124 | serial_list = mavutil.auto_detect_serial(preferred_list=[ 1125 | '*FTDI*', 1126 | "*Arduino_Mega_2560*", 1127 | "*3D_Robotics*", 1128 | "*USB_to_UART*", 1129 | '*Ardu*', 1130 | '*PX4*', 1131 | '*FMU*']) 1132 | if not opts.master: 1133 | print('Auto-detected serial ports are:') 1134 | for port in serial_list: 1135 | print("%s" % port) 1136 | 1137 | # container for status information 1138 | mpstate.settings.target_system = opts.TARGET_SYSTEM 1139 | mpstate.settings.target_component = opts.TARGET_COMPONENT 1140 | 1141 | mpstate.mav_master = [] 1142 | 1143 | mpstate.rl = rline.rline("MAV> ", mpstate) 1144 | 1145 | 1146 | def quit_handler(signum=None, frame=None): 1147 | # print('Signal handler called with signal', signum) 1148 | if mpstate.status.exit: 1149 | print('Clean shutdown impossible, forcing an exit') 1150 | sys.exit(0) 1151 | else: 1152 | mpstate.status.exit = True 1153 | 1154 | 1155 | # Listen for kill signals to cleanly shutdown modules 1156 | fatalsignals = [signal.SIGTERM] 1157 | try: 1158 | fatalsignals.append(signal.SIGHUP) 1159 | fatalsignals.append(signal.SIGQUIT) 1160 | except Exception: 1161 | pass 1162 | if opts.daemon or opts.non_interactive: # SIGINT breaks readline parsing - if we are interactive, just let things die 1163 | fatalsignals.append(signal.SIGINT) 1164 | 1165 | for sig in fatalsignals: 1166 | signal.signal(sig, quit_handler) 1167 | 1168 | load_module('link', quiet=True) 1169 | 1170 | mpstate.settings.source_system = opts.SOURCE_SYSTEM 1171 | mpstate.settings.source_component = opts.SOURCE_COMPONENT 1172 | 1173 | # open master link 1174 | for mdev in opts.master: 1175 | if not mpstate.module('link').link_add(mdev): 1176 | sys.exit(1) 1177 | 1178 | if not opts.master and len(serial_list) == 1: 1179 | print("Connecting to %s" % serial_list[0]) 1180 | mpstate.module('link').link_add(serial_list[0].device) 1181 | elif not opts.master and len(serial_list) > 1: 1182 | print("Error: multiple possible serial ports; use --master to select a single port") 1183 | sys.exit(1) 1184 | elif not opts.master: 1185 | wifi_device = '0.0.0.0:14550' 1186 | mpstate.module('link').link_add(wifi_device) 1187 | 1188 | # open any mavlink output ports 1189 | for port in opts.output: 1190 | mpstate.mav_outputs.append(mavutil.mavlink_connection(port, baud=int(opts.baudrate), input=False)) 1191 | 1192 | if opts.sitl: 1193 | mpstate.sitl_output = mavutil.mavudp(opts.sitl, input=False) 1194 | 1195 | mpstate.settings.streamrate = opts.streamrate 1196 | mpstate.settings.streamrate2 = opts.streamrate 1197 | 1198 | if opts.state_basedir is not None: 1199 | mpstate.settings.state_basedir = opts.state_basedir 1200 | 1201 | msg_period = mavutil.periodic_event(1.0 / 15) 1202 | heartbeat_period = mavutil.periodic_event(1) 1203 | heartbeat_check_period = mavutil.periodic_event(0.33) 1204 | 1205 | mpstate.input_queue = Queue.Queue() 1206 | mpstate.input_count = 0 1207 | mpstate.empty_input_count = 0 1208 | if opts.setup: 1209 | mpstate.rl.set_prompt("") 1210 | 1211 | # call this early so that logdir is setup based on --aircraft 1212 | (mpstate.status.logdir, logpath_telem, logpath_telem_raw) = log_paths() 1213 | 1214 | for module in opts.load_module: 1215 | modlist = module.split(',') 1216 | for mod in modlist: 1217 | process_stdin('module load %s' % (mod)) 1218 | 1219 | if not opts.setup: 1220 | # some core functionality is in modules 1221 | standard_modules = opts.default_modules.split(',') 1222 | for m in standard_modules: 1223 | load_module(m, quiet=True) 1224 | 1225 | if opts.console: 1226 | process_stdin('module load console') 1227 | 1228 | if opts.map: 1229 | process_stdin('module load map') 1230 | 1231 | start_scripts = [] 1232 | if 'HOME' in os.environ and not opts.setup: 1233 | start_script = os.path.join(os.environ['HOME'], ".mavinit.scr") 1234 | start_scripts.append(start_script) 1235 | if 'LOCALAPPDATA' in os.environ and not opts.setup: 1236 | start_script = os.path.join(os.environ['LOCALAPPDATA'], "MAVProxy", "mavinit.scr") 1237 | start_scripts.append(start_script) 1238 | if (mpstate.settings.state_basedir is not None and 1239 | opts.aircraft is not None): 1240 | start_script = os.path.join(mpstate.settings.state_basedir, opts.aircraft, "mavinit.scr") 1241 | start_scripts.append(start_script) 1242 | for start_script in start_scripts: 1243 | if os.path.exists(start_script): 1244 | print("Running script (%s)" % (start_script)) 1245 | run_script(start_script) 1246 | 1247 | if opts.aircraft is not None: 1248 | start_script = os.path.join(opts.aircraft, "mavinit.scr") 1249 | if os.path.exists(start_script): 1250 | run_script(start_script) 1251 | else: 1252 | print("no script %s" % start_script) 1253 | 1254 | if opts.cmd is not None: 1255 | for cstr in opts.cmd: 1256 | cmds = cstr.split(';') 1257 | for c in cmds: 1258 | process_stdin(c) 1259 | 1260 | if opts.profile: 1261 | import yappi # We do the import here so that we won't barf if run normally and yappi not available 1262 | 1263 | yappi.start() 1264 | 1265 | # log all packets from the master, for later replay 1266 | open_telemetry_logs(logpath_telem, logpath_telem_raw) 1267 | 1268 | # run main loop as a thread 1269 | mpstate.status.thread = threading.Thread(target=main_loop, name='main_loop') 1270 | mpstate.status.thread.daemon = True 1271 | mpstate.status.thread.start() 1272 | 1273 | # use main program for input. This ensures the terminal cleans 1274 | # up on exit 1275 | while (mpstate.status.exit != True): 1276 | try: 1277 | if opts.daemon or opts.non_interactive: 1278 | time.sleep(0.1) 1279 | else: 1280 | input_loop() 1281 | except KeyboardInterrupt: 1282 | if mpstate.settings.requireexit: 1283 | print("Interrupt caught. Use 'exit' to quit MAVProxy.") 1284 | 1285 | # Just lost the map and console, get them back: 1286 | for (m, pm) in mpstate.modules: 1287 | if m.name in ["map", "console"]: 1288 | if hasattr(m, 'unload'): 1289 | try: 1290 | m.unload() 1291 | except Exception: 1292 | pass 1293 | reload(m) 1294 | m.init(mpstate) 1295 | 1296 | else: 1297 | mpstate.status.exit = True 1298 | sys.exit(1) 1299 | 1300 | if opts.profile: 1301 | yappi.get_func_stats().print_all() 1302 | yappi.get_thread_stats().print_all() 1303 | 1304 | # this loop executes after leaving the above loop and is for cleanup on exit 1305 | for (m, pm) in mpstate.modules: 1306 | if hasattr(m, 'unload'): 1307 | print("Unloading module %s" % m.name) 1308 | m.unload() 1309 | 1310 | socketio.run(app) 1311 | sys.exit(1) -------------------------------------------------------------------------------- /onesensor_01.py: -------------------------------------------------------------------------------- 1 | from __future__ import division 2 | import time 3 | import RPi.GPIO as GPIO 4 | import Adafruit_PCA9685 5 | from flask import Flask, render_template 6 | from flask_socketio import SocketIO 7 | from dronekit import connect, VehicleMode, LocationGlobalRelative, Battery, GPSInfo, Command 8 | vehicle = connect('/dev/ttyACM0', wait_ready=False) 9 | vehicle.mode = VehicleMode("GUIDED") 10 | while not vehicle.armed: 11 | print("Waiting for arming..:% s" % vehicle.mode) 12 | vehicle.armed = True 13 | time.sleep(1) 14 | 15 | app = Flask(__name__) 16 | app.config['SECRET_KEY'] = 'secret!' 17 | socketio = SocketIO(app) 18 | 19 | i = -1 20 | GPIO.setmode(GPIO.BOARD) 21 | GPIO.setup(12, GPIO.OUT) 22 | 23 | p = GPIO.PWM(12, 60) # channel=12 frequency=50Hz 24 | p.start(0) 25 | 26 | 27 | def initGpioPwm(x): 28 | if x == 1: 29 | p.ChangeDutyCycle(1) 30 | time.sleep(0.1) 31 | elif x == 2: 32 | p.ChangeDutyCycle(3.5) 33 | time.sleep(0.1) 34 | if x == 3: 35 | p.ChangeDutyCycle(4.4) 36 | # time.sleep(0.5) 37 | elif x == 4: 38 | p.ChangeDutyCycle(5.2) 39 | # time.sleep(0.5) 40 | elif x == 5: 41 | p.ChangeDutyCycle(60) 42 | # time.sleep(0.5) 43 | elif x == 6: 44 | p.ChangeDutyCycle(6.8) 45 | # time.sleep(0.5) 46 | elif x == 7: 47 | p.ChangeDutyCycle(7.7) 48 | # time.sleep(0.5) 49 | elif x == 5: 50 | p.ChangeDutyCycle(90) 51 | # time.sleep(0.5) 52 | else: 53 | x = 0 54 | # print("nothings") 55 | # Do the default 56 | 57 | 58 | def reconnect_io_func(): 59 | try: 60 | pwm = Adafruit_PCA9685.PCA9685() 61 | return pwm 62 | except Exception as error: 63 | if "Remote I/O error" in (error): 64 | reconnect_io = True 65 | while reconnect_io: 66 | try: 67 | # print("while Error: "+str(error)) 68 | pwm = Adafruit_PCA9685.PCA9685() 69 | # print(pwm) 70 | reconnect_io = False 71 | return pwm 72 | except Exception as error: 73 | # print((error)) 74 | reconnect_io = True 75 | 76 | 77 | servo_min = 150 # Min pulse length out of 4096 78 | servo_max = 600 # Max pulse length out of 4096 79 | pwm = reconnect_io_func() 80 | from socketIO_client import SocketIO as client_socketio, BaseNamespace 81 | 82 | my_client = client_socketio('184.72.95.87', 3000) 83 | str = "" 84 | 85 | 86 | @socketio.on('chat message') 87 | def handle_message(message): 88 | x = 0 89 | # print('received message: ' + message) 90 | 91 | 92 | # my_client.emit('chat message', str) 93 | # print(str) 94 | def doSomething(data): 95 | alphabet = data 96 | dataStr = alphabet.split(":") # split string into a list 97 | i = 0 98 | # pwm = reconnect_io_func() 99 | for temp in dataStr: 100 | print temp 101 | print("Waiting for arming..:% s" % vehicle.mode) 102 | # print data 103 | # val= int(data) 104 | 105 | if i == 0: 106 | val = int(temp) 107 | try: 108 | pwm.set_pwm(0, 0, val) 109 | except Exception as error: 110 | x = 0 111 | # print("error") 112 | # pwm.set_pwm(4, 0, 52) 113 | elif i == 1: 114 | val = int(temp) 115 | pwm.set_pwm(1, 0, val) 116 | elif i == 2: 117 | val = int(temp) 118 | pwm.set_pwm(2, 0, val) 119 | elif i == 3: 120 | val = int(temp) 121 | pwm.set_pwm(3, 0, val) 122 | elif i == 4: 123 | val = int(temp) 124 | pwm.set_pwm(4, 0, val) 125 | elif i == 5: 126 | val = int(temp) 127 | pwm.set_pwm(5, 0, val) 128 | elif i == 6: 129 | val = int(temp) 130 | pwm.set_pwm(6, 0, val) 131 | elif i == 7: 132 | val = int(temp) 133 | #vehicle.mode = VehicleMode("GUIDED") 134 | #time.sleep(5) 135 | pwm.set_pwm(7, 0, val) 136 | # elif i==8: 137 | # val=int(temp) 138 | # initGpioPwm(val) 139 | else: 140 | x = 0 141 | # print ("Drone Nothing") 142 | # print (temp) 143 | # print (i) 144 | i = i + 1 145 | # pwm.set_pwm(0, 0, servo_min) 146 | # time.sleep(1) 147 | # pwm.set_pwm(0, 0, servo_max) 148 | # time.sleep(1) 149 | 150 | 151 | def set_servo_pulse(channel, pulse): 152 | pulse_length = 1000000 # 1,000,000 us per second 153 | pulse_length //= 60 # 60 Hz 154 | # print('{0}us per period'.format(pulse_length)) 155 | pulse_length //= 4096 # 12 bits of resolution 156 | # print('{0}us per bit'.format(pulse_length)) 157 | pulse *= 1000 158 | pulse //= pulse_length 159 | pwm.set_pwm(channel, 0, pulse) 160 | 161 | 162 | # Set frequency to 60hz, good for servos. 163 | # pwm = reconnect_io_func() 164 | pwm.set_pwm_freq(60) 165 | # pwm.set_pwm(0, 0, 420) 166 | # pwm.set_pwm(1, 0, 420) 167 | # pwm.set_pwm(2, 0, 307) 168 | # pwm.set_pwm(3, 0, 420) 169 | # pwm.set_pwm(4, 0, 307) 170 | # pwm.set_pwm(5, 0, 307) 171 | # pwm.set_pwm(6, 0, 307) 172 | # pwm.set_pwm(7, 0, 307) 173 | # pwm.set_pwm(8, 0, 0) 174 | # my_client.emit('chat message', "420:420:307:420:307:307:307:307:0") 175 | 176 | my_client.on('chat message', doSomething) 177 | my_client.wait() 178 | 179 | if __name__ == '__main__': 180 | socketio.run(app) 181 | -------------------------------------------------------------------------------- /run.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | python mavlink.py --master=/dev/ttyACM0 --baudrate 115200 --aircraft MyCopter 4 | #python mavlink_lib.py --master=/dev/ttyACM0 --baudrate 115200 --aircraft MyCopter 5 | 6 | -------------------------------------------------------------------------------- /t2.py: -------------------------------------------------------------------------------- 1 | from dronekit import connect, VehicleMode, LocationGlobalRelative 2 | from pymavlink import mavutil 3 | import time 4 | 5 | # Receiving from command line 6 | # import argparse 7 | # parser = argparse.ArgumentParser() 8 | # parser.add_argument('--connect',help="PORT_NO") 9 | # args = parser.parse_args() 10 | 11 | # Connecting to the vehicle 12 | # connection_string = args.connect 13 | connection_string = "/dev/ttyAMA0,57600" 14 | print("Connecting to...% s" % connection_string) 15 | vehicle = connect('/dev/ttyACM0', wait_ready=False) 16 | 17 | # Function to arm and takeoff to a specified altitude 18 | print(vehicle.mode) 19 | #vehicle.mode = VehicleMode("STABILIZE") 20 | vehicle.mode = VehicleMode("GUIDED_NOGPS") 21 | while not vehicle.armed: 22 | print("Waiting for arming..:% s" % vehicle.mode) 23 | vehicle.armed = True 24 | time.sleep(1) 25 | 26 | print("Taking off!") 27 | # while True: 28 | # print("Altitude: ", vehicle.location.global_relative_frame.alt) 29 | # time.sleep(0.5) -------------------------------------------------------------------------------- /test.py: -------------------------------------------------------------------------------- 1 | # /usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | from dronekit import connect, VehicleMode, LocationGlobalRelative 4 | from pymavlink import mavutil 5 | import time 6 | 7 | # Receiving from command line 8 | # import argparse 9 | # parser = argparse.ArgumentParser() 10 | # parser.add_argument('--connect',help="PORT_NO") 11 | # args = parser.parse_args() 12 | 13 | # Connecting to the vehicle 14 | # connection_string = args.connect 15 | connection_string = "/dev/ttyAMA0,57600" 16 | print("Connecting to...% s" % connection_string) 17 | vehicle = connect('/dev/ttyACM0', wait_ready=False) 18 | 19 | # Function to arm and takeoff to a specified altitude 20 | print(vehicle.mode) 21 | vehicle.mode = VehicleMode("STABILIZE") 22 | print "ALT ", vehicle.location.global_relative_frame.alt 23 | time.sleep(5) 24 | 25 | # vehicle.mode = VehicleMode("LAND") 26 | # time.sleep(5) 27 | def arm_and_takeoff(aTargetAlt): 28 | print("Basic Prearm checks..dont touch!!") 29 | 30 | # while not vehicle.is_armable: 31 | # print("Waiting for vehicle to initialize") 32 | # time.sleep(2) 33 | 34 | print("Arming Motors..") 35 | # Copter should arm in Guided-mode 36 | vehicle.mode = VehicleMode("GUIDED") 37 | time.sleep(3) 38 | vehicle.armed = True 39 | 40 | while not vehicle.armed: 41 | print("Waiting for arming..:% s" % vehicle.mode) 42 | vehicle.armed = True 43 | time.sleep(2) 44 | 45 | print("Taking Off..") 46 | vehicle.simple_takeoff(aTargetAlt) 47 | time.sleep(5) 48 | 49 | while True: 50 | print("Altitude: ", vehicle.location.global_relative_frame.alt) 51 | # Break and return from function just below target altitude. 52 | if vehicle.location.global_relative_frame.alt >= aTargetAlt * 0.95: 53 | print("Reached Target Altitude..") 54 | modeLand() 55 | # print("Landing....") 56 | # vehicle.mode = VehicleMode("LAND"); 57 | # print("Vehicle mode:%s"% vehicle.mode) 58 | break 59 | 60 | 61 | def modeLand(): 62 | print("Landing now") 63 | vehicle.mode = VehicleMode("LAND") 64 | time.sleep(5) 65 | print("-------------Vehicle is in:-------%s" % vehicle.mode) 66 | 67 | 68 | arm_and_takeoff(2) 69 | print(vehicle.mode) 70 | print("Vehicle object closed") 71 | vehicle.close() --------------------------------------------------------------------------------