pymavlink_tutorial/mavgen.py at master · aabs7/pymavlink_tutorial · 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
121
122
123
124
125
126
#import ardupilotmega module for mavlink1
from pymavlink.dialects.v10 import ardupilotmega as mavlink1

#import common module for mavlink 2
from pymavlink.dialects.v20 import common as mavlink2

from pymavlink import mavutil
import time
import sys

# Start a connection listening to a UDP port
the_connection = mavutil.mavlink_connection('tcp:127.0.0.1:5762')

# Wait for the first heartbeat
#   This sets the system and component ID of remote system for the link
the_connection.wait_heartbeat()
print("Heartbeat from system (system %u component %u)" % (the_connection.target_system, the_connection.target_system))

msg = the_connection.recv_match(blocking=True)

while True:
    msg = the_connection.recv_match(blocking=True)

    print(the_connection.messages['GPS_RAW_INT'].alt )
    time.sleep(1)


the_connection.mav.heartbeat_send(
        6, # type
        8, # autopilot
        192, # base_mode
        0, # custom_mode
        4, # system_status
        3  # mavlink_version
)

the_connection.mav.command_long_send(
        1, # autopilot system id
        1, # autopilot component id
        400, # command id, ARM/DISARM
        0, # confirmation
        1, # arm!
        0,0,0,0,0,0 # unused parameters for this command
)

mode = 'STABILIZE'

# Check if mode is available
# if mode not in the_connection.mode_mapping():
#     print('Unknown mode : {}'.format(mode))
#     print('Try:', list(the_connection.mode_mapping().keys()))
#     sys.exit(1)


# Get mode ID
mode_id = 4
#mode_id = the_connection.mode_mapping()[mode]


# Set new mode
# the_connection.mav.command_long_send(
#    the_connection.target_system, the_connection.target_component,
#    mavutil.mavlink.MAV_CMD_DO_SET_MODE, 0,
#    0, mode_id, 0, 0, 0, 0, 0) or:
# the_connection.set_mode(mode_id) or:

# the_connection.mav.set_mode_send(
#     the_connection.target_system,
#     mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
#     mode_id)

mavutil.mavfile.set_mode(the_connection,4,0,0)

# while True:
#     # Wait for ACK command
#     ack_msg = the_connection.recv_match(type='COMMAND_ACK', blocking=True)
#     ack_msg = ack_msg.to_dict()

#     # Check if command in the same in `set_mode`
#     if ack_msg['command'] != mavutil.mavlink.MAVLINK_MSG_ID_SET_MODE:
#         continue

#     # Print the ACK result !
#     print(mavutil.mavlink.enums['MAV_RESULT'][ack_msg['result']].description)
#     break


altitude = 10

the_connection.mav.command_long_send(the_connection.target_system,
                                    the_connection.target_component,
                                    mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
                                    0, 0, 0, 0, 0, 0, 0, altitude)

print("Before disarm")
time.sleep(20)
print("After disarm")

the_connection.mav.command_long_send(
                1, # autopilot system id
                1, # autopilot component id
                400, # command id, ARM/DISARM
                0, # confirmation
                0, # disarm!
                0,0,0,0,0,0 # unused parameters for this command
                )


####################################################
# the_connection.mav.command_long_send(
#     the_connection.target_system,
#     the_connection.target_component,
#     mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
#     0,
#     1, 0, 0, 0, 0, 0, 0)

# altitude = 30

# the_connection.mav.command_long_send(the_connection.target_system,
#                                     the_connection.target_component,
#                                     mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
#                                     0, 0, 0, 0, 0, 0, 0, altitude)

input()