00001
00002
00003 from dji_sdk.dji_drone import DJIDrone
00004 import dji_sdk.msg
00005 import time
00006 import sys
00007 import math
00008
00009 def display_main_menu():
00010 print "----------- < Main menu > ----------"
00011 print "[a] Request to obtain control"
00012 print "[b] Release control"
00013 print "[c] Takeoff"
00014 print "[d] Landing"
00015 print "[e] Go home"
00016 print "[f] Gimbal control sample"
00017 print "[g] Attitude control sample"
00018 print "[h] Draw circle sample"
00019 print "[i] Draw square sample"
00020 print "[j] Take a picture"
00021 print "[k] Start video"
00022 print "[l] Stop video"
00023 print "[m] Local Navi Test"
00024 print "[n] GPS Navi Test"
00025 print "[o] Waypoint List Test"
00026 print "[p] Arm Test"
00027 print "[q] Disarm Test"
00028 print "[r] Vrc Test"
00029 print "[s] Sync Test"
00030 print "[t] Exit"
00031 print "\ninput a/b/c etc..then press enter key"
00032 print "\nuse `rostopic echo` to query drone status"
00033 print "----------------------------------------"
00034 print "input: "
00035
00036 def main():
00037 drone = DJIDrone()
00038 display_main_menu()
00039 while True:
00040 main_operate_code = sys.stdin.read(1)
00041 if main_operate_code == 'a':
00042 drone.request_sdk_permission_control()
00043 elif main_operate_code == 'b':
00044 drone.release_sdk_permission_control()
00045 elif main_operate_code == 'c':
00046 drone.takeoff()
00047 elif main_operate_code == 'd':
00048 drone.landing()
00049 elif main_operate_code =='e':
00050 drone.gohome()
00051 elif main_operate_code == 'f':
00052 drone.gimbal_angle_control(0, 0, 0, 20)
00053 time.sleep(2)
00054 drone.gimbal_angle_control(0, 0, 1800, 20)
00055 time.sleep(2)
00056 drone.gimbal_angle_control(0, 0, -1800, 20)
00057 time.sleep(2)
00058 drone.gimbal_angle_control(300, 0, 0, 20)
00059 time.sleep(2)
00060 drone.gimbal_angle_control(-300, 0, 0, 20)
00061 time.sleep(2)
00062 drone.gimbal_angle_control(0, 300, 0, 20)
00063 time.sleep(2)
00064 drone.gimbal_angle_control(0, -300, 0, 20)
00065 time.sleep(2)
00066 drone.gimbal_speed_control(100, 0, 0)
00067 time.sleep(2)
00068 drone.gimbal_speed_control(-100, 0, 0)
00069 time.sleep(2)
00070 drone.gimbal_speed_control(0, 0, 200)
00071 time.sleep(2)
00072 drone.gimbal_speed_control(0, 0, -200)
00073 time.sleep(2)
00074 drone.gimbal_speed_control(0, 200, 0)
00075 time.sleep(2)
00076 drone.gimbal_speed_control(0, -200, 0)
00077 time.sleep(2)
00078 drone.gimbal_angle_control(0, 0, 0, 20)
00079 elif main_operate_code == 'g':
00080 drone.takeoff()
00081 time.sleep(5)
00082
00083 for i in range(100):
00084 if i < 90:
00085 drone.attitude_control(0x40, 0, 2, 0, 0)
00086 else:
00087 drone.attitude_control(0x40, 0, 0, 0, 0)
00088 time.sleep(0.02)
00089 time.sleep(1)
00090
00091 for i in range(200):
00092 if i < 180:
00093 drone.attitude_control(0x40, 2, 0, 0, 0)
00094 else:
00095 drone.attitude_control(0x40, 0, 0, 0, 0)
00096 time.sleep(0.02)
00097 time.sleep(1)
00098
00099
00100 for i in range(200):
00101 if i < 180:
00102 drone.attitude_control(0x40, -2, 0, 0, 0)
00103 else:
00104 drone.attitude_control(0x40, 0, 0, 0, 0)
00105 time.sleep(0.02)
00106 time.sleep(1)
00107
00108 for i in range(200):
00109 if i < 180:
00110 drone.attitude_control(0x40, 0, 2, 0, 0)
00111 else:
00112 drone.attitude_control(0x40, 0, 0, 0, 0)
00113 time.sleep(0.02)
00114 time.sleep(1)
00115
00116 for i in range(200):
00117 if i < 180:
00118 drone.attitude_control(0x40, 0, -2, 0, 0)
00119 else:
00120 drone.attitude_control(0x40, 0, 0, 0, 0)
00121 time.sleep(0.02)
00122 time.sleep(1)
00123
00124 for i in range(200):
00125 if i < 180:
00126 drone.attitude_control(0x40, 0, 0, 0.5, 0)
00127 else:
00128 drone.attitude_control(0x40, 0, 0, 0, 0)
00129 time.sleep(0.02)
00130 time.sleep(1)
00131
00132 for i in range(200):
00133 if i < 180:
00134 drone.attitude_control(0x40, 0, 0, -0.5, 0)
00135 else:
00136 drone.attitude_control(0x40, 0, 0, 0, 0)
00137 time.sleep(0.02)
00138 time.sleep(1)
00139
00140 for i in range(200):
00141 if i < 180:
00142 drone.attitude_control(0x40, 0, 0, 0, 90)
00143 else:
00144 drone.attitude_control(0x40, 0, 0, 0, 0)
00145 time.sleep(0.02)
00146 time.sleep(1)
00147
00148 for i in range(200):
00149 if i < 180:
00150 drone.attitude_control(0x40, 0, 0, 0, -90)
00151 else:
00152 drone.attitude_control(0x40, 0, 0, 0, 0)
00153 time.sleep(0.02)
00154 time.sleep(1)
00155
00156 drone.landing()
00157
00158 elif main_operate_code == 'h':
00159 R = 2
00160 V = 2
00161
00162 for i in range(300):
00163 vx = V * math.sin((V/R)*i/50.0)
00164 vy = V * math.cos((V/R)*i/50.0)
00165
00166 drone.attitude_control(DJIDrone.HORIZ_POS|DJIDrone.VERT_VEL|DJIDrone.YAW_ANG|DJIDrone.HORIZ_BODY|DJIDrone.STABLE_ON, vx, vy, 0, 0)
00167 time.sleep(0.02)
00168 elif main_operate_code == 'i':
00169
00170 for i in range(60):
00171 drone.attitude_control(DJIDrone.HORIZ_POS|DJIDrone.VERT_VEL|DJIDrone.YAW_ANG|DJIDrone.HORIZ_BODY|DJIDrone.STABLE_ON, 3, 3, 0, 0)
00172 time.sleep(0.02)
00173 for i in range(60):
00174 drone.attitude_control(DJIDrone.HORIZ_POS|DJIDrone.VERT_VEL|DJIDrone.YAW_ANG|DJIDrone.HORIZ_BODY|DJIDrone.STABLE_ON, -3, 3, 0, 0)
00175 time.sleep(0.02)
00176 for i in range(60):
00177 drone.attitude_control(DJIDrone.HORIZ_POS|DJIDrone.VERT_VEL|DJIDrone.YAW_ANG|DJIDrone.HORIZ_BODY|DJIDrone.STABLE_ON, -3, -3, 0, 0)
00178 time.sleep(0.02)
00179 for i in range(60):
00180 drone.attitude_control(DJIDrone.HORIZ_POS|DJIDrone.VERT_VEL|DJIDrone.YAW_ANG|DJIDrone.HORIZ_BODY|DJIDrone.STABLE_ON, 3, -3, 0, 0)
00181 time.sleep(0.02)
00182 elif main_operate_code == 'j':
00183
00184 drone.take_picture()
00185 elif main_operate_code == 'k':
00186
00187 drone.start_video()
00188 elif main_operate_code == 'l':
00189
00190 drone.stop_video()
00191 elif main_operate_code == 'm':
00192
00193 drone.local_position_navigation_send_request(-100, -100, 100)
00194 elif main_operate_code == 'n':
00195
00196 drone.global_position_navigation_send_request(22.535, 113.95, 100)
00197 elif main_operate_code == 'o':
00198
00199 newWaypointList = [
00200 dji_sdk.msg.Waypoint(latitude = 22.535, longitude = 113.95, altitude = 100, staytime = 5, heading = 0),
00201 dji_sdk.msg.Waypoint(latitude = 22.535, longitude = 113.96, altitude = 100, staytime = 0, heading = 90),
00202 dji_sdk.msg.Waypoint(latitude = 22.545, longitude = 113.96, altitude = 100, staytime = 4, heading = -90),
00203 dji_sdk.msg.Waypoint(latitude = 22.545, longitude = 113.96, altitude = 10, staytime = 2, heading = 180),
00204 dji_sdk.msg.Waypoint(latitude = 22.525, longitude = 113.93, altitude = 50, staytime = 0, heading = -180)]
00205 drone.waypoint_navigation_send_request(newWaypointList)
00206 elif main_operate_code == 'p':
00207 drone.arm_drone()
00208 elif main_operate_code == 'q':
00209 drone.disarm_drone()
00210 elif main_operate_code == 'r':
00211 drone.vrc_start();
00212 for i in range(100):
00213 drone.vrc_control(1024-660, 1024-660, 1024-660, 1024+660)
00214 time.sleep(0.02)
00215 for i in range(1000):
00216 drone.vrc_control(1024, 1024, 1024+660, 1024+660)
00217 time.sleep(0.02)
00218 for i in range(1000):
00219 drone.vrc_control(1024-660, 1024-660)
00220 time.sleep(0.02)
00221 drone.vrc_stop()
00222 elif main_operate_code == 's':
00223 drone.sync_timestamp(50)
00224 elif main_operate_code == 't':
00225 return
00226 else:
00227 display_main_menu()
00228
00229 if __name__ == "__main__":
00230 main()