Last active
February 13, 2023 13:22
-
-
Save vo/9331349 to your computer and use it in GitHub Desktop.
Simple Mavlink Reader in Python using pymavlink
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#!/usr/bin/env python | |
import sys, os | |
from optparse import OptionParser | |
# tell python where to find mavlink so we can import it | |
sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)), '../mavlink')) | |
from pymavlink import mavutil | |
def handle_heartbeat(msg): | |
mode = mavutil.mode_string_v10(msg) | |
is_armed = msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED | |
is_enabled = msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_GUIDED_ENABLED | |
def handle_rc_raw(msg): | |
channels = (msg.chan1_raw, msg.chan2_raw, msg.chan3_raw, msg.chan4_raw, | |
msg.chan5_raw, msg.chan6_raw, msg.chan7_raw, msg.chan8_raw) | |
def handle_hud(msg): | |
hud_data = (msg.airspeed, msg.groundspeed, msg.heading, | |
msg.throttle, msg.alt, msg.climb) | |
print "Aspd\tGspd\tHead\tThro\tAlt\tClimb" | |
print "%0.2f\t%0.2f\t%0.2f\t%0.2f\t%0.2f\t%0.2f" % hud_data | |
def handle_attitude(msg): | |
attitude_data = (msg.roll, msg.pitch, msg.yaw, msg.rollspeed, | |
msg.pitchspeed, msg.yawspeed) | |
print "Roll\tPit\tYaw\tRSpd\tPSpd\tYSpd" | |
print "%0.2f\t%0.2f\t%0.2f\t%0.2f\t%0.2f\t%0.2f\t" % attitude_data | |
def read_loop(m): | |
while(True): | |
# grab a mavlink message | |
msg = m.recv_match(blocking=False) | |
if not msg: | |
return | |
# handle the message based on its type | |
msg_type = msg.get_type() | |
if msg_type == "BAD_DATA": | |
if mavutil.all_printable(msg.data): | |
sys.stdout.write(msg.data) | |
sys.stdout.flush() | |
elif msg_type == "RC_CHANNELS_RAW": | |
handle_rc_raw(msg) | |
elif msg_type == "HEARTBEAT": | |
handle_heartbeat(msg) | |
elif msg_type == "VFR_HUD": | |
handle_hud(msg) | |
elif msg_type == "ATTITUDE": | |
handle_attitude(msg) | |
def main(): | |
# read command line options | |
parser = OptionParser("readdata.py [options]") | |
parser.add_option("--baudrate", dest="baudrate", type='int', | |
help="master port baud rate", default=115200) | |
parser.add_option("--device", dest="device", default=None, help="serial device") | |
parser.add_option("--rate", dest="rate", default=4, type='int', help="requested stream rate") | |
parser.add_option("--source-system", dest='SOURCE_SYSTEM', type='int', | |
default=255, help='MAVLink source system for this GCS') | |
parser.add_option("--showmessages", dest="showmessages", action='store_true', | |
help="show incoming messages", default=False) | |
(opts, args) = parser.parse_args() | |
if opts.device is None: | |
print("You must specify a serial device") | |
sys.exit(1) | |
# create a mavlink serial instance | |
master = mavutil.mavlink_connection(opts.device, baud=opts.baudrate) | |
# wait for the heartbeat msg to find the system ID | |
master.wait_heartbeat() | |
# request data to be sent at the given rate | |
master.mav.request_data_stream_send(master.target_system, master.target_component, | |
mavutil.mavlink.MAV_DATA_STREAM_ALL, opts.rate, 1) | |
# enter the data loop | |
read_loop(master) | |
if __name__ == '__main__': | |
main() |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
How do I find the serial device option in this. Can you please add an example of usage?
Thanks.