diff --git a/Firmware/tools/mavtester3.py b/Firmware/tools/mavtester3.py new file mode 100755 index 00000000..b54bf8ed --- /dev/null +++ b/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()