Permalink
Browse files

a 3-way version of mavtester

for testing radio relaying
1 parent c738ae5 commit 48faed2bf97f1b5fb379efa2b4d3bd63ee81050e @tridge tridge committed Aug 4, 2016
Showing with 375 additions and 0 deletions.
  1. +375 −0 Firmware/tools/mavtester3.py
@@ -0,0 +1,375 @@
+#!/usr/bin/env python
+
+'''
+test MAVLink performance between three radios
+'''
+
+import sys, time, os, threading, Queue
+
+from optparse import OptionParser
+parser = OptionParser("mavtester.py [options]")
+
+parser.add_option("--baudrate", type='int',
+ help="connection baud rate", default=57600)
+parser.add_option("--port1", default=None, help="serial port 1 (GCS)")
+parser.add_option("--port2", default=None, help="serial port 2 (relay)")
+parser.add_option("--port3", default=None, help="serial port 3 (retrieval)")
+parser.add_option("--rate", default=4, type='float', help="initial stream rate")
+parser.add_option("--override-rate", default=1, type='float', help="RC_OVERRIDE rate")
+parser.add_option("--show", action='store_true', default=False, help="show messages")
+parser.add_option("--rtscts", action='store_true', default=False, help="enable RTSCTS hardware flow control")
+parser.add_option("--mav20", action='store_true', default=False, help="enable MAVLink2")
+parser.add_option("--key", default=None, help="MAVLink2 signing key")
+
+(opts, args) = parser.parse_args()
+
+if opts.mav20:
+ os.environ['MAVLINK20'] = '1'
+
+from pymavlink import mavutil
+
+if opts.port1 is None or opts.port2 is None:
+ print("You must specify two serial ports")
+ sys.exit(1)
+
+# create GCS connection
+gcs = mavutil.mavlink_connection(opts.port1, baud=opts.baudrate, input=True, source_system=255)
+gcs.setup_logfile('gcs.tlog')
+
+relay = mavutil.mavlink_connection(opts.port2, baud=opts.baudrate, input=False, source_system=1)
+relay.setup_logfile('relay.tlog')
+
+retrieval = mavutil.mavlink_connection(opts.port3, baud=opts.baudrate, input=False, source_system=2)
+retrieval.setup_logfile('retrieval.tlog')
+
+print("Draining ports")
+gcs.port.timeout = 1
+relay.port.timeout = 1
+retrieval.port.timeout = 1
+
+while True:
+ r = gcs.port.read(1024)
+ if not r:
+ break
+ print("Drained %u bytes from gcs" % len(r))
+ time.sleep(0.01)
+while True:
+ r = relay.port.read(1024)
+ if not r:
+ break
+ print("Drained %u bytes from relay" % len(r))
+ time.sleep(0.01)
+while True:
+ r = retrieval.port.read(1024)
+ if not r:
+ break
+ print("Drained %u bytes from retrieval" % len(r))
+ time.sleep(0.01)
+
+if opts.rtscts:
+ print("Enabling RTSCTS")
+ gcs.set_rtscts(True)
+ relay.set_rtscts(True)
+ retrieval.set_rtscts(True)
+else:
+ gcs.set_rtscts(False)
+ relay.set_rtscts(False)
+ retrieval.set_rtscts(False)
+
+def allow_unsigned(mav, msgId):
+ '''see if an unsigned packet should be allowed'''
+ allow = {
+ mavutil.mavlink.MAVLINK_MSG_ID_RADIO : True,
+ mavutil.mavlink.MAVLINK_MSG_ID_RADIO_STATUS : True
+ }
+ if msgId in allow:
+ return True
+ return False
+
+if opts.mav20 and opts.key is not None:
+ import hashlib
+ h = hashlib.new('sha256')
+ h.update(opts.key)
+ key = h.digest()
+ gcs.setup_signing(key, sign_outgoing=True, allow_unsigned_callback=allow_unsigned)
+ relay.setup_signing(key, sign_outgoing=True, allow_unsigned_callback=allow_unsigned)
+ retrieval.setup_signing(key, sign_outgoing=True, allow_unsigned_callback=allow_unsigned)
+
+# we use thread based receive to avoid problems with serial buffer overflow in the Linux kernel.
+def receive_thread(mav, q):
+ '''continuously receive packets are put them in the queue'''
+ last_pkt = time.time()
+ while True:
+ m = mav.recv_match(blocking=False)
+ if m is not None:
+ q.put(m)
+ last_pkt = time.time()
+
+# start receive threads for the
+print("Starting threads")
+gcs_queue = Queue.Queue()
+gcs_thread = threading.Thread(target=receive_thread, args=(gcs, gcs_queue))
+gcs_thread.daemon = True
+gcs_thread.start()
+
+relay.queue = Queue.Queue()
+relay.thread = threading.Thread(target=receive_thread, args=(relay, relay.queue))
+relay.thread.daemon = True
+relay.thread.start()
+
+retrieval.queue = Queue.Queue()
+retrieval.thread = threading.Thread(target=receive_thread, args=(retrieval, retrieval.queue))
+retrieval.thread.daemon = True
+retrieval.thread.start()
+
+start_time = time.time()
+last_relay_send = time.time()
+last_retrieval_send = time.time()
+last_gcs_send = time.time()
+last_override_send = time.time()
+
+relay.lat = 0
+retrieval.lat = 0
+gcs_lat = [0]*3
+
+relay.last_send = 0
+retrieval.last_send = 0
+
+relay.received = 0
+retrieval.received = 0
+
+relay.radio_received = 0
+retrieval.radio_received = 0
+
+def send_telemetry(vehicle):
+ '''
+ send telemetry packets from the vehicle to
+ the GCS. This emulates the typical pattern of telemetry in
+ ArduPlane 2.75 in AUTO mode
+ '''
+ now = time.time()
+ # send at rate specified by user. This doesn't do rate adjustment yet (APM does adjustment
+ # based on RADIO packets)
+ if now - vehicle.last_send < 1.0/opts.rate:
+ return
+ vehicle.last_send = now
+ time_usec = int((now - start_time) * 1.0e6)
+ time_ms = time_usec // 1000
+
+ vehicle.mav.heartbeat_send(1, 3, 217, 10, 4, 3)
+ vehicle.mav.global_position_int_send(time_ms, vehicle.lat, 1491642131, 737900, 140830, 2008, -433, 224, 35616)
+ vehicle.mav.rc_channels_scaled_send(time_boot_ms=time_ms, port=0, chan1_scaled=280, chan2_scaled=3278, chan3_scaled=-3023, chan4_scaled=0, chan5_scaled=0, chan6_scaled=0, chan7_scaled=0, chan8_scaled=0, rssi=0)
+ if opts.mav20:
+ vehicle.mav.servo_output_raw_send(time_usec=time_usec, port=0, servo1_raw=1470, servo2_raw=1628, servo3_raw=1479, servo4_raw=1506, servo5_raw=1500, servo6_raw=1556, servo7_raw=1500, servo8_raw=1500,
+ servo9_raw=1500, servo10_raw=1500, servo11_raw=1500, servo12_raw=1500, servo13_raw=1500, servo14_raw=1500, servo15_raw=1500, servo16_raw=1500)
+ else:
+ vehicle.mav.servo_output_raw_send(time_usec=time_usec, port=0, servo1_raw=1470, servo2_raw=1628, servo3_raw=1479, servo4_raw=1506, servo5_raw=1500, servo6_raw=1556, servo7_raw=1500, servo8_raw=1500)
+ vehicle.mav.rc_channels_raw_send(time_boot_ms=time_ms, port=0, chan1_raw=1470, chan2_raw=1618, chan3_raw=1440, chan4_raw=1509, chan5_raw=1168, chan6_raw=1556, chan7_raw=1224, chan8_raw=994, rssi=0)
+ vehicle.mav.raw_imu_send(time_usec, 562, 382, -3917, -3330, 3445, 35, -24, 226, -523)
+ vehicle.mav.scaled_pressure_send(time_boot_ms=time_ms, press_abs=950.770019531, press_diff=-0.0989062488079, temperature=463)
+ vehicle.mav.sensor_offsets_send(mag_ofs_x=-68, mag_ofs_y=-143, mag_ofs_z=-34, mag_declination=0.206146687269, raw_press=95077, raw_temp=463, gyro_cal_x=-0.063114002347, gyro_cal_y=0.0479440018535, gyro_cal_z=0.0190890002996, accel_cal_x=0.418922990561, accel_cal_y=0.284875005484, accel_cal_z=-0.436598002911)
+ vehicle.mav.sys_status_send(onboard_control_sensors_present=64559, onboard_control_sensors_enabled=64559, onboard_control_sensors_health=64559, load=82, voltage_battery=11877, current_battery=0, battery_remaining=100, drop_rate_comm=0, errors_comm=0, errors_count1=0, errors_count2=0, errors_count3=0, errors_count4=0)
+ vehicle.mav.mission_current_send(seq=1)
+ vehicle.mav.gps_raw_int_send(time_usec=time_usec, fix_type=3, lat=-353637616, lon=1491642012, alt=737900, eph=169, epv=65535, vel=2055, cog=34782, satellites_visible=9)
+ vehicle.mav.nav_controller_output_send(nav_roll=0.0, nav_pitch=0.319999992847, nav_bearing=-18, target_bearing=343, wp_dist=383, alt_error=-37.0900001526, aspd_error=404.800537109, xtrack_error=1.52732038498)
+ vehicle.mav.attitude_send(time_boot_ms=time_ms, roll=0.00283912196755, pitch=-0.0538846850395, yaw=-0.0708072632551, rollspeed=0.226980209351, pitchspeed=-0.00743395090103, yawspeed=-0.154820173979)
+ vehicle.mav.vfr_hud_send(airspeed=21.9519939423, groundspeed=20.5499992371, heading=355, throttle=35, alt=737.900024414, climb=-0.784280121326)
+ vehicle.mav.ahrs_send(omegaIx=0.000540865410585, omegaIy=-0.00631708558649, omegaIz=0.00380697473884, accel_weight=0.0, renorm_val=0.0, error_rp=0.094664350152, error_yaw=0.0121578350663)
+ vehicle.mav.hwstatus_send(Vcc=0, I2Cerr=0)
+ vehicle.mav.wind_send(direction=27.729429245, speed=5.35723495483, speed_z=-1.92264056206)
+
+ vehicle.lat += 1
+
+def send_GCS():
+ '''
+ send GCS heartbeat messages
+ '''
+ global last_gcs_send
+ now = time.time()
+ if now - last_gcs_send < 1.0:
+ return
+ gcs.mav.heartbeat_send(1, 6, 0, 0, 0, 0)
+ last_gcs_send = now
+
+def send_override():
+ '''
+ send RC_CHANNELS_OVERRIDE messages from GCS
+ '''
+ global last_override_send
+ now = time.time()
+ if opts.override_rate == 0:
+ return
+ if now - last_override_send < 1.0/opts.override_rate:
+ return
+ time_ms = int((now - start_time) * 1.0e3)
+ time_ms_low = time_ms % 65536
+ time_ms_high = (time_ms - time_ms_low) // 65536
+ gcs.mav.rc_channels_override_send(1, 2, time_ms_low, time_ms_high, 0, 0, 0, 0, 0, 0)
+ last_override_send = now
+
+def process_override(m):
+ '''
+ process an incoming RC_CHANNELS_OVERRIDE message, measuring latency
+ '''
+ now = time.time()
+ time_ms_sent = m.chan2_raw*65536 + m.chan1_raw
+ time_ms = int((now - start_time) * 1.0e3)
+ latency = time_ms - time_ms_sent
+
+ stats.latency_count += 1
+ stats.latency_total += latency
+ if stats.latency_min == 0 or latency < stats.latency_min:
+ stats.latency_min = latency
+ if latency > stats.latency_max:
+ stats.latency_max = latency
+
+def recv_vehicle(vehicle):
+ '''
+ receive packets in the vehicle
+ '''
+ try:
+ m = vehicle.queue.get(block=False)
+ except Queue.Empty:
+ return False
+ if m.get_type() == 'BAD_DATA':
+ stats.vehicle_bad_data += 1
+ return True
+ if opts.show:
+ print(m)
+ vehicle.received += 1
+ if m.get_type() in ['RADIO','RADIO_STATUS']:
+ #print('VRADIO: ', str(m))
+ vehicle.radio_received += 1
+ stats.vehicle_txbuf = m.txbuf
+ stats.vehicle_fixed = m.fixed
+ if m.get_type() == 'RC_CHANNELS_OVERRIDE':
+ process_override(m)
+ return True
+
+def recv_GCS():
+ '''
+ receive packets in the GCS
+ '''
+ try:
+ m = gcs_queue.get(block=False)
+ except Queue.Empty:
+ return False
+ if m.get_type() == 'BAD_DATA':
+ stats.gcs_bad_data += 1
+ return True
+ if m.get_type() == 'GLOBAL_POSITION_INT':
+ global gcs_lat
+ src_system = m.get_srcSystem()
+ if gcs_lat[src_system] != m.lat:
+ print("Lost %u GLOBAL_POSITION_INT messages" % (m.lat - gcs_lat[src_system]))
+ gcs_lat[src_system] = m.lat
+ gcs_lat[src_system] += 1
+ if opts.show:
+ print(m)
+ stats.gcs_received += 1
+ if m.get_type() in ['RADIO','RADIO_STATUS']:
+ #print('GRADIO: ', str(m))
+ stats.gcs_radio_received += 1
+ stats.gcs_txbuf = m.txbuf
+ stats.gcs_fixed = m.fixed
+ return True
+
+
+
+class PacketStats(object):
+ '''
+ class to hold statistics on the link
+ '''
+ def __init__(self):
+ self.gcs_sent = 0
+ self.relay_sent = 0
+ self.retrieval_sent = 0
+ self.gcs_received = 0
+ relay.received = 0
+ retrieval.received = 0
+ self.gcs_radio_received = 0
+ relay.radio_received = 0
+ retrieval.radio_received = 0
+ self.gcs_last_bytes_sent = 0
+ self.vehicle_last_bytes_sent = 0
+ self.latency_count = 0
+ self.latency_total = 0
+ self.latency_min = 0
+ self.latency_max = 0
+ self.vehicle_bad_data = 0
+ self.gcs_bad_data = 0
+ self.last_gcs_radio = None
+ self.last_vehicle_radio = None
+ self.vehicle_txbuf = 100
+ self.gcs_txbuf = 100
+ self.vehicle_fixed = 0
+ self.gcs_fixed = 0
+
+ def __str__(self):
+ gcs_bytes_sent = gcs.mav.total_bytes_sent - self.gcs_last_bytes_sent
+ total_sent = relay.mav.total_bytes_sent + retrieval.mav.total_bytes_sent
+ vehicle_bytes_sent = total_sent - self.vehicle_last_bytes_sent
+ self.gcs_last_bytes_sent = gcs.mav.total_bytes_sent
+ self.vehicle_last_bytes_sent = total_sent
+
+ avg_latency = 0
+ if stats.latency_count != 0:
+ avg_latency = stats.latency_total / stats.latency_count
+
+ return "Rel:%u/%u/%u Ret:%u/%u/%u GCS:%u/%u/%u pend:%u rates:%u/%u lat:%u/%u/%u bad:%u/%u txbuf:%u/%u loss:%u:%u%%/%u:%u%%/%u:%u%% fixed:%u/%u" % (
+ self.relay_sent,
+ relay.received,
+ relay.received - relay.radio_received,
+ self.retrieval_sent,
+ retrieval.received,
+ retrieval.received - retrieval.radio_received,
+ self.gcs_sent,
+ self.gcs_received,
+ self.gcs_received - self.gcs_radio_received,
+ (self.relay_sent + self.retrieval_sent) - (self.gcs_received - self.gcs_radio_received),
+ gcs_bytes_sent,
+ vehicle_bytes_sent,
+ stats.latency_min,
+ stats.latency_max,
+ avg_latency,
+ self.vehicle_bad_data,
+ self.gcs_bad_data,
+ self.vehicle_txbuf,
+ self.gcs_txbuf,
+ gcs.mav_loss,
+ gcs.packet_loss(),
+ relay.mav_loss,
+ relay.packet_loss(),
+ retrieval.mav_loss,
+ retrieval.packet_loss(),
+ stats.vehicle_fixed,
+ stats.gcs_fixed)
+
+
+'''
+main code
+'''
+last_report = time.time()
+stats = PacketStats()
+
+while True:
+
+ send_telemetry(relay)
+ send_telemetry(retrieval)
+ stats.relay_sent = relay.mav.total_packets_sent
+ stats.retrieval_sent = retrieval.mav.total_packets_sent
+
+ send_GCS()
+ send_override()
+ stats.gcs_sent = gcs.mav.total_packets_sent
+
+ while True:
+ recv1 = recv_vehicle(relay)
+ recv1 = recv_vehicle(retrieval)
+ recv2 = recv_GCS()
+ if not recv1 and not recv2:
+ break
+
+ if time.time() - last_report >= 1.0:
+ print(stats)
+ last_report = time.time()

0 comments on commit 48faed2

Please sign in to comment.