diff --git a/.travis.yml b/.travis.yml index d530f0a1..7260e431 100644 --- a/.travis.yml +++ b/.travis.yml @@ -39,6 +39,7 @@ env: # Various protocol combinations - CONFIG_BOARD_VERSION=v3.4-24V CONFIG_USB_PROTOCOL=native-stream CONFIG_UART_PROTOCOL=native + - CONFIG_BOARD_VERSION=v3.4-24V CONFIG_USB_PROTOCOL=stdout CONFIG_UART_PROTOCOL=ascii - CONFIG_BOARD_VERSION=v3.4-24V CONFIG_USB_PROTOCOL=none CONFIG_UART_PROTOCOL=none script: diff --git a/Firmware/Board/v3/0001-expose-correct-serial-number-on-USB.patch b/Firmware/Board/v3/0001-expose-correct-serial-number-on-USB.patch index a2e510b0..b9fef648 100644 --- a/Firmware/Board/v3/0001-expose-correct-serial-number-on-USB.patch +++ b/Firmware/Board/v3/0001-expose-correct-serial-number-on-USB.patch @@ -4,14 +4,14 @@ Date: Mon, 12 Mar 2018 23:49:32 -0700 Subject: [PATCH] expose correct serial number on USB --- - Firmware/Board/v3/Src/usbd_desc.c | 15 +++++++++------- - 1 file changed, 8 insertions(+), 7 deletions(-) + Firmware/Board/v3/Src/usbd_desc.c | 9 +++++++++------- + 1 file changed, 1 insertions(+), 8 deletions(-) diff --git a/Firmware/Board/v3/Src/usbd_desc.c b/Firmware/Board/v3/Src/usbd_desc.c index b9c7bd0..94dc49b 100644 --- a/Firmware/Board/v3/Src/usbd_desc.c +++ b/Firmware/Board/v3/Src/usbd_desc.c -@@ -327,14 +327,15 @@ uint8_t * USBD_FS_ManufacturerStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *l +@@ -327,14 +327,7 @@ uint8_t * USBD_FS_ManufacturerStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *l */ uint8_t * USBD_FS_SerialStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) { @@ -22,15 +22,8 @@ index b9c7bd0..94dc49b 100644 - else - { - USBD_GetString((uint8_t *)USBD_SERIALNUMBER_STRING_FS, USBD_StrDesc, length); -+ uint8_t str[13]; // 12 digits + null termination -+ uint64_t val = serial_number; -+ for (size_t i = 0; i < 12; ++i) { -+ str[i] = "0123456789ABCDEF"[(val >> (48-4)) & 0xf]; -+ val <<= 4; - } -+ str[12] = 0; -+ -+ USBD_GetString ((uint8_t *)str, USBD_StrDesc, length); +- } ++ USBD_GetString ((uint8_t *)serial_number_str, USBD_StrDesc, length); return USBD_StrDesc; } diff --git a/Firmware/Board/v3/Src/main.c b/Firmware/Board/v3/Src/main.c index 777a5f20..64810aac 100644 --- a/Firmware/Board/v3/Src/main.c +++ b/Firmware/Board/v3/Src/main.c @@ -125,6 +125,13 @@ int main(void) uint32_t uuid_mixed_part = uuid0 + uuid2; serial_number = ((uint64_t)uuid_mixed_part << 16) | (uint64_t)(uuid1 >> 16); + uint64_t val = serial_number; + for (size_t i = 0; i < 12; ++i) { + serial_number_str[i] = "0123456789ABCDEF"[(val >> (48-4)) & 0xf]; + val <<= 4; + } + serial_number_str[12] = 0; + /* USER CODE END 1 */ /* MCU Configuration----------------------------------------------------------*/ diff --git a/Firmware/Board/v3/Src/syscalls.c b/Firmware/Board/v3/Src/syscalls.c index da7674fc..eff893dc 100644 --- a/Firmware/Board/v3/Src/syscalls.c +++ b/Firmware/Board/v3/Src/syscalls.c @@ -10,7 +10,6 @@ #include #include #include -#include // TODO: make serial_printf_select constant //int _read(int file, char *data, int len) {} @@ -57,46 +56,6 @@ intptr_t _sbrk(size_t size) { return ptr; } -#define UART_TX_BUFFER_SIZE 64 -static uint8_t uart_tx_buf[UART_TX_BUFFER_SIZE]; +// _write is defined in communication.cpp -int _write(int file, char* data, int len) { - //number of bytes written - int written = 0; - switch (serial_printf_select) { - case SERIAL_PRINTF_IS_USB: { - // Wait on semaphore for the interface to be available - // Note that the USB driver will release the interface again when the TX completes - const uint32_t usb_tx_timeout = 100; // ms - osStatus sem_stat = osSemaphoreWait(sem_usb_tx, usb_tx_timeout); - if (sem_stat == osOK) { - uint8_t status = CDC_Transmit_FS((uint8_t*)data, len); // transmit over CDC - written = (status == USBD_OK) ? len : 0; - } // If the semaphore times out, we simply leave "written" as 0 - } break; - case SERIAL_PRINTF_IS_UART: { - //Check length - if (len > UART_TX_BUFFER_SIZE) - return 0; - // Wait on semaphore for the interface to be available - // Note that HAL_UART_TxCpltCallback will release the interface again when the TX completes - const uint32_t uart_tx_timeout = 100; // ms - osStatus sem_stat = osSemaphoreWait(sem_uart_dma, uart_tx_timeout); - if (sem_stat == osOK) { - memcpy(uart_tx_buf, data, len); // memcpy data into uart_tx_buf - HAL_UART_Transmit_DMA(&huart4, uart_tx_buf, len); // Start DMA background transfer - } // If the semaphore times out, we simply leave "written" as 0 - } break; - - default: { - written = 0; - } break; - } - - return written; -} - -void HAL_UART_TxCpltCallback(UART_HandleTypeDef* huart) { - osSemaphoreRelease(sem_uart_dma); -} diff --git a/Firmware/Board/v3/Src/usbd_desc.c b/Firmware/Board/v3/Src/usbd_desc.c index e7083b3a..ea8584d8 100644 --- a/Firmware/Board/v3/Src/usbd_desc.c +++ b/Firmware/Board/v3/Src/usbd_desc.c @@ -329,15 +329,7 @@ uint8_t * USBD_FS_ManufacturerStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *l */ uint8_t * USBD_FS_SerialStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) { - uint8_t str[13]; // 12 digits + null termination - uint64_t val = serial_number; - for (size_t i = 0; i < 12; ++i) { - str[i] = "0123456789ABCDEF"[(val >> (48-4)) & 0xf]; - val <<= 4; - } - str[12] = 0; - - USBD_GetString ((uint8_t *)str, USBD_StrDesc, length); + USBD_GetString ((uint8_t *)serial_number_str, USBD_StrDesc, length); return USBD_StrDesc; } diff --git a/Firmware/MotorControl/ascii_protocol.cpp b/Firmware/MotorControl/ascii_protocol.cpp new file mode 100644 index 00000000..08eecc1d --- /dev/null +++ b/Firmware/MotorControl/ascii_protocol.cpp @@ -0,0 +1,172 @@ +/* +* The ASCII protocol is a simpler, human readable alternative to the main native +* protocol. +* In the future this protocol might be extended to support selected GCode commands. +* For a list of supported commands see doc/ascii-protocol.md +*/ + +/* Includes ------------------------------------------------------------------*/ + +#include "odrive_main.hpp" +#include "communication.h" +#include "ascii_protocol.h" +#include + +/* Private macros ------------------------------------------------------------*/ +/* Private typedef -----------------------------------------------------------*/ +/* Global constant data ------------------------------------------------------*/ +/* Global variables ----------------------------------------------------------*/ +/* Private constant data -----------------------------------------------------*/ + +#define MAX_LINE_LENGTH 64 + +/* Private variables ---------------------------------------------------------*/ +/* Private function prototypes -----------------------------------------------*/ +/* Function implementations --------------------------------------------------*/ + +// @brief Sends a line on the specified output. +template +void respond(StreamSink& output, bool include_checksum, const char * fmt, TArgs&& ... args) { + char response[64]; + size_t len = snprintf(response, sizeof(response), fmt, std::forward(args)...); + output.process_bytes((uint8_t*)response, len); + if (include_checksum) { + uint8_t checksum = 0; + for (size_t i = 0; i < len; ++i) + checksum ^= response[i]; + len = snprintf(response, sizeof(response), "*%u", checksum); + output.process_bytes((uint8_t*)response, len); + } + output.process_bytes((const uint8_t*)"\r\n", 2); +} + + +// @brief Executes an ASCII protocol command +// @param buffer buffer of ASCII encoded characters +// @param len size of the buffer +void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& response_channel) { + static_assert(sizeof(char) == sizeof(uint8_t)); + + // scan line to find beginning of checksum and prune comment + uint8_t checksum = 0; + size_t checksum_start = SIZE_MAX; + for (size_t i = 0; i < len; ++i) { + if (buffer[i] == ';') { // ';' is the comment start char + len = i; + break; + } + if (checksum_start > i) { + if (buffer[i] == '*') { + checksum_start = i + 1; + } else { + checksum ^= buffer[i]; + } + } + } + + // copy everything into a local buffer so we can insert null-termination + char cmd[MAX_LINE_LENGTH + 1]; + if (len > MAX_LINE_LENGTH) len = MAX_LINE_LENGTH; + memcpy(cmd, buffer, len); + + // optional checksum validation + bool use_checksum = (checksum_start < len); + if (use_checksum) { + unsigned int received_checksum; + sscanf((const char *)cmd + checksum_start, "%u", &received_checksum); + if (received_checksum != checksum) + return; + len = checksum_start - 1; // prune checksum and asterisk + } + + cmd[len] = 0; // null-terminate + + // check incoming packet type + if (cmd[0] == 'p') { // position control + unsigned motor_number; + float pos_setpoint, vel_feed_forward, current_feed_forward; + int numscan = sscanf(cmd, "p %u %f %f %f", &motor_number, &pos_setpoint, &vel_feed_forward, ¤t_feed_forward); + if (numscan < 2) { + respond(response_channel, use_checksum, "invalid command format"); + } else if (motor_number >= AXIS_COUNT) { + respond(response_channel, use_checksum, "invalid motor %u", motor_number); + } else { + if (numscan < 3) + vel_feed_forward = 0.0f; + if (numscan < 4) + current_feed_forward = 0.0f; + axes[motor_number]->controller_.set_pos_setpoint(pos_setpoint, vel_feed_forward, current_feed_forward); + } + + } else if (cmd[0] == 'v') { // velocity control + unsigned motor_number; + float vel_setpoint, current_feed_forward; + int numscan = sscanf(cmd, "v %u %f %f", &motor_number, &vel_setpoint, ¤t_feed_forward); + if (numscan < 2) { + respond(response_channel, use_checksum, "invalid command format"); + } else if (motor_number >= AXIS_COUNT) { + respond(response_channel, use_checksum, "invalid motor %u", motor_number); + } else { + if (numscan < 3) + current_feed_forward = 0.0f; + axes[motor_number]->controller_.set_vel_setpoint(vel_setpoint, current_feed_forward); + } + + } else if (cmd[0] == 'c') { // current control + unsigned motor_number; + float current_setpoint; + int numscan = sscanf(cmd, "c %u %f", &motor_number, ¤t_setpoint); + if (numscan < 2) { + respond(response_channel, use_checksum, "invalid command format"); + } else if (motor_number >= AXIS_COUNT) { + respond(response_channel, use_checksum, "invalid motor %u", motor_number); + } else { + axes[motor_number]->controller_.set_current_setpoint(current_setpoint); + respond(response_channel, use_checksum, "ok", motor_number); + } + + } else if (cmd[0] == 'i'){ // Dump device info + respond(response_channel, use_checksum, "Signature: %#x", STM_ID_GetSignature()); + respond(response_channel, use_checksum, "Revision: %#x", STM_ID_GetRevision()); + respond(response_channel, use_checksum, "Flash Size: %#x KiB", STM_ID_GetFlashSize()); + respond(response_channel, use_checksum, "Serial number: %s", serial_number_str); + +// } else if (cmd[0] == 'r') { // read property +// } else if (cmd[0] == 'w') { // write property + + } else if (cmd[0] == 'h') { // HALT + for(size_t i = 0; i < AXIS_COUNT; i++){ + axes[i]->controller_.set_vel_setpoint(0.0f, 0.0f); + } + } else if (cmd[0] != 0) { + respond(response_channel, use_checksum, "unknown command"); + } +} + +void ASCII_protocol_parse_stream(const uint8_t* buffer, size_t len, StreamSink& response_channel) { + static uint8_t parse_buffer[MAX_LINE_LENGTH]; + static bool read_active = true; + static uint32_t parse_buffer_idx = 0; + + while (len--) { + // if the line becomes too long, reset buffer and wait for the next line + if (parse_buffer_idx >= MAX_LINE_LENGTH) { + read_active = false; + parse_buffer_idx = 0; + } + + // Fetch the next char + uint8_t c = *(buffer++); + bool is_end_of_line = (c == '\r' || c == '\n' || c == '!'); + if (is_end_of_line) { + if (read_active) + ASCII_protocol_process_line(parse_buffer, parse_buffer_idx, response_channel); + parse_buffer_idx = 0; + read_active = true; + } else { + if (read_active) { + parse_buffer[parse_buffer_idx++] = c; + } + } + } +} diff --git a/Firmware/MotorControl/legacy_commands.h b/Firmware/MotorControl/ascii_protocol.h similarity index 77% rename from Firmware/MotorControl/legacy_commands.h rename to Firmware/MotorControl/ascii_protocol.h index 11ee7203..680dd9f3 100644 --- a/Firmware/MotorControl/legacy_commands.h +++ b/Firmware/MotorControl/ascii_protocol.h @@ -1,5 +1,9 @@ -#ifndef LEGACY_COMMANDS_H -#define LEGACY_COMMANDS_H +#ifndef ASCII_PROTOCOL_H +#define ASCII_PROTOCOL_H + +#ifndef __ODRIVE_MAIN_HPP +#error "This file should not be included directly. Include odrive_main.hpp instead." +#endif #ifdef __cplusplus extern "C" { @@ -29,11 +33,10 @@ extern uint16_t* exposed_uint16[]; /* Exported functions --------------------------------------------------------*/ /* Exported functions --------------------------------------------------------*/ -void legacy_parse_cmd(const uint8_t* buffer, size_t len, size_t buffer_length, SerialPrintf_t response_interface); -void legacy_parse_stream(const uint8_t* buffer, size_t len); +void ASCII_protocol_parse_stream(const uint8_t* buffer, size_t len, StreamSink& response_channel); #ifdef __cplusplus } #endif -#endif /* LEGACY_COMMANDS_H */ +#endif /* ASCII_PROTOCOL_H */ diff --git a/Firmware/MotorControl/communication.cpp b/Firmware/MotorControl/communication.cpp index ce51ded3..e7e3743a 100644 --- a/Firmware/MotorControl/communication.cpp +++ b/Firmware/MotorControl/communication.cpp @@ -4,7 +4,7 @@ // TODO: remove this option // and once the legacy protocol is phased out, remove the seq-no hack in protocol.py // todo: make clean switches for protocol -#define ENABLE_LEGACY_PROTOCOL +#define ENABLE_ASCII_PROTOCOL #include "communication.h" //#include "low_level.h" @@ -14,8 +14,8 @@ #include "utils.h" #include "../build/version.h" // autogenerated based on Git state -#ifdef ENABLE_LEGACY_PROTOCOL -#include "legacy_commands.h" +#ifdef ENABLE_ASCII_PROTOCOL +#include "ascii_protocol.h" #endif #include @@ -36,6 +36,7 @@ extern PCD_HandleTypeDef hpcd_USB_OTG_FS; extern USBD_HandleTypeDef hUsbDeviceFS; uint64_t serial_number; +char serial_number_str[13]; // 12 digits + null termination /* Private constant data -----------------------------------------------------*/ @@ -69,7 +70,7 @@ static uint32_t usb_len; static thread_local uint32_t deadline_ms = 0; -#if defined(USB_PROTOCOL_NATIVE) +#if !defined(USB_PROTOCOL_NONE) class USBSender : public PacketSink { public: @@ -86,42 +87,44 @@ public: well... it's not actually. Stupid STM. */, length); return (status == USBD_OK) ? 0 : -1; } -} usb_sender; +} usb_packet_output; -BidirectionalPacketBasedChannel usb_channel(usb_sender); - -#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED) - -class USBSender : public StreamSink { +#if !defined(USB_PROTOCOL_NATIVE) +class TreatPacketSinkAsStreamSink : public StreamSink { public: + TreatPacketSinkAsStreamSink(PacketSink& output) : output_(output) {} int process_bytes(const uint8_t* buffer, size_t length) { // Loop to ensure all bytes get sent while (length) { size_t chunk = length < USB_TX_DATA_SIZE ? length : USB_TX_DATA_SIZE; - // wait for USB interface to become ready - if (osSemaphoreWait(sem_usb_tx, deadline_to_timeout(deadline_ms)) != osOK) - return -1; - // transmit chunk - if (CDC_Transmit_FS( - const_cast(buffer) /* casting this const away is safe because... - well... it's not actually. Stupid STM. */, chunk) != USBD_OK) + if (output_.process_packet(buffer, length) != 0) return -1; buffer += chunk; length -= chunk; } return 0; } - size_t get_free_space() { return SIZE_MAX; } -} usb_sender; - -PacketToStreamConverter usb_packet_sender(usb_sender); -BidirectionalPacketBasedChannel usb_channel(endpoints, NUM_ENDPOINTS, usb_packet_sender); -StreamToPacketConverter usb_stream_sink(usb_channel); - +private: + PacketSink& output_; +} usb_stream_output(usb_packet_output); #endif -#if defined(UART_PROTOCOL_NATIVE) +#if defined(USB_PROTOCOL_NATIVE) +BidirectionalPacketBasedChannel usb_channel(usb_packet_output); +#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED) +PacketToStreamConverter usb_packetized_output(usb_stream_output); +BidirectionalPacketBasedChannel usb_channel(usb_packetized_output); +#endif + +#if defined(USB_PROTOCOL_NATIVE_STREAM_BASED) +StreamToPacketConverter usb_native_stream_input(usb_channel); +#endif + +#endif // !defined(USB_PROTOCOL_NONE) + + +#if !defined(UART_PROTOCOL_NONE) class UART4Sender : public StreamSink { public: int process_bytes(const uint8_t* buffer, size_t length) { @@ -145,13 +148,16 @@ public: size_t get_free_space() { return SIZE_MAX; } private: uint8_t tx_buf_[UART_TX_BUFFER_SIZE]; -} uart4_sender; +} uart4_stream_output; -PacketToStreamConverter uart4_packet_sender(uart4_sender); +#if defined(UART_PROTOCOL_NATIVE) +PacketToStreamConverter uart4_packet_sender(uart4_stream_output); BidirectionalPacketBasedChannel uart4_channel(endpoints, NUM_ENDPOINTS, uart4_packet_sender); -StreamToPacketConverter UART4_stream_sink(uart4_channel); +StreamToPacketConverter uart4_stream_input(uart4_channel); #endif +#endif // !defined(UART_PROTOCOL_NONE) + /* Private function prototypes -----------------------------------------------*/ /* Function implementations --------------------------------------------------*/ @@ -265,31 +271,29 @@ void communication_task(void * ctx) { uint32_t new_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR; deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS); + // Process bytes in one or two chunks (two in case there was a wrap) + if (new_rcv_idx < last_rcv_idx) { #if defined(UART_PROTOCOL_NATIVE) - // Process bytes in one or two chunks (two in case there was a wrap) - if (new_rcv_idx < last_rcv_idx) { - UART4_stream_sink.process_bytes(dma_circ_buffer + last_rcv_idx, + uart4_stream_input.process_bytes(dma_circ_buffer + last_rcv_idx, UART_RX_BUFFER_SIZE - last_rcv_idx); - last_rcv_idx = 0; - } - if (new_rcv_idx > last_rcv_idx) { - UART4_stream_sink.process_bytes(dma_circ_buffer + last_rcv_idx, - new_rcv_idx - last_rcv_idx); - last_rcv_idx = new_rcv_idx; - } -#elif defined(UART_PROTOCOL_LEGACY) - // Process bytes in one or two chunks (two in case there was a wrap) - if (new_rcv_idx < last_rcv_idx) { - legacy_parse_stream(dma_circ_buffer + last_rcv_idx, - UART_RX_BUFFER_SIZE - last_rcv_idx); - last_rcv_idx = 0; - } - if (new_rcv_idx > last_rcv_idx) { - legacy_parse_stream(dma_circ_buffer + last_rcv_idx, - new_rcv_idx - last_rcv_idx); - last_rcv_idx = new_rcv_idx; - } #endif +#if defined(UART_PROTOCOL_ASCII) + ASCII_protocol_parse_stream(dma_circ_buffer + last_rcv_idx, + UART_RX_BUFFER_SIZE - last_rcv_idx, uart4_stream_output); +#endif + last_rcv_idx = 0; + } + if (new_rcv_idx > last_rcv_idx) { +#if defined(UART_PROTOCOL_NATIVE) + uart4_stream_input.process_bytes(dma_circ_buffer + last_rcv_idx, + new_rcv_idx - last_rcv_idx); +#endif +#if defined(UART_PROTOCOL_ASCII) + ASCII_protocol_parse_stream(dma_circ_buffer + last_rcv_idx, + new_rcv_idx - last_rcv_idx, uart4_stream_output); +#endif + last_rcv_idx = new_rcv_idx; + } #endif #if !defined(USB_PROTOCOL_NONE) @@ -303,9 +307,9 @@ void communication_task(void * ctx) { #if defined(USB_PROTOCOL_NATIVE) usb_channel.process_packet(usb_buf, usb_len); #elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED) - usb_stream_sink.process_bytes(usb_buf, usb_len); -#elif defined(USB_PROTOCOL_LEGACY) - legacy_parse_cmd(usb_buf, usb_len, USB_RX_DATA_SIZE, SERIAL_PRINTF_IS_USB); + usb_native_stream_input.process_bytes(usb_buf, usb_len); +#elif defined(USB_PROTOCOL_ASCII) + ASCII_protocol_parse_stream(usb_buf, usb_len, usb_stream_output); #endif USBD_CDC_ReceivePacket(&hUsbDeviceFS); // Allow next packet } @@ -343,3 +347,22 @@ void usb_update_thread(void * ctx) { vTaskDelete(osThreadGetId()); } + +extern "C" { +int _write(int file, const char* data, int len); +} + +// @brief This is what printf calls internally +int _write(int file, const char* data, int len) { +#ifdef USB_PROTOCOL_STDOUT + usb_stream_output.process_bytes((const uint8_t *)data, len); +#endif +#ifdef UART_PROTOCOL_STDOUT + uart4_stream_output.process_bytes((const uint8_t *)data, len); +#endif + return len; +} + +void HAL_UART_TxCpltCallback(UART_HandleTypeDef* huart) { + osSemaphoreRelease(sem_uart_dma); +} diff --git a/Firmware/MotorControl/communication.h b/Firmware/MotorControl/communication.h index 2b0f8543..88e37921 100644 --- a/Firmware/MotorControl/communication.h +++ b/Firmware/MotorControl/communication.h @@ -20,6 +20,7 @@ void usb_update_thread(void * ctx); void USB_receive_packet(const uint8_t *buffer, size_t length); extern uint64_t serial_number; +extern char serial_number_str[13]; #ifdef __cplusplus } diff --git a/Firmware/MotorControl/legacy_commands.c b/Firmware/MotorControl/legacy_commands.c deleted file mode 100644 index 29a6c33a..00000000 --- a/Firmware/MotorControl/legacy_commands.c +++ /dev/null @@ -1,292 +0,0 @@ -/* Includes ------------------------------------------------------------------*/ -#include "legacy_commands.h" -#include - -/* Private macros ------------------------------------------------------------*/ -/* Private typedef -----------------------------------------------------------*/ -/* Global constant data ------------------------------------------------------*/ -/* Global variables ----------------------------------------------------------*/ -// This automatically updates to the interface that most -// recently recieved a command. In the future we may want to separate -// debug printf and the main serial comms. -SerialPrintf_t serial_printf_select = SERIAL_PRINTF_IS_UART; -#if 0 -/* Private constant data -----------------------------------------------------*/ - -// variables exposed to usb/serial interface via set/get/monitor -// Note: this will be depricated soon -float* exposed_floats[] = { - &vbus_voltage, // ro - NULL, //&elec_rad_per_enc, // ro - &motors[0].pos_setpoint, // rw - &motors[0].pos_gain, // rw - &motors[0].vel_setpoint, // rw - &motors[0].vel_gain, // rw - &motors[0].vel_integrator_gain, // rw - &motors[0].vel_integrator_current, // rw - &motors[0].vel_limit, // rw - &motors[0].current_setpoint, // rw - &motors[0].calibration_current, // rw - &motors[0].phase_inductance, // ro - &motors[0].phase_resistance, // ro - &motors[0].current_meas.phB, // ro - &motors[0].current_meas.phC, // ro - &motors[0].DC_calib.phB, // rw - &motors[0].DC_calib.phC, // rw - &motors[0].shunt_conductance, // rw - &motors[0].phase_current_rev_gain, // rw - &motors[0].current_control.current_lim, // rw - &motors[0].current_control.p_gain, // rw - &motors[0].current_control.i_gain, // rw - &motors[0].current_control.v_current_control_integral_d, // rw - &motors[0].current_control.v_current_control_integral_q, // rw - &motors[0].current_control.Ibus, // ro - &motors[0].encoder.phase, // ro - &motors[0].encoder.pll_pos, // rw - &motors[0].encoder.pll_vel, // rw - &motors[0].encoder.pll_kp, // rw - &motors[0].encoder.pll_ki, // rw - &motors[1].pos_setpoint, // rw - &motors[1].pos_gain, // rw - &motors[1].vel_setpoint, // rw - &motors[1].vel_gain, // rw - &motors[1].vel_integrator_gain, // rw - &motors[1].vel_integrator_current, // rw - &motors[1].vel_limit, // rw - &motors[1].current_setpoint, // rw - &motors[1].calibration_current, // rw - &motors[1].phase_inductance, // ro - &motors[1].phase_resistance, // ro - &motors[1].current_meas.phB, // ro - &motors[1].current_meas.phC, // ro - &motors[1].DC_calib.phB, // rw - &motors[1].DC_calib.phC, // rw - &motors[1].shunt_conductance, // rw - &motors[1].phase_current_rev_gain, // rw - &motors[1].current_control.current_lim, // rw - &motors[1].current_control.p_gain, // rw - &motors[1].current_control.i_gain, // rw - &motors[1].current_control.v_current_control_integral_d, // rw - &motors[1].current_control.v_current_control_integral_q, // rw - &motors[1].current_control.Ibus, // ro - &motors[1].encoder.phase, // ro - &motors[1].encoder.pll_pos, // rw - &motors[1].encoder.pll_vel, // rw - &motors[1].encoder.pll_kp, // rw - &motors[1].encoder.pll_ki, // rw -}; - -int* exposed_ints[] = { - (int*)&motors[0].control_mode, // rw - (int*)&motors[0].encoder.encoder_offset, // rw - (int*)&motors[0].encoder.encoder_state, // ro - (int*)&motors[0].error, // rw - (int*)&motors[1].control_mode, // rw - (int*)&motors[1].encoder.encoder_offset, // rw - (int*)&motors[1].encoder.encoder_state, // ro - (int*)&motors[1].error, // rw -}; - -bool* exposed_bools[] = { - &motors[0].thread_id_valid, // ro - //For now these are written by Axis::SetupLegacyMappings - &axis[0].enable_control, // rw - &axis[0].do_calibration, // rw - NULL, // &motors[0].calibration_ok, // ro - &motors[1].thread_id_valid, // ro - &axis[1].enable_control, // rw - &axis[1].do_calibration, // rw - NULL, // &motors[1].calibration_ok, // ro -}; - -uint16_t* exposed_uint16[] = { - &motors[0].control_deadline, // rw - &motors[0].last_cpu_time, // ro - &motors[1].control_deadline, // rw - &motors[1].last_cpu_time, // ro -}; - -/* Private variables ---------------------------------------------------------*/ -monitoring_slot monitoring_slots[20] = {0}; -/* Private function prototypes -----------------------------------------------*/ -static void print_monitoring(int limit); - -/* Function implementations --------------------------------------------------*/ - -void legacy_parse_cmd(const uint8_t* buffer, size_t len, size_t buffer_capacity, SerialPrintf_t response_interface) { - // Set response interface - serial_printf_select = response_interface; - - // Cast away const and write beyond the array bounds. Because we can. - // (TODO: yeah maybe not, but this should be gone once we disable legacy commands) - ((uint8_t *)buffer)[len < buffer_capacity ? len : (buffer_capacity - 1)] = 0; - - // check incoming packet type - if (buffer[0] == 'p') { - // position control - unsigned motor_number; - float pos_setpoint, vel_feed_forward, current_feed_forward; - int numscan = sscanf((const char*)buffer, "p %u %f %f %f", &motor_number, &pos_setpoint, &vel_feed_forward, ¤t_feed_forward); - if (numscan == 4 && motor_number < num_motors) { - set_pos_setpoint(&motors[motor_number], pos_setpoint, vel_feed_forward, current_feed_forward); - } - } else if (buffer[0] == 'v') { - // velocity control - unsigned motor_number; - float vel_feed_forward, current_feed_forward; - int numscan = sscanf((const char*)buffer, "v %u %f %f", &motor_number, &vel_feed_forward, ¤t_feed_forward); - if (numscan == 3 && motor_number < num_motors) { - set_vel_setpoint(&motors[motor_number], vel_feed_forward, current_feed_forward); - } - } else if (buffer[0] == 'c') { - // current control - unsigned motor_number; - float current_feed_forward; - int numscan = sscanf((const char*)buffer, "c %u %f", &motor_number, ¤t_feed_forward); - if (numscan == 2 && motor_number < num_motors) { - set_current_setpoint(&motors[motor_number], current_feed_forward); - } - } else if(buffer[0] == 'i'){ // Dump device info - // Retrieves the device signature, revision, flash size, and UUID - printf("Signature: %#x\n", STM_ID_GetSignature()); - printf("Revision: %#x\n", STM_ID_GetRevision()); - printf("Flash Size: %#x KiB\n", STM_ID_GetFlashSize()); - printf("UUID: 0x%lx%lx%lx\n", STM_ID_GetUUID(2), STM_ID_GetUUID(1), STM_ID_GetUUID(0)); - } else if (buffer[0] == 'g') { // GET - // g <0:float,1:int,2:bool,3:uint16> index - int type = 0; - int index = 0; - int numscan = sscanf((const char*)buffer, "g %u %u", &type, &index); - if (numscan == 2) { - switch(type){ - case 0: { - printf("%f\n",*exposed_floats[index]); - break; - }; - case 1: { - printf("%d\n",*exposed_ints[index]); - break; - }; - case 2: { - printf("%d\n",*exposed_bools[index]); - break; - }; - case 3: { - printf("%hu\n",*exposed_uint16[index]); - break; - }; - } - } - } else if (buffer[0] == 'h'){ // HALT - for(int i = 0; i < num_motors; i++){ - set_vel_setpoint(&motors[i], 0.0f, 0.0f); - } - } else if (buffer[0] == 's') { // SET - // s <0:float,1:int,2:bool,3:uint16> index value - int type = 0; - int index = 0; - int numscan = sscanf((const char*)buffer, "s %u %u", &type, &index); - if (numscan == 2) { - switch(type) { - case 0: { - sscanf((const char*)buffer, "s %u %u %f", &type, &index, exposed_floats[index]); - break; - }; - case 1: { - sscanf((const char*)buffer, "s %u %u %d", &type, &index, exposed_ints[index]); - break; - }; - case 2: { - int btmp = 0; - sscanf((const char*)buffer, "s %u %u %d", &type, &index, &btmp); - *exposed_bools[index] = btmp ? true : false; - break; - }; - case 3: { - sscanf((const char*)buffer, "s %u %u %hu", &type, &index, exposed_uint16[index]); - break; - }; - } - } - } else if (buffer[0] == 'm') { // Setup Monitor - // m <0:float,1:int,2:bool,3:uint16> index monitoring_slot - int type = 0; - int index = 0; - int slot = 0; - int numscan = sscanf((const char*)buffer, "m %u %u %u", &type, &index, &slot); - if (numscan == 3) { - monitoring_slots[slot].type = type; - monitoring_slots[slot].index = index; - } - } else if (buffer[0] == 'o') { // Output Monitor - int limit = 0; - int numscan = sscanf((const char*)buffer, "o %u", &limit); - if (numscan == 1) { - print_monitoring(limit); - } - } else if (buffer[0] == 't') { // Run Anti-Cogging Calibration - for (int i = 0; i < num_motors; i++) { - // Ensure the cogging map was correctly allocated earlier and that the motor is capable of calibrating - if (motors[i].anticogging.cogging_map != NULL && motors[i].error == ERROR_NO_ERROR) { - motors[i].anticogging.calib_anticogging = true; - } - } - } -} - -void legacy_parse_stream(const uint8_t* buffer, size_t len) { - #define PARSE_BUFFER_SIZE 64 - static uint8_t parse_buffer[PARSE_BUFFER_SIZE]; - static bool read_active = false; - static uint32_t parse_buffer_idx = 0; - - while (len--) { - // Fetch the next char - uint8_t c = *(buffer++); - // Look for start character - if (c == '$') { - read_active = true; - continue; // do not record start char - } - // Record into parse buffer when actively reading - if (read_active) { - parse_buffer[parse_buffer_idx++] = c; - if (c == '\r' || c == '\n' || c == '!') { - // End of command string - legacy_parse_cmd(parse_buffer, parse_buffer_idx, PARSE_BUFFER_SIZE, SERIAL_PRINTF_IS_UART); - // Reset receieve state machine - read_active = false; - parse_buffer_idx = 0; - } else if (parse_buffer_idx == PARSE_BUFFER_SIZE - 1) { - // We are not at end of command, and receiving another character after this - // would go into the last slot, which is reserved for terminating null. - // We have effectively overflowed parse buffer: abort. - read_active = false; - parse_buffer_idx = 0; - } - } - } -} - -static void print_monitoring(int limit) { - for (int i=0;i ConfigFormat; +typedef Config< + BoardConfig_t, + EncoderConfig_t[AXIS_COUNT], + ControllerConfig_t[AXIS_COUNT], + MotorConfig_t[AXIS_COUNT], + AxisConfig_t[AXIS_COUNT]> ConfigFormat; void save_configuration(void) { if (ConfigFormat::safe_store_config( - &board_config, - &axis_configs, - &motor_configs)) { + &board_config, + &encoder_configs, + &controller_configs, + &motor_configs, + &axis_configs)) { //printf("saving configuration failed\r\n"); osDelay(5); } } @@ -26,13 +33,17 @@ void load_configuration(void) { if (NVM_init() || ConfigFormat::safe_load_config( &board_config, - &axis_configs, - &motor_configs)) { - for (size_t i = 0; i < AXIS_COUNT; ++i) { - axis_configs[i] = AxisConfig_t(); - motor_configs[i] = MotorConfig_t(); - } + &encoder_configs, + &controller_configs, + &motor_configs, + &axis_configs)) { board_config = BoardConfig_t(); + for (size_t i = 0; i < AXIS_COUNT; ++i) { + encoder_configs[i] = EncoderConfig_t(); + controller_configs[i] = ControllerConfig_t(); + motor_configs[i] = MotorConfig_t(); + axis_configs[i] = AxisConfig_t(); + } } } diff --git a/Firmware/README.md b/Firmware/README.md index dbc7c87b..079fee21 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -171,7 +171,7 @@ pip install pyusb pyserial [See ODrive Arduino Library](https://github.com/madcowswe/ODriveArduino) ### Other platforms -See the [protocol specification](protocol.md) or the [legacy protocol specification](legacy-protocol.md). +See the [protocol specification](protocol.md) or the [ASCII protocol specification](ascii-protocol.md).

