PyMavlink-WS/utils.py at main · JoC2000/PyMavlink-WS · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
from pymavlink import mavutil
import math
import time

def arm(connection):
    connection.mav.command_long_send(connection.target_system,
                                     connection.target_component,
                                     mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
                                     0,
                                     1,
                                     0,
                                     0,0,0,0,0)

    # Wait for ACK
    msg = connection.recv_match(type="COMMAND_ACK", blocking=True)
    print(msg)


def disarm(connection):
    connection.mav.command_long_send(connection.target_system,
                                     connection.target_component,
                                     mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
                                     0,
                                     0,
                                     0,
                                     0,0,0,0,0)

    # Wait for ACK
    msg = connection.recv_match(type="COMMAND_ACK", blocking=True)
    print(msg)


def takeoff(connection):
    connection.mav.command_long_send(connection.target_system,
                                     connection.target_component,
                                     mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
                                     0,
                                     0,0,0,0,0,0,10)

    # Wait for ACK
    msg = connection.recv_match(type="COMMAND_ACK", blocking=True)
    print(msg)

    # Wait for desired altitude
    is_takeoff = 1

    while is_takeoff:
        pos_msg = connection.recv_match(type="LOCAL_POSITION_NED", blocking=True)
        print("Altitude: ", -pos_msg.z)
        if pos_msg.z < -9.8:
            print("Reached altitude")
            is_takeoff = 0


def req_local_pos(connection):
    # TODO: MAV_CMD_SET_MESSAGE_INTERVAL
    connection.mav.command_long_send(connection.target_system,
                                     connection.target_component,
                                     mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL,
                                     0,
                                     mavutil.mavlink.MAVLINK_MSG_ID_LOCAL_POSITION_NED,
                                     100000,
                                     0,0,0,0,0)

    # Wait for ACK
    msg = connection.recv_match(type="COMMAND_ACK", blocking=True)
    print(msg)


def set_local_pos(connection, x, y, z):
    """
    Set target location in local frame NED.

    Parameters:
        x : distance in meters
        y : distance in meters
        z : distance in meters
    """

    connection.mav.send(mavutil.mavlink.MAVLink_set_position_target_local_ned_message(
                        10,
                        connection.target_system,
                        connection.target_component,
                        mavutil.mavlink.MAV_FRAME_LOCAL_NED,
                        int(0b110111111000),
                        x, y, z,
                        0,0,0,
                        0,0,0,
                        0,0
                        ))

    moving = 1
    tolerance = 0.2
    while moving:
        pos_msg = connection.recv_match(type="LOCAL_POSITION_NED", blocking=True)
        print("x: %f y: %f z: %f\n" % (pos_msg.x,pos_msg.y,pos_msg.z))

        ex = x - pos_msg.x
        ey = y - pos_msg.y
        ez = z - pos_msg.z

        distance = math.sqrt(ex*ex + ey*ey + ez*ez)
        print("Distance: ", distance)

        if distance < tolerance:
            print("Reached point")
            moving = 0


def land(connection):
    #TODO: MAV_CMD_NAV_LAND
    connection.mav.command_long_send(connection.target_system,
                                     connection.target_component,
                                     mavutil.mavlink.MAV_CMD_NAV_LAND,
                                     0,
                                     0,0,0,0,0,0,0)

    # Wait for ACK
    msg = connection.recv_match(type="COMMAND_ACK", blocking=True)
    print(msg)