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() |
why is there a master.wait_heartbeart()?
And MAV_DATA_STREAM_ALL is said to be deprecated as per new MAVLink definitions i went through.I don't have a clue what its consequence is.
And can you help me out?
Instead of MAV_DATA_STREAM_ALL, use MAV_DATA_STREAM_POSITION or one of the others, It will work.
How do I find the serial device option in this. Can you please add an example of usage?
Thanks.
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Sir, this is very well written and easy to understand. You have saved me many hours of work. Thank you!
I do have a question though. Why does this script only extract a small portion of the available messages. When I run it with a print statement to print everything 'print (master.messages)', these are all that I see.
{'LOCAL_POSITION_NED':
<pymavlink.dialects.v10.ardupilotmega.MAVLink_local_position_ned_message object at 0x758ac050>, 'TERRAIN_REPORT':
<pymavlink.dialects.v10.ardupilotmega.MAVLink_terrain_report_message object at 0x758ac990>, 'ATTITUDE':
<pymavlink.dialects.v10.ardupilotmega.MAVLink_attitude_message object at 0x758ac650>, 'HEARTBEAT':
<pymavlink.dialects.v10.ardupilotmega.MAVLink_heartbeat_message object at 0x758ac330>, 'HOME':
<pymavlink.dialects.v10.ardupilotmega.MAVLink_gps_raw_int_message object at 0x758acb50>, 'MISSION_CURRENT':
<pymavlink.dialects.v10.ardupilotmega.MAVLink_mission_current_message object at 0x758acdf0>, 'RAW_IMU':
<pymavlink.dialects.v10.ardupilotmega.MAVLink_raw_imu_message object at 0x758ac190>, 'GPS_RAW_INT':
<pymavlink.dialects.v10.ardupilotmega.MAVLink_gps_raw_int_message object at 0x758ac430>, 'NAV_CONTROLLER_OUTPUT':
<pymavlink.dialects.v10.ardupilotmega.MAVLink_nav_controller_output_message object at 0x758ac0f0>, 'VFR_HUD':
<pymavlink.dialects.v10.ardupilotmega.MAVLink_vfr_hud_message object at 0x758acfd0>, 'MEMINFO':
<pymavlink.dialects.v10.ardupilotmega.MAVLink_meminfo_message object at 0x758ac810>, 'HWSTATUS':
<pymavlink.dialects.v10.ardupilotmega.MAVLink_hwstatus_message object at 0x758ac530>, 'VIBRATION':
<pymavlink.dialects.v10.ardupilotmega.MAVLink_vibration_message object at 0x758ac4d0>, 'SCALED_IMU2':
<pymavlink.dialects.v10.ardupilotmega.MAVLink_scaled_imu2_message object at 0x758acf10>, 'BAD_DATA':
<pymavlink.dialects.v10.ardupilotmega.MAVLink_bad_data object at 0x759526b0>, 'SENSOR_OFFSETS':
<pymavlink.dialects.v10.ardupilotmega.MAVLink_sensor_offsets_message object at 0x758accf0>, 'SYS_STATUS':
<pymavlink.dialects.v10.ardupilotmega.MAVLink_sys_status_message object at 0x758acaf0>, 'SCALED_PRESSURE':
<pymavlink.dialects.v10.ardupilotmega.MAVLink_scaled_pressure_message object at 0x758acd30>, 'GLOBAL_POSITION_INT':
<pymavlink.dialects.v10.ardupilotmega.MAVLink_global_position_int_message object at 0x758ac8b0>, 'AHRS':
<pymavlink.dialects.v10.ardupilotmega.MAVLink_ahrs_message object at 0x758aca90>, 'POWER_STATUS':
<pymavlink.dialects.v10.ardupilotmega.MAVLink_power_status_message object at 0x758ac2b0>, 'SYSTEM_TIME':
<pymavlink.dialects.v10.ardupilotmega.MAVLink_system_time_message object at 0x758ac210>, 'EKF_STATUS_REPORT':
<pymavlink.dialects.v10.ardupilotmega.MAVLink_ekf_status_report_message object at 0x758aca10>, 'MAV':
<pymavlink.mavutil.mavserial object at 0x75952910>, 'AHRS3':
<pymavlink.dialects.v10.ardupilotmega.MAVLink_ahrs3_message object at 0x758acf50>, 'AHRS2':
<pymavlink.dialects.v10.ardupilotmega.MAVLink_ahrs2_message object at 0x758acc50>, 'SERVO_OUTPUT_RAW':
<pymavlink.dialects.v10.ardupilotmega.MAVLink_servo_output_raw_message object at 0x758ac8f0>, 'RC_CHANNELS_RAW':
<pymavlink.dialects.v10.ardupilotmega.MAVLink_rc_channels_raw_message object at 0x758ac3d0>, 'POSITION_TARGET_GLOBAL_INT':
<pymavlink.dialects.v10.ardupilotmega.MAVLink_position_target_global_int_message object at 0x758acbb0>, 'WIND':
<pymavlink.dialects.v10.ardupilotmega.MAVLink_wind_message object at 0x758acf90>}