From a8b5cef1d0c834388f488e332f8771d7f128235e Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Mon, 16 Oct 2017 22:14:35 +0200 Subject: [PATCH] preliminary (untested) port of new protocol to python --- Firmware/MotorControl/low_level.h | 2 +- Firmware/MotorControl/protocol.cpp | 12 +- Firmware/MotorControl/protocol.hpp | 16 +-- Firmware/tools/odrive/protocol.py | 175 +++++++++++++++++++++++++++++ 4 files changed, 191 insertions(+), 14 deletions(-) create mode 100644 Firmware/tools/odrive/protocol.py diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index b0f1f58a..56cfa566 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -153,7 +153,7 @@ typedef struct{ /* Exported constants --------------------------------------------------------*/ extern const size_t num_motors; -extern float elec_rad_per_enc; // TODO: make this const (once exposed_floats is gone) +extern const float elec_rad_per_enc; /* Exported variables --------------------------------------------------------*/ extern float vbus_voltage; extern Motor_t motors[]; diff --git a/Firmware/MotorControl/protocol.cpp b/Firmware/MotorControl/protocol.cpp index 989e5bc5..e0955fbb 100644 --- a/Firmware/MotorControl/protocol.cpp +++ b/Firmware/MotorControl/protocol.cpp @@ -108,7 +108,7 @@ int StreamToPacketConverter::write_bytes(const uint8_t *buffer, size_t length) { } else if (header_index_ == 3 && calc_crc8(CRC8_INIT, header_buffer_, 3)) { header_index_ = 0; } else if (header_index_ == 3) { - packet_length_ = header_buffer_[1] + 2; // expect 2 more bytes than indicated (CRC16) + packet_length_ = header_buffer_[1]; } } else if (packet_index_ < sizeof(packet_buffer_)) { // Process payload byte @@ -117,7 +117,7 @@ int StreamToPacketConverter::write_bytes(const uint8_t *buffer, size_t length) { // If both header and packet are fully received, hand it on to the packet processor if (header_index_ == 3 && packet_index_ == packet_length_) { - result |= output_.write_packet(packet_buffer_, packet_length_ - 2); + result |= output_.write_packet(packet_buffer_, packet_length_); header_index_ = packet_index_ = packet_length_ = 0; } buffer++; @@ -209,6 +209,8 @@ int BidirectionalPacketBasedChannel::write_packet(const uint8_t* buffer, size_t uint16_t seq_no = read_le(&buffer, &length); if (seq_no & 0x8000) { + if (calc_crc16(crc16, crc16_termination, sizeof(crc16_termination))) + return -1; // TODO: ack handling } else { // TODO: think about some kind of ordering guarantees @@ -236,14 +238,14 @@ int BidirectionalPacketBasedChannel::write_packet(const uint8_t* buffer, size_t uint16_t expected_response_length = read_le(&buffer, &length); // Let the endpoint do the processing - size_t requested_size = expected_response_length < (sizeof(tx_buf_) - 2) ? expected_response_length : (sizeof(tx_buf_) - 2); + size_t requested_size = expected_response_length < (sizeof(tx_buf_) - 4) ? expected_response_length : (sizeof(tx_buf_) - 4); size_t remaining_size = requested_size; - endpoint->handle(buffer, length, tx_buf_ + 2, &remaining_size); + endpoint->handle(buffer, length - 2, tx_buf_ + 2, &remaining_size); // Send response if (expect_response) { write_le(seq_no | 0x8000, tx_buf_); - output_.write_packet(tx_buf_, (requested_size - remaining_size) + 2); + output_.write_packet(tx_buf_, (requested_size - remaining_size) + 4); } } diff --git a/Firmware/MotorControl/protocol.hpp b/Firmware/MotorControl/protocol.hpp index 7e727797..289b7986 100644 --- a/Firmware/MotorControl/protocol.hpp +++ b/Firmware/MotorControl/protocol.hpp @@ -56,6 +56,14 @@ #include #include "crc.hpp" + +constexpr uint8_t SYNC_BYTE = '$'; +constexpr uint8_t CRC8_INIT = 0; +constexpr uint16_t CRC16_INIT = 0; +constexpr uint16_t PROTOCOL_VERSION = 1; +constexpr uint16_t TX_BUF_SIZE = 64; + + template inline size_t write_le(T value, uint8_t* buffer); @@ -216,14 +224,6 @@ private: }; - -constexpr uint8_t SYNC_BYTE = '$'; -constexpr uint8_t CRC8_INIT = 0; -constexpr uint16_t CRC16_INIT = 0; -constexpr uint16_t PROTOCOL_VERSION = 1; -constexpr uint16_t TX_BUF_SIZE = 64; - - class PacketWriter { public: // @brief Processes a packet. diff --git a/Firmware/tools/odrive/protocol.py b/Firmware/tools/odrive/protocol.py new file mode 100644 index 00000000..1313386f --- /dev/null +++ b/Firmware/tools/odrive/protocol.py @@ -0,0 +1,175 @@ +# See protocol.hpp for an overview of the protocol + +import struct + +SYNC_BYTE = '$' +CRC8_INIT = 0 +CRC16_INIT = 0 +PROTOCOL_VERSION = 1 + +CRC8_DEFAULT = 0x37 # this must match the polynomial in the C++ implementation +CRC16_DEFAULT = 0x3d65 # this must match the polynomial in the C++ implementation + +def calc_crc(remainder, value, polynomial, bitwidth): + topbit = (1 << (bitwidth - 1)) + + # Bring the next byte into the remainder. + remainder ^= (value << (bitwidth - 8)) + for bitnumber in range(0,8): + if (remainder & topbit): + remainder = (remainder << 1) ^ polynomial + else: + remainder = (remainder << 1) + + return remainder & ((1 << bitwidth) - 1) + +def calc_crc8(remainder, value): + if type(value) == bytearray or isinstance(value, list): + for b in value: + remainder = calc_crc(remainder, b, CRC8_DEFAULT, 8) + else: + remainder = calc_crc(remainder, b, CRC8_DEFAULT, 8) + return remainder + +def calc_crc16(remainder, value): + if type(value) == bytearray or isinstance(value, list): + for b in value: + remainder = calc_crc(remainder, b, CRC16_DEFAULT, 16) + else: + remainder = calc_crc(remainder, b, CRC16_DEFAULT, 16) + return remainder + +# Can be verified with http://www.sunshine2k.de/coding/javascript/crc/crc_js.html: +#print(hex(calc_crc8(0x12, [1, 2, 3, 4, 5, 0x10, 0x13, 0x37]))) +#print(hex(calc_crc16(0xfeef, [1, 2, 3, 4, 5, 0x10, 0x13, 0x37]))) + +class StreamWriter(object): + pass + +class PacketWriter(object): + pass + + +class StreamToPacketConverter(StreamWriter): + _header = [] + _packet = [] + _packet_length = 0 + + def __init__(self, output): + self._output = output + + def write_bytes(self, bytes): + result = None + + for b in bytes: + if (len(self._header) < 3): + # Process header byte + self._header.append(b) + if (len(self._header) == 1) and (self._header[0] != SYNC_BYTE): + self._header = [] + elif (len(self._header) == 2) and (self._header[1] & 0x80): + self._header = [] # TODO: support packets larger than 128 bytes + elif (len(self._header) == 3) and calc_crc8(CRC8_INIT, self._header): + self._header = [] + elif (len(self._header) == 3): + self._packet_length = self._header[1] + else: + # Process payload byte + self._packet.append(b) + + # If both header and packet are fully received, hand it on to the packet processor + if (len(self._header) == 3) and (len(self._packet) == self._packet_length): + try: + self._output.write_packet(self._packet) + except Exception, ex: + result = ex + self._header = [] + self._packet = [] + self._packet_length = 0 + + if isinstance(result, Exception): + raise Exception("something went wrong") + + +class PacketToStreamConverter(PacketWriter): + def __init__(self, output): + self._output = output + + def write_packet(self, packet): + if (len(packet) >= 128): + raise Exception("packet larger than 127 currently not supported") + + header = [SYNC_BYTE, len(packet)] + header.append(calc_crc8(CRC8_INIT, header)) + + self._output.write_bytes(header) + self._output.write_bytes(packet) + + +class Channel(PacketWriter): + _outbound_seq_no = 0 + _interface_definition_crc = bytearray(2) + _expected_acks = {} + + def __init__(self, input, output): + """ + Params: + input: A PacketReader where this channel will source packets from on + demand. Alternatively packets can be provided to this channel + directly by calling write_packet on this instance. + output: A PacketWriter where this channel will put outgoing packets. + """ + self._input = input + self._output = output + + def remote_endpoint_operation(self, endpoint_id, input, expect_ack, output_length): + if (len(input) >= 128): + raise Exception("packet larger than 127 currently not supported") + + if (expect_ack): + endpoint_id |= 0x8000 + + self._outbound_seq_no = ((self._outbound_seq_no + 1) & 0x7fff) + seq_no = self._outbound_seq_no + packet = struct.pack('