Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-09-21 15:34:33 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

update test_communication.py to new backend, deferred USB processing, move CRC16 to stream based layer, some protocol refactoring

Browse Source
This commit is contained in:
Samuel Sadok
2017-10-30 23:39:21 +01:00
parent 87498ef1b9
commit ba46e5fb29
13 changed files with 642 additions and 470 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/MotorControl/protocol.cpp
+99 -139
View File
@@ -8,98 +8,80 @@
#include <stdlib.h>
/* Private defines -----------------------------------------------------------*/
//#define DEGUG_PROTOCOL
/* Private macros ------------------------------------------------------------*/
#ifdef DEGUG_PROTOCOL
#define LOG_PROTO(...) do { printf(__VA_ARGS__); osDelay(10); } while (0)
#else
#define LOG_PROTO(...) ((void) 0)
#endif
/* Private typedef -----------------------------------------------------------*/
/* Global constant data ------------------------------------------------------*/
/* Global variables ----------------------------------------------------------*/
/* Private constant data -----------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
// The order in this list must correspond to the order in EndpointTypeID_t
//Oskar: Isn't it better to then have an array of tuples(or structs), so that they are always paired at definition?
const char *type_names_[] = {
"json",
"int32[]",
"float",
"int",
"bool",
"uint16",
"tree"
};
/* Private function prototypes -----------------------------------------------*/
static void hexdump(const uint8_t* buf, size_t len);
static void write_buffer(const uint8_t* input, size_t input_length, size_t* skip, uint8_t** output, size_t* output_length);
static void write_string(const char* str, size_t* skip, uint8_t** output, size_t* output_length);
static inline int write_string(const char* str, StreamSink* output);
/* Function implementations --------------------------------------------------*/
// For debugging only
#if 0
void hexdump(const uint8_t* buf, size_t len) {
for (size_t pos = 0; pos < len; ++pos) {
printf(" %02x", buf[pos]);
if ((((pos + 1) % 16) == 0) || ((pos + 1) == len))
printf("\r\n");
osDelay(1);
osDelay(2);
}
}
#else
void hexdump(const uint8_t* buf, size_t len) {
(void) buf;
(void) len;
}
#endif
// @brief Copies an input buffer to an output buffer, skipping a couple of bytes on the input buffer if required.
// @param input: input buffer
// @param input_length: number of bytes in the input buffer
// @param skip: number of bytes to skip in the input buffer - will be set to max{skip - input_length, 0}
// @param output: output buffer - will be increased by the number of bytes copied
// @param output_length: length of the output buffer - will be decreased by the number of bytes copied
void write_buffer(const uint8_t* input, size_t input_length, size_t* skip, uint8_t** output, size_t* output_length) {
if (*skip >= input_length) {
*skip -= input_length;
} else {
input_length -= *skip;
input += *skip;
*skip = 0;
size_t length = input_length < *output_length ? input_length : *output_length;
memcpy(*output, input, length);
*output += length;
*output_length -= length;
}
static inline int write_string(const char* str, StreamSink* output) {
return output->process_bytes(reinterpret_cast<const uint8_t*>(str), strlen(str));
}
//Oskar: consier a JsonWriter object (as per slack discussion)
static void write_string(const char* str, size_t* skip, uint8_t** output, size_t* output_length) {
write_buffer(reinterpret_cast<const uint8_t*>(str), strlen(str), skip, output, output_length);
}
void Endpoint::write_json(size_t id, size_t* skip, uint8_t** output, size_t* output_length, bool* need_comma) {
if (type_id_ == END_TREE) {
write_string("]}", skip, output, output_length);
void Endpoint::write_json(size_t id, bool* need_comma, StreamSink* output) const {
if (type_ == CLOSE_TREE) {
write_string("]}", output);
*need_comma = true;
return;
} else if (type_id_ < END_TREE) {
} else {
if (*need_comma)
write_string(",", skip, output, output_length);
write_string(",", output);
write_string("{\"name\":\"", skip, output, output_length);
// write name
write_string("{\"name\":\"", output);
if (name_)
write_string(name_, skip, output, output_length);
write_string("\",\"id\":", skip, output, output_length);
write_string(name_, output);
// write endpoint ID
write_string("\",\"id\":", output);
char id_buf[10];
snprintf(id_buf, sizeof(id_buf), "%u", id); // TODO: get rid of printf
write_string(id_buf, skip, output, output_length);
write_string(",\"type\":\"", skip, output, output_length);
if (type_names_[type_id_])
write_string(type_names_[type_id_], skip, output, output_length);
write_string("\"", skip, output, output_length);
write_string(id_buf, output);
if (type_id_ == BEGIN_TREE) {
write_string(",\"content\":[", skip, output, output_length);
// write additional JSON data
if (json_modifier_ && json_modifier_[0]) {
write_string(",", output);
write_string(json_modifier_, output);
}
if (type_ == BEGIN_OBJECT) {
write_string(",\"content\":[", output);
*need_comma = false;
} else {
if (json_modifier_ && json_modifier_[0]) {
write_string(",", skip, output, output_length);
write_string(json_modifier_, skip, output, output_length);
}
write_string("}", skip, output, output_length);
} else if (type_ == BEGIN_FUNCTION) {
write_string(",\"arguments\":[", output);
*need_comma = false;
} else if (type_ == PROPERTY) {
write_string("}", output);
*need_comma = true;
}
}
@@ -107,7 +89,7 @@ void Endpoint::write_json(size_t id, size_t* skip, uint8_t** output, size_t* out
int StreamToPacketConverter::write_bytes(const uint8_t *buffer, size_t length) {
int StreamToPacketConverter::process_bytes(const uint8_t *buffer, size_t length) {
int result = 0;
while (length--) {
@@ -121,7 +103,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];
packet_length_ = header_buffer_[1] + 2;
}
} else if (packet_index_ < sizeof(packet_buffer_)) {
// Process payload byte
@@ -130,7 +112,9 @@ 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_);
if (calc_crc16(CRC16_INIT, packet_buffer_, packet_length_) == 0) {
result |= output_.process_packet(packet_buffer_, packet_length_ - 2);
}
header_index_ = packet_index_ = packet_length_ = 0;
}
buffer++;
@@ -139,10 +123,12 @@ int StreamToPacketConverter::write_bytes(const uint8_t *buffer, size_t length) {
return result;
}
int PacketToStreamConverter::write_packet(const uint8_t *buffer, size_t length) {
int PacketToStreamConverter::process_packet(const uint8_t *buffer, size_t length) {
// TODO: support buffer size >= 128
if (length >= 128)
return -1;
LOG_PROTO("send header\r\n");
uint8_t header[] = {
SYNC_BYTE,
static_cast<uint8_t>(length),
@@ -150,86 +136,66 @@ int PacketToStreamConverter::write_packet(const uint8_t *buffer, size_t length)
};
header[2] = calc_crc8(CRC8_INIT, header, 2);
if (output_.write_bytes(header, sizeof(header)))
if (output_.process_bytes(header, sizeof(header)))
return -1;
//printf("send payload:\r\n"); osDelay(5); hexdump(buffer, length);
if (output_.write_bytes(buffer, length))
LOG_PROTO("send payload:\r\n");
hexdump(buffer, length);
if (output_.process_bytes(buffer, length))
return -1;
//osDelay(5); printf("sent!\r\n"); osDelay(5);
LOG_PROTO("send crc16\r\n");
uint16_t crc16 = calc_crc16(CRC16_INIT, buffer, length);
uint8_t crc16_buffer[] = {
(uint8_t)((crc16 >> 8) & 0xff),
(uint8_t)((crc16 >> 0) & 0xff)
};
if (output_.process_bytes(crc16_buffer, 2))
return -1;
LOG_PROTO("sent!\r\n");
return 0;
}
// Calculates the CRC16 of the JSON interface descriptor.
// Make sure this stays consistent with what interface_query returns.
// The init value is the protocol version.
//Oskar: consider non-loop version of calc_crc16 in write_buffer, as per slack discussion
uint16_t BidirectionalPacketBasedChannel::calculate_json_crc16(void) {
uint8_t buffer[64];
size_t offset = 0;
bool need_comma = false;
CRC16Calculator crc16_calculator(PROTOCOL_VERSION);
uint8_t *buffer_ptr = buffer;
size_t buffer_length = sizeof(buffer);
write_string("[", &offset, &buffer_ptr, &buffer_length);
uint16_t crc16 = calc_crc16(PROTOCOL_VERSION, buffer, sizeof(buffer) - buffer_length);
uint8_t offset[4] = { 0 };
interface_query(offset, sizeof(offset), &crc16_calculator);
for (size_t i = 0; i < n_endpoints_; ++i) {
buffer_ptr = buffer;
buffer_length = sizeof(buffer);
get_endpoint(i)->write_json(i, &offset, &buffer_ptr, &buffer_length, &need_comma);
crc16 = calc_crc16(crc16, buffer, sizeof(buffer) - buffer_length);
}
buffer_ptr = buffer;
buffer_length = sizeof(buffer);
write_string("]", &offset, &buffer_ptr, &buffer_length);
crc16 = calc_crc16(crc16, buffer, sizeof(buffer) - buffer_length);
return crc16;
return crc16_calculator.get_crc16();
}
// Returns part of the JSON interface definition.
// Make sure this stays consistent with what calculate_json_crc16 calculates.
// The init value is the protocol version.
void BidirectionalPacketBasedChannel::interface_query(const uint8_t* input, size_t input_length, uint8_t* output, size_t* output_length) {
void BidirectionalPacketBasedChannel::interface_query(const uint8_t* input, size_t input_length, StreamSink* output) {
// The request must contain a 32 bit integer to specify an offset
if (input_length < 4)
return;
uint32_t offset32 = 0;
read_le<uint32_t>(&offset32, input);
size_t offset = offset32;
uint32_t offset = 0;
read_le<uint32_t>(&offset, input);
NullStreamSink output_with_offset = NullStreamSink(offset, *output);
bool need_comma = false;
write_string("[", &offset, &output, output_length);
write_string("[", &output_with_offset);
for (size_t i = 0; i < n_endpoints_; ++i) {
get_endpoint(i)->write_json(i, &offset, &output, output_length, &need_comma);
get_endpoint(i)->write_json(i, &need_comma, &output_with_offset);
if (!output->get_free_space())
return; // return early if the output cannot take more bytes
}
write_string("]", &offset, &output, output_length);
write_string("]", &output_with_offset);
}
//Oskar: Can you please make a google sheet which describes the packet layout/format
int BidirectionalPacketBasedChannel::write_packet(const uint8_t* buffer, size_t length) {
//printf("got packet of length %d: \r\n", length); osDelay(5); hexdump(buffer, length);
int BidirectionalPacketBasedChannel::process_packet(const uint8_t* buffer, size_t length) {
LOG_PROTO("got packet of length %d: \r\n", length);
hexdump(buffer, length);
if (length < 4)
return -1;
// calculate CRC for later validation
uint16_t crc16 = calc_crc16(CRC16_INIT, buffer, length - 2);
uint8_t crc16_termination[] = {
(PROTOCOL_VERSION >> 0) & 0xff,
(PROTOCOL_VERSION >> 8) & 0xff,
buffer[length - 2],
buffer[length - 1]
};
uint16_t seq_no = read_le<uint16_t>(&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
@@ -239,46 +205,40 @@ int BidirectionalPacketBasedChannel::write_packet(const uint8_t* buffer, size_t
bool expect_response = endpoint_id & 0x8000;
endpoint_id &= 0x7fff;
Endpoint* endpoint = get_endpoint(endpoint_id);
const Endpoint* endpoint = get_endpoint(endpoint_id);
if (!endpoint)
return -1;
// Verify packet CRC. The expected CRC termination value depends on the selected endpoint.
// Verify packet footer. The expected footer value depends on the selected endpoint.
// For endpoint 0 this is just the protocol version, for all other endpoints it's a
// CRC over the entire JSON descriptor tree (this may change in future versions).
if (endpoint_id) {
crc16_termination[0] = (json_crc_ >> 0) & 0xff;
crc16_termination[1] = (json_crc_ >> 8) & 0xff;
}
if (calc_crc16(crc16, crc16_termination, sizeof(crc16_termination))) {
//printf("crc16 for endpoint %d failed: expected termination %02x %02x\r\n", endpoint_id, crc16_termination[0], crc16_termination[1]); osDelay(5);
uint16_t expected_footer = endpoint_id ? json_crc_ : PROTOCOL_VERSION;
uint16_t actual_footer = buffer[length - 2] | (buffer[length - 1] << 8);
if (expected_footer != actual_footer) {
LOG_PROTO("footer mismatch for endpoint %d: expected %04x, got %04x\r\n", endpoint_id, expected_footer, actual_footer);
return -1;
}
//printf("crc16 ok\r\n"); osDelay(5);
LOG_PROTO("footer ok\r\n");
// TODO: if more bytes than the MTU were requested, should we abort or just return as much as possible?
uint16_t expected_response_length = read_le<uint16_t>(&buffer, &length);
// Let the endpoint do the processing
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 - 2, tx_buf_ + 2, &remaining_size);
// Limit response length according to our local TX buffer size
if (expected_response_length > sizeof(tx_buf_) - 2)
expected_response_length = sizeof(tx_buf_) - 2;
MemoryStreamSink output(tx_buf_ + 2, expected_response_length);
endpoint->handle(buffer, length - 2, &output);
// Send response
if (expect_response) {
size_t tx_size = (requested_size - remaining_size) + 4;
size_t actual_response_length = expected_response_length - output.get_free_space() + 2;
write_le<uint16_t>(seq_no | 0x8000, tx_buf_);
// Add protocol version for CRC calculation (overwritten by actual CRC)
tx_buf_[tx_size - 2] = (PROTOCOL_VERSION >> 0) & 0xff;
tx_buf_[tx_size - 1] = (PROTOCOL_VERSION >> 8) & 0xff;
crc16 = calc_crc16(CRC16_INIT, tx_buf_, tx_size);
// Append CRC in big endian
tx_buf_[tx_size - 2] = (crc16 >> 8) & 0xff;
tx_buf_[tx_size - 1] = (crc16 >> 0) & 0xff;
output_.write_packet(tx_buf_, tx_size);
LOG_PROTO("send packet:\r\n");
hexdump(tx_buf_, actual_response_length);
output_.process_packet(tx_buf_, actual_response_length);
}
}