7. test1.py(드론 조종) 관련

오늘 한 일

• test1.py 리팩토링: exploit.py 코드처럼 함수를 쉽게 가져다 쓸 수 있게 고쳤다.
• 드론 모드 변경 관련 코드 작성: 대다수의 드론은 STABLIZE 모드에서만 arm이 걸린다. 하지만 우리가 필요한 것은 MANUAL 모드이기 때문에 전환하는 작업이 필요하다
• dronemode 함수가 그 역할을 한다.

def dronemode(master, i):
    mode_id = master.mode_mapping()[mode[i]]
    master.mav.set_mode_send(master.target_system, mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, mode_id)
    

from pymavlink import mavutil
import keyboard
import time

val = [
    # x, y, z, r
    [0, 0, 0, 0], #land 0
    [250, 0, 500, 0], #front 1
    [-250, 0, 500, 0], #back 2
    [0, 250, 500, 0], #right 3
    [0, -250, 500, 0], #left 4
    [0, 0, 1000, 0], # up 5
    [0, 0, 250, 0], # down 6
    [0, 0, 500, 0] # hold 7
];

warn = ["'d' is armed/disarmed, 's' is a land mode", "If you arm or disarm your drone, which changes modes automatically",
"you can use arrow key to move your drone horizontal.", "you can use ctrl, and space bar to move your drone vertical.",
"'z' is a hovering button"]

mode = ['STABLIZE', 'MANUAL', 'LAND']


def droneconnect():
    master = mavutil.mavlink_connection('udpin:0.0.0.0:14550')
    master.wait_heartbeat()
    print('Drone is connected!')
    return master

def dronemode(master, i):
    mode_id = master.mode_mapping()[mode[i]]
    master.mav.set_mode_send(master.target_system, mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, mode_id)
    
def dronearm(master, armed):
    master.mav.command_long_send(master.target_system, master.target_component,mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 0, not armed, 0, 0, 0, 0, 0, 0)
    if(not armed):
        print("Waiting for the vehicle to arm")
        master.motors_armed_wait()
        print('Armed!')
        dronemode(master, 1)
    else:
        dronemode(master, 0)
        print('Waiting for the vehicle to disarm')
        master.motors_disarmed_wait()
        print('Disarmed!')
    return not armed

def dronecontrol(master, i):
    master.mav.manual_control_send(master.target_system, val[i][0], val[i][1], val[i][2], val[i][3], 0)
    time.sleep(0.085)

def inputkeyboard(master):
    armed = False;
    while 1:
        i = 7;
        if keyboard.is_pressed('a'):#arm/disarm
            armed = dronearm(master, armed)
        if keyboard_is_pressed('s'):#land mode
            dronemde(master, 2)
        if keyboard.is_pressed('left'):#left
            i = 4
        if keyboard.is_pressed('right'):#right
            i = 3
        if keyboard.is_pressed('up'):#front
            i = 1
        if keyboard.is_pressed('down'):#back
            i = 2
        if keyboard.is_pressed('space'):#up
            i = 5
        if keyboard.is_pressed('ctrl'):#down
            i = 6
        if keyboard.is_pressed('z'):#hovering
            i = 7
        if(armed):
            dronecontrol(master, i)
 
if __name__ == '__main__':
    for i in range(4):
        print(warn[i])
   
    master = droneconnect()
    inputkeyboard(master)