apmsetrate.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 '''
00004 set stream rate on an APM
00005 '''
00006 
00007 import sys, struct, time, os
00008 
00009 from argparse import ArgumentParser
00010 parser = ArgumentParser(description=__doc__)
00011 
00012 parser.add_argument("--baudrate", type=int,
00013                   help="master port baud rate", default=115200)
00014 parser.add_argument("--device", required=True, help="serial device")
00015 parser.add_argument("--rate", default=4, type=int, help="requested stream rate")
00016 parser.add_argument("--source-system", dest='SOURCE_SYSTEM', type=int,
00017                   default=255, help='MAVLink source system for this GCS')
00018 parser.add_argument("--showmessages", action='store_true',
00019                   help="show incoming messages", default=False)
00020 args = parser.parse_args()
00021 
00022 from pymavlink import mavutil
00023 
00024 def wait_heartbeat(m):
00025     '''wait for a heartbeat so we know the target system IDs'''
00026     print("Waiting for APM heartbeat")
00027     m.wait_heartbeat()
00028     print("Heartbeat from APM (system %u component %u)" % (m.target_system, m.target_system))
00029 
00030 def show_messages(m):
00031     '''show incoming mavlink messages'''
00032     while True:
00033         msg = m.recv_match(blocking=True)
00034         if not msg:
00035             return
00036         if msg.get_type() == "BAD_DATA":
00037             if mavutil.all_printable(msg.data):
00038                 sys.stdout.write(msg.data)
00039                 sys.stdout.flush()
00040         else:
00041             print(msg)
00042 
00043 # create a mavlink serial instance
00044 master = mavutil.mavlink_connection(args.device, baud=args.baudrate)
00045 
00046 # wait for the heartbeat msg to find the system ID
00047 wait_heartbeat(master)
00048 
00049 print("Sending all stream request for rate %u" % args.rate)
00050 for i in range(0, 3):
00051     master.mav.request_data_stream_send(master.target_system, master.target_component,
00052                                         mavutil.mavlink.MAV_DATA_STREAM_ALL, args.rate, 1)
00053 if args.showmessages:
00054     show_messages(master)


mavlink
Author(s): Lorenz Meier
autogenerated on Sun May 22 2016 04:05:43