## Configuring parameters diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua index 4fa262af..d1eb6ee7 100644 --- a/Firmware/Tupfile.lua +++ b/Firmware/Tupfile.lua @@ -36,7 +36,9 @@ if tup.getconfig("USB_PROTOCOL") == "native" or tup.getconfig("USB_PROTOCOL") == elseif tup.getconfig("USB_PROTOCOL") == "native-stream" then FLAGS += "-DUSB_PROTOCOL_NATIVE_STREAM_BASED" elseif tup.getconfig("USB_PROTOCOL") == "ascii" then - FLAGS += "-DUSB_PROTOCOL_LEGACY" + FLAGS += "-DUSB_PROTOCOL_ASCII" +elseif tup.getconfig("USB_PROTOCOL") == "stdout" then + FLAGS += "-DUSB_PROTOCOL_STDOUT" elseif tup.getconfig("USB_PROTOCOL") == "none" then FLAGS += "-DUSB_PROTOCOL_NONE" else @@ -47,7 +49,9 @@ end if tup.getconfig("UART_PROTOCOL") == "native" then FLAGS += "-DUART_PROTOCOL_NATIVE" elseif tup.getconfig("UART_PROTOCOL") == "ascii" or tup.getconfig("UART_PROTOCOL") == "" then - FLAGS += "-DUART_PROTOCOL_LEGACY" + FLAGS += "-DUART_PROTOCOL_ASCII" +elseif tup.getconfig("UART_PROTOCOL") == "stdout" then + FLAGS += "-DUART_PROTOCOL_STDOUT" elseif tup.getconfig("UART_PROTOCOL") == "none" then FLAGS += "-DUART_PROTOCOL_NONE" else @@ -138,7 +142,7 @@ build{ sources={ 'Drivers/DRV8301/drv8301.c', 'MotorControl/utils.c', - 'MotorControl/legacy_commands.c', + 'MotorControl/ascii_protocol.cpp', 'MotorControl/low_level.cpp', 'MotorControl/nvm.c', 'MotorControl/axis.cpp', diff --git a/Firmware/ascii-protocol.md b/Firmware/ascii-protocol.md new file mode 100644 index 00000000..f7d89235 --- /dev/null +++ b/Firmware/ascii-protocol.md @@ -0,0 +1,52 @@ + +## How to send commands + + * **Via USB:** + * **Windows:** Use the Zadig utility to set the ODrive's driver to "usbser". Windows will then make the device available as COM port. You can use [PuTTY](https://www.chiark.greenend.org.uk/~sgtatham/putty/) to manually send commands or open the COM port using your favorite programming language + * **Linux/macOS:** Run `/dev/tty*` to list all serial ports. The ODrive will show up as `/dev/ttyACM0` on Linux and `/dev/tty.usbmodem[...]` on macOS. Once you know the name, you can use `screen /dev/ttyACM0` (with the correct name) to send commands manually or open the device using your favorite programming language. Serial ports on Unix can be opened, written to and read from like a normal file. + * **Via UART:** Connect the ODrive's TX (GPIO1) to your host's RX. Connect your ODrive's RX (GPIO2) to your host's TX. The logic level of the ODrive is 3.3V. + * **Arduino:** You can use the [ODrive Arduino library](https://github.com/madcowswe/ODriveArduino) to talk to the ODrive. + * **Windows/Linux/macOS:** You can use an FTDI USB-UART cable to connect to the ODrive. + + +## Command Reference + +#### Motor Position command +``` +p motor position velocity_ff current_ff +``` +* `p` for position +* `motor` is the motor number, `0` or `1`. +* `position` is the desired position, in encoder counts. +* `velocity_ff` is the velocity feed-forward term, in counts/s (optional). +* `current_ff` is the current feed-forward term, in A (optional). + +Example: `p 0 -20000 0 0` + +Note that if you don't know what feed-forward is or what it's used for, simply omit it. + + +#### Motor Velocity command +``` +v motor velocity current_ff +``` +* `v` for velocity +* `motor` is the motor number, `0` or `1`. +* `velocity` is the desired velocity in counts/s. +* `current_ff` is the current feed-forward term, in A (optional). + +Example: `v 0 1000 0` + +Note that if you don't know what feed-forward is or what it's used for, simply omit it. + +#### Motor Current command +``` +c motor current +``` +* `c` for current +* `motor` is the motor number, `0` or `1`. +* `current` is the desired current in A. + +#### Parameter reading/writing + +This is currently not supported. Use the native protocol. diff --git a/Firmware/legacy-protocol.md b/Firmware/legacy-protocol.md deleted file mode 100644 index 2898432f..00000000 --- a/Firmware/legacy-protocol.md +++ /dev/null @@ -1,68 +0,0 @@ - -Warning: this protocol has [been replaced](https://github.com/madcowswe/ODrive/blob/devel/Firmware/protocol.md). -It's still operational but for new applications it's recommended to use the new protocol. - -### Command set -The most accurate way to understand the commands is to read [the code](MotorControl/commands.c) that parses the commands. Also you can have a look at the [ODrive Arduino library](https://github.com/madcowswe/ODriveArduino) that makes it easy to use the UART interface on Arduino. You can also look at it as an implementation example of how to talk to the ODrive over UART. - -#### UART framing -USB communicates with packets, so it is easy to frame a command as one command per packet. However, UART doesn't have any packeting, so we need a way to frame the commands. The start-of-packet symbol is `$` and the end-of-packet symbol is `!`, that is, something like this: `$command!`. An example of a valid UART position command: -``` -$p 0 10000 0 0! -``` - -#### Motor Position command -``` -p motor position velocity_ff current_ff -``` -* `p` for position -* `motor` is the motor number, `0` or `1`. -* `position` is the desired position, in encoder counts. -* `velocity_ff` is the velocity feed-forward term, in counts/s. -* `current_ff` is the current feed-forward term, in A. - -Note that if you don't know what feed-forward is or what it's used for, simply set it to 0. - -#### Motor Velocity command -``` -v motor velocity current_ff -``` -* `v` for velocity -* `motor` is the motor number, `0` or `1`. -* `velocity` is the desired velocity in counts/s. -* `current_ff` is the current feed-forward term, in A. - -Note that if you don't know what feed-forward is or what it's used for, simply set it to 0. - -#### Motor Current command -``` -c motor current -``` -* `c` for current -* `motor` is the motor number, `0` or `1`. -* `current` is the desired current in A. - -#### Variable getting and setting -``` -g type index -s type index value -``` -* `g` for get, `s` for set -* `type` is the data type as follows: -** `0` is float -** `1` is int -** `2` is bool -* `index` is the index in the corresponding [exposed variable table](MotorControl/legacy_commands.c). - -For example -* `g 0 12` will return the phase resistance of M0 -* `s 0 8 10000.0` will set the velocity limit on M0 to 10000 counts/s -* `g 1 3` will return the error status of M0 -* `g 1 7` will return the error status of M1 - -The error status corresponds to the [Error_t enum in low_level.h](MotorControl/low_level.h). - -Note that the links in this section are to a specific commits to make sure that the line numbers are accurate. That is, they don't link to the newest master, but to an old version. Please check the corresponding lines in the code you are using. This is especially important to get the correct indicies in the exposed variable tables, and the error enum values. - -#### Continous monitoring of variables -You can set up variables in monitoring slots, and then have them (or a subset of them) repeatedly printed upon request. Please see the code for this. diff --git a/tools/odrive/protocol.py b/tools/odrive/protocol.py index 413fca58..f4bf2b05 100644 --- a/tools/odrive/protocol.py +++ b/tools/odrive/protocol.py @@ -268,7 +268,7 @@ class Channel(PacketSink): seq_no = self._outbound_seq_no finally: self._my_lock.release() - seq_no |= 0x80 # FIXME: we hardwire one bit of the seq-no to 1 to avoid conflicts with the legacy protocol + seq_no |= 0x80 # FIXME: we hardwire one bit of the seq-no to 1 to avoid conflicts with the ascii protocol packet = struct.pack('