diff --git a/Firmware/radio/at.c b/Firmware/radio/at.c index 850b7548..564eeab6 100644 --- a/Firmware/radio/at.c +++ b/Firmware/radio/at.c @@ -205,7 +205,7 @@ at_timer(void) at_cmd_ready = true; break; default: - // should never happen, but otherwise harmless + ; // should never happen, but otherwise harmless } } } @@ -339,7 +339,7 @@ at_i(void) printf("%u\n", g_board_bl_version); return; case '5': - print_ID_vals(' ', PARAM_MAX, param_name, param_get); + print_ID_vals('S', PARAM_MAX, param_name, param_get); return; case '6': tdm_report_timing(); diff --git a/Firmware/radio/main.c b/Firmware/radio/main.c index 555e0c0a..6e4d6a30 100644 --- a/Firmware/radio/main.c +++ b/Firmware/radio/main.c @@ -97,7 +97,6 @@ __pdata struct statistics statistics, remote_statistics; /// optional features bool feature_golay; -bool feature_opportunistic_resend; uint8_t feature_mavlink_framing; bool feature_rtscts; @@ -121,7 +120,6 @@ main(void) // setup boolean features feature_mavlink_framing = param_get(PARAM_MAVLINK); - feature_opportunistic_resend = param_get(PARAM_OPPRESEND)?true:false; feature_golay = param_get(PARAM_ECC)?true:false; feature_rtscts = param_get(PARAM_RTSCTS)?true:false; diff --git a/Firmware/radio/mavlink.c b/Firmware/radio/mavlink.c index 6669de89..a8645b0b 100644 --- a/Firmware/radio/mavlink.c +++ b/Firmware/radio/mavlink.c @@ -40,9 +40,6 @@ extern __xdata uint8_t pbuf[MAX_PACKET_LENGTH]; static __pdata uint8_t seqnum; -#define MAVLINK_MSG_ID_RADIO 166 -#define MAVLINK_RADIO_CRC_EXTRA 21 - // new RADIO_STATUS common message #define MAVLINK_MSG_ID_RADIO_STATUS 109 #define MAVLINK_RADIO_STATUS_CRC_EXTRA 185 @@ -112,17 +109,8 @@ struct mavlink_RADIO_v10 { uint8_t remnoise; }; -static void swap_bytes(__pdata uint8_t ofs, __pdata uint8_t len) -{ - register uint8_t i; - for (i=ofs; irxerrors = errors.rx_errors; - m->fixed = errors.corrected_packets; - m->txbuf = serial_read_space(); - m->rssi = statistics.average_rssi; - m->remrssi = remote_statistics.average_rssi; - m->noise = statistics.average_noise; - m->remnoise = remote_statistics.average_noise; - mavlink_crc(MAVLINK_RADIO_CRC_EXTRA); - - if (serial_write_space() < sizeof(struct mavlink_RADIO_v10)+8) { - // don't cause an overflow - return; - } - - serial_write_buf(pbuf, sizeof(struct mavlink_RADIO_v10)+8); - - // now the new RADIO_STATUS common message pbuf[5] = MAVLINK_MSG_ID_RADIO_STATUS; + + m->rxerrors = errors.rx_errors; + m->fixed = errors.corrected_packets; + m->txbuf = serial_read_space(); + m->rssi = statistics.average_rssi; + m->remrssi = remote_statistics.average_rssi; + m->noise = statistics.average_noise; + m->remnoise = remote_statistics.average_noise; mavlink_crc(MAVLINK_RADIO_STATUS_CRC_EXTRA); if (serial_write_space() < sizeof(struct mavlink_RADIO_v10)+8) { diff --git a/Firmware/radio/packet.c b/Firmware/radio/packet.c index ebade8cd..5f78dbcc 100644 --- a/Firmware/radio/packet.c +++ b/Firmware/radio/packet.c @@ -80,9 +80,9 @@ bool seen_mavlink; // monitoring of link quality static void check_heartbeat(__xdata uint8_t * __pdata buf) { - if (buf[0] == MAVLINK10_STX && - buf[1] == 9 && buf[5] == 0) { - // looks like a MAVLink 1.0 heartbeat + if ((buf[1] == 9 && buf[0] == MAVLINK10_STX && buf[5] == 0) || + (buf[1] <= 9 && buf[0] == MAVLINK20_STX && buf[7] == 0 && buf[8] == 0 && buf[9] == 0)) { + // looks like a MAVLink heartbeat seen_mavlink = true; } } @@ -90,46 +90,6 @@ static void check_heartbeat(__xdata uint8_t * __pdata buf) #define MSG_TYP_RC_OVERRIDE 70 #define MSG_LEN_RC_OVERRIDE (9 * 2) -/// Return the offset of any high priority packet (so we can ensure that this packet goes out in the next -/// tx window -static -int16_t extract_hipri(uint8_t max_xmit) -{ - __xdata uint16_t slen = serial_read_available(); - __xdata uint16_t offset = 0; - __xdata int16_t high_offset = -1; - - // Walk the serial buffer to find the _last_ high pri packet - while (slen >= 8) { - register uint8_t c = serial_peekx(offset); - if (c != MAVLINK10_STX) { - // we've lost mavlink framing - stop scanning for this window - break; - } - c = serial_peekx(offset + 1); - if (c >= 255 - 8 || - c+8 > max_xmit - last_sent_len) { - // it won't fit - break; - } - if (c+8 > slen) { - // we don't have the full MAVLink packet in - // the serial buffer - break; - } - - if(serial_peekx(offset + 5) == MSG_TYP_RC_OVERRIDE && c == MSG_LEN_RC_OVERRIDE) { - // if(high_offset != -1) printf("found 2nd\r\n"); else printf("found rc\r\n"); - high_offset = offset; - } - - c += 8; - slen -= c; - offset += c; - } - - return high_offset; -} #define MAVLINK_FRAMING_DISABLED 0 #define MAVLINK_FRAMING_SIMPLE 1 @@ -140,65 +100,52 @@ int16_t extract_hipri(uint8_t max_xmit) static uint8_t mavlink_frame(uint8_t max_xmit, __xdata uint8_t * __pdata buf) { - __data uint16_t slen, offset = 0, high_offset; + __data uint16_t slen; - // - // There is already a packet sitting waiting here - // - // but this optimization is redundant with the loop below. By letting the very slightly - // more expensive version its thing we can ensure we skip _all_ redundant rc_override msgs -#if 0 - serial_read_buf(last_sent, mav_pkt_len); - last_sent_len = mav_pkt_len; - memcpy(buf, last_sent, last_sent_len); - check_heartbeat(buf); -#else last_sent_len = 0; -#endif mav_pkt_len = 0; - high_offset = (feature_mavlink_framing == MAVLINK_FRAMING_HIGHPRI) ? extract_hipri(max_xmit) : -1; - slen = serial_read_available(); // see if we have more complete MAVLink frames in the serial // buffer that we can fit in this packet while (slen >= 8) { - register uint8_t c = serial_peek(); - if (c != MAVLINK10_STX) { + register uint8_t c = serial_peekx(0); + register uint8_t extra_len = 8; + if (c != MAVLINK10_STX && c != MAVLINK20_STX) { // its not a MAVLink packet return last_sent_len; } - c = serial_peek2(); - if (c >= 255 - 8 || - c+8 > max_xmit - last_sent_len) { + if (c == MAVLINK20_STX) { + extra_len += 4; + if (serial_peekx(2) & 1) { + // signed packet + extra_len += 13; + } + } + // fetch the length byte + c = serial_peekx(1); + if (c >= 255 - extra_len || + c+extra_len > max_xmit - last_sent_len) { // it won't fit break; } - if (c+8 > slen) { + if (c+extra_len > slen) { // we don't have the full MAVLink packet in // the serial buffer break; } - // If we are using the special highpri mode, we might skip some override packets - if(high_offset != -1 && high_offset != offset && serial_peekx(5) == MSG_TYP_RC_OVERRIDE && c == MSG_LEN_RC_OVERRIDE) { - // printf("skipping rc\r\n"); - c += 8; - } - else { - c += 8; - - // we can add another MAVLink frame to the packet - serial_read_buf(&last_sent[last_sent_len], c); - memcpy(&buf[last_sent_len], &last_sent[last_sent_len], c); - - check_heartbeat(buf+last_sent_len); - } + c += extra_len; + // we can add another MAVLink frame to the packet + serial_read_buf(&last_sent[last_sent_len], c); + memcpy(&buf[last_sent_len], &last_sent[last_sent_len], c); + + check_heartbeat(buf+last_sent_len); + last_sent_len += c; slen -= c; - offset += c; } return last_sent_len; @@ -270,11 +217,7 @@ packet_get_next(register uint8_t max_xmit, __xdata uint8_t *buf) last_sent_is_injected = false; slen = serial_read_available(); - if (force_resend || - (feature_opportunistic_resend && - last_sent_is_resend == false && - last_sent_len != 0 && - slen < PACKET_RESEND_THRESHOLD)) { + if (force_resend) { if (max_xmit < last_sent_len) { return 0; } @@ -348,8 +291,8 @@ packet_get_next(register uint8_t max_xmit, __xdata uint8_t *buf) // We are now looking for a new packet (mav_pkt_len == 0) while (slen > 0) { - register uint8_t c = serial_peek(); - if (c == MAVLINK10_STX) { + register uint8_t c = serial_peekx(0); + if (c == MAVLINK10_STX || c == MAVLINK20_STX) { if (slen == 1) { // we got a bare MAVLink header byte if (last_sent_len == 0) { @@ -362,9 +305,9 @@ packet_get_next(register uint8_t max_xmit, __xdata uint8_t *buf) } break; } - mav_pkt_len = serial_peek2(); - if (mav_pkt_len >= 255-8 || - mav_pkt_len+8 > mav_max_xmit) { + mav_pkt_len = serial_peekx(1); + if (mav_pkt_len >= 255-(8+4+13) || + mav_pkt_len+(8+4+13) > mav_max_xmit) { // its too big for us to cope with mav_pkt_len = 0; last_sent[last_sent_len++] = serial_read(); // Send the STX and try again (we will lose framing) @@ -375,6 +318,13 @@ packet_get_next(register uint8_t max_xmit, __xdata uint8_t *buf) // the length byte doesn't include // the header or CRC mav_pkt_len += 8; + if (c == MAVLINK20_STX) { + mav_pkt_len += 4; + if (slen > 2 && (serial_peekx(2) & 1)) { + // it is signed + mav_pkt_len += 13; + } + } if (last_sent_len != 0) { // send what we've got so far, diff --git a/Firmware/radio/packet.h b/Firmware/radio/packet.h index 33db6cae..d06ee37d 100644 --- a/Firmware/radio/packet.h +++ b/Firmware/radio/packet.h @@ -70,6 +70,7 @@ extern void packet_set_serial_speed(uint16_t speed); /// extern void packet_inject(__xdata uint8_t *buf, __pdata uint8_t len); -// mavlink 1.0 marker +// mavlink 1.0 and 2.0 markers #define MAVLINK10_STX 254 +#define MAVLINK20_STX 253 diff --git a/Firmware/radio/parameters.c b/Firmware/radio/parameters.c index 623f494b..ae9d7f1f 100644 --- a/Firmware/radio/parameters.c +++ b/Firmware/radio/parameters.c @@ -69,9 +69,9 @@ __code const struct parameter_info { {"LBT_RSSI", 0}, {"MANCHESTER", 0}, {"RTSCTS", 0}, - {"MAX_WINDOW", 131}, + {"MAX_WINDOW", 131}, #ifdef INCLUDE_AES - {"ENCRYPTION_LEVEL", 0}, // no Enycryption (0), 128 or 256 bit key + {"ENCRYPTION_LEVEL", 0}, // no Enycryption (0), 128 or 256 bit key #endif }; @@ -210,8 +210,6 @@ param_set(__data enum ParamID param, __pdata param_t value) break; case PARAM_OPPRESEND: - feature_opportunistic_resend = value?true:false; - value = feature_opportunistic_resend?1:0; break; case PARAM_RTSCTS: diff --git a/Firmware/radio/parameters.h b/Firmware/radio/parameters.h index 9d8d9978..d8f2a827 100644 --- a/Firmware/radio/parameters.h +++ b/Firmware/radio/parameters.h @@ -53,7 +53,7 @@ enum ParamID { PARAM_TXPOWER, // transmit power (dBm) PARAM_ECC, // ECC using golay encoding PARAM_MAVLINK, // MAVLink framing, 0=ignore, 1=use, 2=rc-override - PARAM_OPPRESEND, // opportunistic resend + PARAM_OPPRESEND, // opportunistic resend // DISABLED PARAM_MIN_FREQ, // min frequency in MHz PARAM_MAX_FREQ, // max frequency in MHz PARAM_NUM_CHANNELS, // number of hopping channels diff --git a/Firmware/radio/product.mk b/Firmware/radio/product.mk index bfb5bb93..cf21a154 100644 --- a/Firmware/radio/product.mk +++ b/Firmware/radio/product.mk @@ -27,8 +27,8 @@ # Makefile for the Si1000 radio application # -VERSION_MAJOR = 1 -VERSION_MINOR = 13 +VERSION_MAJOR = 2 +VERSION_MINOR = 0 PRODUCT = radio~$(BOARD) PRODUCT_DIR := $(patsubst %/,%,$(dir $(lastword $(MAKEFILE_LIST)))) diff --git a/Firmware/radio/segment.rules b/Firmware/radio/segment.rules new file mode 100644 index 00000000..d793fbba --- /dev/null +++ b/Firmware/radio/segment.rules @@ -0,0 +1,46 @@ +# segment.rules files assign source code modules to specific banks +# These files are only used when we build code with cpu supporting banking +# The final segment.rules file is constructed from any segment.rules found in +# the search path, defined in the CPU Makefile +# When building bankable code, the bank-alloc.py script automatically allocates +# modules to banks. segment.rules files provide hints, instructing the script +# as to which files are safe to move around and which files to leave alone +# In other words, only specify a rule for a file if you need to +# comments starting with "#" are supported +# The file spec in rules is actually interpreted as a python regex so you can +# write a rule that will match multiple files +# +# general rules -- +# SDCC's standard libraries will always go in CSEG - We don't touch them +# Interrupt code must be in HOME. Specify all files with an ISR here +# All files without an associated rule get allocated to a bank automatically + +# Files with ISRs must be in HOME +BANK3 timer.c +BANK3 radio.c +BANK3 serial.c +HOME main.c +BANK3 printfl.c + +BANK3 rtc.c +BANK3 pins_user.c +BANK3 mavlink.c + +BANK3 packet.c +BANK3 golay.c +BANK3 crc.c + +# There is a Parameter shared between these functions and should be in the same bank? +BANK3 parameters.c +BANK3 aesEncrypt.c +BANK3 at.c +BANK3 main_init.c + +# freq_hooping needs to be in the same bank as tdm (due to __nonbanked to save space) +BANK3 freq_hopping.c +BANK3 tdm.c + + +# Flash MUST be in bank3 as it needs to read the end of this page +BANK3 flash.c + diff --git a/Firmware/radio/serial.c b/Firmware/radio/serial.c index 7bf6fa12..13202739 100644 --- a/Firmware/radio/serial.c +++ b/Firmware/radio/serial.c @@ -461,18 +461,6 @@ serial_peek(void) return c; } -uint8_t -serial_peek2(void) -{ - register uint8_t c; - - ES0_SAVE_DISABLE; - c = BUF_PEEK2(rx); - ES0_RESTORE; - - return c; -} - uint8_t serial_peekx(uint16_t offset) { diff --git a/Firmware/radio/serial.h b/Firmware/radio/serial.h index e7b65102..9f90d26a 100644 --- a/Firmware/radio/serial.h +++ b/Firmware/radio/serial.h @@ -115,13 +115,6 @@ extern uint8_t serial_read(void); /// extern uint8_t serial_peek(void); -/// peek at the byte after next from the serial port, without removing it -/// caller must ensure serial available is > 1 -/// -/// @return The byte after next in the receive FIFO. -/// -extern uint8_t serial_peek2(void); - /// peek at the byte x bytes in from the serial port, without removing it /// caller must ensure serial available is > offset /// diff --git a/Firmware/radio/tdm.c b/Firmware/radio/tdm.c index 5a40f43e..f412ca39 100644 --- a/Firmware/radio/tdm.c +++ b/Firmware/radio/tdm.c @@ -987,80 +987,81 @@ tdm_init(void) #define REGULATORY_MAX_WINDOW (((1000000UL/16)*4)/10) #define LBT_MIN_TIME_USEC 5000 - - // tdm_build_timing_table(); - - // calculate how many 16usec ticks it takes to send each byte - ticks_per_byte = (8+(8000000UL/(air_rate*1000UL)))/16; - ticks_per_byte++; - - // calculate the minimum packet latency in 16 usec units - // we initially assume a preamble length of 40 bits, then - // adjust later based on actual preamble length. This is done - // so that if one radio has antenna diversity and the other - // doesn't, then they will both using the same TDM round timings - packet_latency = (8+(10/2)) * ticks_per_byte + 13; - - if (feature_golay) { - max_data_packet_length = (MAX_PACKET_LENGTH/2) - (6+sizeof(trailer)); - - // golay encoding doubles the cost per byte - ticks_per_byte *= 2; - - // and adds 4 bytes - packet_latency += 4*ticks_per_byte; - } else { - max_data_packet_length = MAX_PACKET_LENGTH - sizeof(trailer); - } - - // set the silence period to two times the packet latency - silence_period = 2*packet_latency; - - // set the transmit window to allow for 3 full sized packets - window_width = 3*(packet_latency+(max_data_packet_length*(uint32_t)ticks_per_byte)); - - // if LBT is enabled, we need at least 3*5ms of window width - if (lbt_rssi != 0) { - // min listen time is 5ms - lbt_min_time = LBT_MIN_TIME_USEC/16; - window_width = constrain(window_width, 3*lbt_min_time, window_width); - } - - // the window width cannot be more than 0.4 seconds to meet US - // regulations - if (window_width >= REGULATORY_MAX_WINDOW && num_fh_channels > 1) { - window_width = REGULATORY_MAX_WINDOW; - } - - // user specified window is in milliseconds - if (window_width > param_get(PARAM_MAX_WINDOW)*(1000/16)) { - window_width = param_get(PARAM_MAX_WINDOW)*(1000/16); - } - - // make sure it fits in the 13 bits of the trailer window - if (window_width > 0x1fff) { - window_width = 0x1fff; - } - - tx_window_width = window_width; - - // now adjust the packet_latency for the actual preamble - // length, so we get the right flight time estimates, while - // not changing the round timings - packet_latency += ((settings.preamble_length-10)/2) * ticks_per_byte; - - // tell the packet subsystem our max packet size, which it - // needs to know for MAVLink packet boundary detection - i = (tx_window_width - packet_latency) / ticks_per_byte; - if (i > max_data_packet_length) { - i = max_data_packet_length; - } - packet_set_max_xmit(i); - + + // tdm_build_timing_table(); + + // calculate how many 16usec ticks it takes to send each byte + ticks_per_byte = (8+(8000000UL/(air_rate*1000UL)))/16; + ticks_per_byte++; + + // calculate the minimum packet latency in 16 usec units + // we initially assume a preamble length of 40 bits, then + // adjust later based on actual preamble length. This is done + // so that if one radio has antenna diversity and the other + // doesn't, then they will both using the same TDM round timings + packet_latency = (8+(10/2)) * ticks_per_byte + 13; + + if (feature_golay) { + max_data_packet_length = (MAX_PACKET_LENGTH/2) - (6+sizeof(trailer)); + + // golay encoding doubles the cost per byte + ticks_per_byte *= 2; + + // and adds 4 bytes + packet_latency += 4*ticks_per_byte; + } else { + max_data_packet_length = MAX_PACKET_LENGTH - sizeof(trailer); + } + + // set the silence period to two times the packet latency + silence_period = 2*packet_latency; + + // set the transmit window to allow for 3 full sized packets + window_width = 3*(packet_latency+(max_data_packet_length*(uint32_t)ticks_per_byte)); + + // min listen time is 5ms + lbt_min_time = LBT_MIN_TIME_USEC/16; + + // if LBT is enabled, we need at least 3*5ms of window width + if (lbt_rssi != 0) { + window_width = constrain(window_width, 3*lbt_min_time, window_width); + } + + // the window width cannot be more than 0.4 seconds to meet US + // regulations + if (window_width >= REGULATORY_MAX_WINDOW && num_fh_channels > 1) { + window_width = REGULATORY_MAX_WINDOW; + } + + // user specified window is in milliseconds + if (window_width > param_get(PARAM_MAX_WINDOW)*(1000/16)) { + window_width = param_get(PARAM_MAX_WINDOW)*(1000/16); + } + + // make sure it fits in the 13 bits of the trailer window + if (window_width > 0x1fff) { + window_width = 0x1fff; + } + + tx_window_width = window_width; + + // now adjust the packet_latency for the actual preamble + // length, so we get the right flight time estimates, while + // not changing the round timings + packet_latency += ((settings.preamble_length-10)/2) * ticks_per_byte; + + // tell the packet subsystem our max packet size, which it + // needs to know for MAVLink packet boundary detection + i = (tx_window_width - packet_latency) / ticks_per_byte; + if (i > max_data_packet_length) { + i = max_data_packet_length; + } + packet_set_max_xmit(i); + #ifdef TDM_SYNC_LOGIC - TDM_SYNC_PIN = false; + TDM_SYNC_PIN = false; #endif // TDM_SYNC_LOGIC - + // crc_test(); // tdm_test_timing(); diff --git a/Firmware/tools/atcommander.py b/Firmware/tools/atcommander.py new file mode 100755 index 00000000..c7d800c9 --- /dev/null +++ b/Firmware/tools/atcommander.py @@ -0,0 +1,383 @@ +#!/usr/bin/env python +# +# Provide command line access to AT command set on radios +# + +import serial, sys, argparse, time, fdpexpect + +class ATCommandSet(object): + ''' Interface to the AT command set ''' + + ### AT Command Constants ### + + # Prefix determines if commanding attached or linked radio + AT_LOCAL_PREFIX = 'AT' + AT_REMOTE_PREFIX = 'RT' + + # AT commands that are implemented in this class + AT_SHOW_BRD_TYPE = 'I2' + AT_SHOW_BRD_FREQ = 'I3' + AT_SHOW_BRD_VER = 'I4' + AT_SHOW_PARAM = 'I5' + AT_EXIT = 'O' + AT_PARAM = 'S' + AT_REBOOT = 'Z' + AT_PARAM_WRITE = '&W' + # AT commands yet to be implemented here + AT_SHOW_VER_LONG = 'I0' + AT_SHOW_VER = 'I1' + AT_SHOW_TDM = 'I6' + AT_SHOW_RSSI = 'I7' + AT_PARAM_FACTORY = '&F' + AT_DEBUG_RSSI = '&T=RSSI' + AT_DEBUG_TDM = '&T=TDM' + AT_DEBUG_OFF = '&T' + + # Parameters are gotten with AT_PARAM + PARAM_* + '?" + # Parameters are set with AT_PARAM + PARAM_* + '=' + value + PARAM_FORMAT = 0 + PARAM_SERIAL_SPEED = 1 + PARAM_AIR_SPEED = 2 + PARAM_NETID = 3 + PARAM_TXPOWER = 4 + PARAM_ECC = 5 + PARAM_MAVLINK = 6 + PARAM_OPPRESEND = 7 + PARAM_MIN_FREQ = 8 + PARAM_MAX_FREQ = 9 + PARAM_NUM_CHANNELS = 10 + PARAM_DUTY_CYCLE = 11 + PARAM_LBT_RSSI = 12 + PARAM_MANCHESTER = 13 + PARAM_RTSCTS = 14 + + ### Internals ### + + # Create object and immediate attempt to connect to radio + def __init__(self, device, baudrate=57600, debug=False, + dsrdtr=False, rtscts=False, xonxoff=False): + # Initialize object data members + self.is_command = False # Track if we've entered command mode + self.is_remote = False # Track if operating on remote radio + self.read_timeout = 5 # Max time to wait for data + + logfile=None + if debug: + logfile=sys.stdout + + # Initialize the serial connection + # Note: we pass the buck on raised exceptions + self.port = serial.Serial(device, baudrate=baudrate, timeout=0, + dsrdtr=dsrdtr, rtscts=rtscts, xonxoff=xonxoff) + self.ser = fdpexpect.fdspawn(self.port.fileno(), logfile=logfile) + + # Send raw text to radio + def __send(self, text): + if (self.port is None) or (self.ser is None): + return False + + try: + res = self.ser.send(text) + time.sleep(0.2) # Let the serial line catch up + return res + except: + return False + + # Form an AT command and send to radio + def __send_at(self, command): + # Don't send bytes if in "normal" mode, other radio is listening! + if not self.is_command: + return False + + prefix = ATCommandSet.AT_LOCAL_PREFIX + if self.is_remote and (command != ATCommandSet.AT_EXIT): + prefix = ATCommandSet.AT_REMOTE_PREFIX + text = '\r\n' + prefix + str(command) + '\r\n' + return not not self.__send(text) + + # Look for 'pattern' (string RE) and return MatchObject if seen before read_timeout + def __expect(self, pattern_list): + if (self.port is None) or (self.ser is None): + return False + + try: + self.ser.expect(pattern_list, timeout=self.read_timeout) + res = self.ser.match + time.sleep(0.2) # Let the serial line catch up + return res + except: + return False + + # Send AT command, then look for pattern + def __query(self, command, pattern): + if not self.__send_at(command): + return False + val = self.__expect(pattern) + return val + + # Query for an int + def __query_int(self, command): + val = self.__query(command, ['(\d+)\r\n']) + if val: + return int(val.group(0)) + return False + + # Query for a float + def __query_float(self, command): + val = self.__query(command, ['(\d+\.\d+)\r\n']) + if val: + return float(val.group(0)) + return False + + # Query for literal text (return True if found) + def __query_exact(self, command, text): + return not not self.__query(command, ['%s\r\n' % text]) + + + ### Manage command mode ### + + def enter_command_mode(self): + # Technically okay to resend this command, but won't see an 'OK' back + if self.is_command: + return False + + # Will raise a timeout exception if already in command mode + # (due to another process leaving it that way?) + time.sleep(1) + if not self.__send('+++'): + return False + time.sleep(1) + if not self.__expect(['OK']): + return False + + self.is_command = True + return True + + def leave_command_mode(self): + # Don't send bytes if in "normal" mode, other radio is listening! + self.__send_at(ATCommandSet.AT_EXIT) + self.is_command = False + + def leave_command_mode_force(self): + # Overrides mode check, use only if radio is "stuck" in command mode + self.is_command = True + self.leave_command_mode() + + def is_command_mode(self): + return self.is_command + + ### Select local or remote operation ### + + def set_remote_mode(self, remote=False): + # True = remote (linked) radio, False = local (attached) radio + self.is_remote = remote + + def is_remote_mode(self): + return self.is_remote + + ### Get general info ### + + def get_radio_version(self): + return self.__query_float(ATCommandSet.AT_SHOW_VER) + + def get_board_type(self): + return self.__query_int(ATCommandSet.AT_SHOW_BRD_TYPE) + + def get_board_frequency(self): + return self.__query_int(ATCommandSet.AT_SHOW_BRD_FREQ) + + def get_board_version(self): + return self.__query_int(ATCommandSet.AT_SHOW_BRD_VER) + + # Return a multi-line string containing all parameters, for display + def get_params_text(self): + res = self.__query(ATCommandSet.AT_SHOW_PARAM, ['(S0:.*S14:.*)\r\n']) + if res: + return res.group(0) + else: + return "** Could not access parameters **" + + ### Parameters (settings) access ### + + def get_param(self, p_id): + # Assumes all params are ints + return self.__query_int(ATCommandSet.AT_PARAM + str(p_id) + '?') + + def set_param(self, p_id, p_val): + return self.__query_exact(ATCommandSet.AT_PARAM + str(p_id) + '=' + str(p_val), 'OK') + + # Stores params to EEPROM (necessary after 1+ set_param() calls) + def write_params(self): + return self.__query_exact(ATCommandSet.AT_PARAM_WRITE, 'OK') + + ### Miscellaneous ### + + # Reboot the radio (necessary for settings to take effect) + def reboot(self): + if not self.__send_at(ATCommandSet.AT_REBOOT): + return False + # The local radio leaves command mode upon reboot + if not self.is_remote: + self.is_command = False + return True + +### User Interface ### + +if __name__ == '__main__': + param_map = { 'format' : ATCommandSet.PARAM_FORMAT, + 'serialspeed' : ATCommandSet.PARAM_SERIAL_SPEED, + 'airspeed' : ATCommandSet.PARAM_AIR_SPEED, + 'netid' : ATCommandSet.PARAM_NETID, + 'txpower' : ATCommandSet.PARAM_TXPOWER, + 'ecc' : ATCommandSet.PARAM_ECC, + 'mavlink' : ATCommandSet.PARAM_MAVLINK, + 'oppresend' : ATCommandSet.PARAM_OPPRESEND, + 'minfreq' : ATCommandSet.PARAM_MIN_FREQ, + 'maxfreq' : ATCommandSet.PARAM_MAX_FREQ, + 'channels' : ATCommandSet.PARAM_NUM_CHANNELS, + 'duty' : ATCommandSet.PARAM_DUTY_CYCLE, + 'lbtrssi' : ATCommandSet.PARAM_LBT_RSSI, + 'manchester' : ATCommandSet.PARAM_MANCHESTER, + 'rtscts' : ATCommandSet.PARAM_RTSCTS } + + # Grok arguments + parser = argparse.ArgumentParser(description='Change settings on local and remote radio.', + epilog="Settable parameters (can use multiple --set-*): %s" % \ + " ".join(sorted(param_map.keys()))) + parser.add_argument("--baudrate", type=int, default=57600, help='connect at baud rate') + parser.add_argument("--rtscts", action='store_true', default=False, help='connect using rtscts') + parser.add_argument("--dsrdtr", action='store_true', default=False, help='connect using dsrdtr') + parser.add_argument("--xonxoff", action='store_true', default=False, help='connect using xonxoff') + parser.add_argument("--force", action='store_true', default=False, help='cycle command mode first') + parser.add_argument("--debug", action='store_true', default=False, help='intermix raw AT traffic') + parser.add_argument("--list-local", action='store_true', default=False, + help='list local parameters and exit') + parser.add_argument("--list-remote", action='store_true', default=False, + help='list remote parameters and exit') + parser.add_argument("--set-local", nargs=2, action='append', metavar=('PARAM', 'VALUE'), + help='set local parameter (will reboot radio at end)') + parser.add_argument("--set-remote", nargs=2, action='append', metavar=('PARAM', 'VALUE'), + help='set remote parameter (will reboot radio at end)') + parser.add_argument("--set-both", nargs=2, action='append', metavar=('PARAM', 'VALUE'), + help='set on BOTH radios (takes precedence)') + parser.add_argument("device", help='locally attached radio device') + args = parser.parse_args() + + # If no get/set was requested, then bail + if not (args.list_local or args.list_remote or \ + args.set_local or args.set_remote or args.set_both): + print "Please specify a --list-* or --set-* operation (try -h if unsure)" + sys.exit(0) + # Also bail if attempting to get and set (we could, but we don't) + if (args.list_local or args.list_remote) and \ + (args.set_local or args.set_remote or args.set_both): + print "We don't support listing and setting in the same command" + sys.exit(0) + + # Parse any --set-* args and build dictionaries of parameters to set + # Note: --set-both overrides --set-local and --set-remote. Beyond that, + # we don't guard against the user specifying strange combinations. + def _parse_set(params, myset): + for pair in params: + prm, val = pair + if prm not in param_map: + print "Parameter not valid: %s" % prm + sys.exit(-1) + try: + myset[prm] = int(val) + except: + print "Param '%s' value must be an integer: %s" % (prm, val) + sys.exit(-1) + return myset + local_set = {} + remote_set = {} + if args.set_local: + local_set = _parse_set(args.set_local, local_set) + if args.set_remote: + remote_set = _parse_set(args.set_remote, remote_set) + if args.set_both: + local_set = _parse_set(args.set_both, local_set) + remote_set = _parse_set(args.set_both, remote_set) + + # Initialize the serial connection + at = ATCommandSet(args.device, baudrate=args.baudrate, dsrdtr=args.dsrdtr, + rtscts=args.rtscts, xonxoff=args.xonxoff, debug=args.debug) + + # In case the radio was left in command mode, we can force it out + # (Could just not "enter" command mode, but this seems safer somehow) + if args.force: + print "Forcing out of command mode first..." + at.leave_command_mode_force() + + # Try to enter command mode, bail if radio doesn't give expected response + print "Entering command mode..." + if not at.enter_command_mode(): + print "Could not enter command mode; try --force" + sys.exit(-1) + + # If --list-* was requested, do that and exit (don't set any parameters) + def _list_info(): + r_ver = at.get_radio_version() + if not r_ver: + print "** Could not access radio **" + else: + print "radio version: %g board type: %d board version: %d" % \ + (r_ver, + at.get_board_type() or -1, + at.get_board_version() or -1) + print "Parameters: \n%s" % at.get_params_text() + if args.list_local: + print "Querying local radio..." + _list_info() + if args.list_remote: + at.set_remote_mode(True) + print "Querying remote radio..." + _list_info() + at.set_remote_mode(False) + if args.list_local or args.list_remote: + print "Leaving command mode..." + at.leave_command_mode() + sys.exit(0) + + # If --set-* was requested, attempt to do all of them, then write and reboot + # only the radio(s) that was/were changed + def _set_params(myset): + for prm in myset: + if at.set_param(param_map[prm], myset[prm]): + print "Set %s to %d" % (prm, myset[prm]) + else: + print "Failed to set %s, aborting without saving changes." % prm + return False + if at.write_params(): + print "Wrote parameters to EEPROM." + else: + print "Failed to write parameters to EEPROM, aborting without saving changes." + return False + if at.reboot(): + print "Commanded reboot; changes should be in effect momentarily." + else: + print "Failed to command reboot; please manually reboot the radio." + return True + # Try remote radio first + remote_failed = False + if remote_set: + at.set_remote_mode(True) + if not at.get_radio_version: + print "Could not contact remote radio, aborting without saving changes." + remote_failed = True + else: + print "Changing settings on remote radio..." + remote_failed = _set_params(remote_set) + at.set_remote_mode(False) + # Try local radio second (only if no remote failures) + if local_set and not remote_failed: + # Since we have to successfully be in command mode, don't need more checks + print "Changing settings on local radio..." + _set_params(local_set) + + # Always leave command mode when finished + # (If we rebooted the local radio at the very end, this will be ignored) + print "Leaving command mode..." + at.leave_command_mode() + sys.exit(0) + diff --git a/Firmware/tools/mavtester.py b/Firmware/tools/mavtester.py index 6ff66b76..e188415c 100755 --- a/Firmware/tools/mavtester.py +++ b/Firmware/tools/mavtester.py @@ -17,9 +17,14 @@ 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: @@ -32,19 +37,60 @@ vehicle = mavutil.mavlink_connection(opts.port2, baud=opts.baudrate, input=False) vehicle.setup_logfile('vehicle.tlog') +print("Draining ports") +gcs.port.timeout = 1 +vehicle.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 = vehicle.port.read(1024) + if not r: + break + print("Drained %u bytes from vehicle" % len(r)) + time.sleep(0.01) + if opts.rtscts: + print("Enabling RTSCTS") gcs.set_rtscts(True) vehicle.set_rtscts(True) +else: + gcs.set_rtscts(False) + vehicle.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) + vehicle.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=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 @@ -81,7 +127,11 @@ def send_telemetry(): 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) - 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) + 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) @@ -156,6 +206,7 @@ def recv_vehicle(): print(m) stats.vehicle_received += 1 if m.get_type() in ['RADIO','RADIO_STATUS']: + #print('VRADIO: ', str(m)) stats.vehicle_radio_received += 1 stats.vehicle_txbuf = m.txbuf stats.vehicle_fixed = m.fixed @@ -185,6 +236,7 @@ def recv_GCS(): 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 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() diff --git a/Firmware/tools/pattern.py b/Firmware/tools/pattern.py index bd49605f..ce4979d2 100755 --- a/Firmware/tools/pattern.py +++ b/Firmware/tools/pattern.py @@ -1,7 +1,7 @@ #!/usr/bin/env python # reflect input bytes to output, printing as it goes -import serial, sys, optparse, time +import serial, sys, optparse, time, random parser = optparse.OptionParser("pattern") parser.add_option("--baudrate", type='int', default=57600, help='baud rate') @@ -13,6 +13,8 @@ parser.add_option("--rtscts", action='store_true', default=False, help='enable rtscts') parser.add_option("--dsrdtr", action='store_true', default=False, help='enable dsrdtr') parser.add_option("--xonxoff", action='store_true', default=False, help='enable xonxoff') +parser.add_option("--randlength", action='store_true', default=False, help='use random pattern length') +parser.add_option("--reopen", type='float', default=0, help='reopen every N seconds') opts, args = parser.parse_args() @@ -25,9 +27,13 @@ port = serial.Serial(device, opts.baudrate, timeout=0, dsrdtr=opts.dsrdtr, rtscts=opts.rtscts, xonxoff=opts.xonxoff) counter = 0 +open_t = time.time() while True: try: buf = opts.pattern[:] + if opts.randlength: + rlen = int(random.uniform(0, len(buf))) + buf = buf[:rlen] if opts.counter: buf += "%02u" % (counter % 100) if opts.crlf: @@ -46,6 +52,12 @@ pass if opts.delay > 0.0: time.sleep(opts.delay) + if opts.reopen > 0 and time.time() > open_t + opts.reopen: + port.close() + port = serial.Serial(device, opts.baudrate, timeout=0, + dsrdtr=opts.dsrdtr, rtscts=opts.rtscts, xonxoff=opts.xonxoff) + open_t = time.time() + print("Reopened") counter += 1 except KeyboardInterrupt: sys.exit(0) diff --git a/Firmware/tools/test_binary.py b/Firmware/tools/test_binary.py new file mode 100755 index 00000000..d6fbdbac --- /dev/null +++ b/Firmware/tools/test_binary.py @@ -0,0 +1,58 @@ +#!/usr/bin/env python + +''' +test sending of binary data +''' + +import sys, time, os, serial + +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") +parser.add_option("--port2", default=None, help="serial port 2") +parser.add_option("--blocksize", type='int', default=5000, help="blocksize") +parser.add_option("--timeout", type='int', default=8, help="timeout") + +(opts, args) = parser.parse_args() + + +port = [] +port.append(serial.Serial(opts.port1, opts.baudrate, timeout=5, dsrdtr=False, rtscts=True, xonxoff=False)) +port.append(serial.Serial(opts.port2, opts.baudrate, timeout=5, dsrdtr=False, rtscts=True, xonxoff=False)) + +r = open("/dev/urandom") + +def read_timeout(p, length, timeout): + '''read len bytes with given timeout''' + t_start = time.time() + ret = '' + while (time.time() < t_start + timeout): + b = p.read(length) + if len(b) == 0: + print("short read") + break + ret += b + length -= len(b) + if length == 0: + break + return ret + +while True: + b = r.read(opts.blocksize) + t0 = time.time() + port[0].write(b) + b2 = read_timeout(port[1], opts.blocksize, opts.timeout) + port[1].write(b) + b3 = read_timeout(port[0], opts.blocksize, opts.timeout) + t1 = time.time() + print(len(b), len(b2), len(b3), ' %.3f kByte/sec' % (1.0e-3*opts.blocksize*2/(t1-t0))) + if b != b2: + print("data error b2", b[:8], b2[:8]) + break + if b != b3: + print("data error b3", b[:8], b3[:8]) + break + time.sleep(1) diff --git a/Firmware/tools/uploader.py b/Firmware/tools/uploader.py index 86ad4f39..e93ffaba 100755 --- a/Firmware/tools/uploader.py +++ b/Firmware/tools/uploader.py @@ -321,10 +321,17 @@ def send(self, s): self.port.write(s) sys.stdout.write(s) + def setBaudrate(self, baudrate): + try: + self.port.setBaudrate(baudrate) + except Exception: + # for pySerial 3.0, which doesn't have setBaudrate() + self.port.baudrate = baudrate + def autosync(self): '''use AT&UPDATE to put modem in update mode''' if self.atbaudrate != 115200: - self.port.setBaudrate(self.atbaudrate) + self.setBaudrate(self.atbaudrate) print("Trying autosync") self.send('\r\n') time.sleep(1.0) @@ -342,11 +349,11 @@ def autosync(self): time.sleep(0.7) self.port.flushInput() if self.atbaudrate != 115200: - self.port.setBaudrate(115200) + self.setBaudrate(115200) print("Sent update command") return True if self.atbaudrate != 115200: - self.port.setBaudrate(115200) + self.setBaudrate(115200) return False @@ -390,6 +397,7 @@ def upload(self, fw, erase_params = False): parser.add_argument("--mavlink", action='store_true', default=False, help='update over MAVLink') parser.add_argument("--mavport", type=int, default=0, help='MAVLink port number') parser.add_argument("--debug", type=int, default=0, help='debug level') + parser.add_argument('--port', required=True, action="store", help="port to upload to") parser.add_argument('firmware', action="store", help="Firmware file to be uploaded") args = parser.parse_args() diff --git a/README.markdown b/README.markdown index 9e4254f6..80517641 100644 --- a/README.markdown +++ b/README.markdown @@ -10,7 +10,7 @@ SiK is a collection of firmware and tools for radios based on the cheap, versati ## Documentation For user documentation please see this site: -http://planner.ardupilot.com/wiki/other-project-and-common-topics/common-optional-hardware/common-telemetry-landingpage/common-3dr-radio-version-2/ +http://ardupilot.org/copter/docs/common-sik-telemetry-radio.html Addition configuration guide can also be found here: diff --git a/SiKUploader/uploader/SiKUploader.csproj b/SiKUploader/uploader/SiKUploader.csproj index 1ca04b29..d291cdaa 100644 --- a/SiKUploader/uploader/SiKUploader.csproj +++ b/SiKUploader/uploader/SiKUploader.csproj @@ -1,5 +1,5 @@ - + Debug x86