diff --git a/Firmware/communication/ascii_protocol.cpp b/Firmware/communication/ascii_protocol.cpp index 1cb5f794..a87c7388 100644 --- a/Firmware/communication/ascii_protocol.cpp +++ b/Firmware/communication/ascii_protocol.cpp @@ -11,7 +11,7 @@ #include "../build/version.h" // autogenerated based on Git state #include "communication.h" #include "ascii_protocol.hpp" -#include +#include #include /* Private macros ------------------------------------------------------------*/ @@ -26,20 +26,36 @@ /* Private variables ---------------------------------------------------------*/ /* Private function prototypes -----------------------------------------------*/ + +void setPosition(char * pStr, StreamSink& response_channel, bool use_checksum); +void setPositionWL(char * pStr, StreamSink& response_channel, bool use_checksum); +void setVelocity(char * pStr, StreamSink& response_channel, bool use_checksum); +void setCurrent(char * pStr, StreamSink& response_channel, bool use_checksum); +void setTrapezoidTrajectory(char * pStr, StreamSink& response_channel, bool use_checksum); +void getFeedback(char * pStr, StreamSink& response_channel, bool use_checksum); +void help(char * pStr, StreamSink& response_channel, bool use_checksum); +void infoDump(char * pStr, StreamSink& response_channel, bool use_checksum); +void systemCTRL(char * pStr, StreamSink& response_channel, bool use_checksum); +void readProperty(char * pStr, StreamSink& response_channel, bool use_checksum); +void writeProperty(char * pStr, StreamSink& response_channel, bool use_checksum); +void updateAxisWDG(char * pStr, StreamSink& response_channel, bool use_checksum); +void unknownCMD(char * pStr, StreamSink& response_channel, bool use_checksum); + /* 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, nullptr); // TODO: use process_all instead - if (include_checksum) { + 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, nullptr); + output.process_bytes((uint8_t*)response, snprintf(response, sizeof(response), "*%u", checksum), nullptr); } output.process_bytes((const uint8_t*)"\r\n", 2, nullptr); } @@ -59,7 +75,8 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& len = i; break; } - if (checksum_start > i) { + if (checksum_start > i) + { if (buffer[i] == '*') { checksum_start = i + 1; } else { @@ -72,200 +89,306 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& char cmd[MAX_LINE_LENGTH + 1]; if (len > MAX_LINE_LENGTH) len = MAX_LINE_LENGTH; memcpy(cmd, buffer, len); + cmd[len] = 0; // null-terminate // 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) + int numscan = sscanf(&cmd[checksum_start], "%u", &received_checksum); + if ((numscan < 1) || (received_checksum != checksum)) return; len = checksum_start - 1; // prune checksum and asterisk + cmd[len] = 0; // null-terminate } - 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; - Axis* axis = axes[motor_number]; - axis->controller_.set_pos_setpoint(pos_setpoint, vel_feed_forward, current_feed_forward); - axis->watchdog_feed(); - } - - } else if (cmd[0] == 'q') { // position control with limits - unsigned motor_number; - float pos_setpoint, vel_limit, current_lim; - int numscan = sscanf(cmd, "q %u %f %f %f", &motor_number, &pos_setpoint, &vel_limit, ¤t_lim); - 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 { - Axis* axis = axes[motor_number]; - axis->controller_.pos_setpoint_ = pos_setpoint; - if (numscan >= 3) - axis->controller_.config_.vel_limit = vel_limit; - if (numscan >= 4) - axis->motor_.config_.current_lim = current_lim; - - axis->watchdog_feed(); - } - - } 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; - Axis* axis = axes[motor_number]; - axis->controller_.set_vel_setpoint(vel_setpoint, current_feed_forward); - axis->watchdog_feed(); - } - - } 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 { - Axis* axis = axes[motor_number]; - axis->controller_.set_current_setpoint(current_setpoint); - axis->watchdog_feed(); - } - - } else if (cmd[0] == 't') { // trapezoidal trajectory - unsigned motor_number; - float goal_point; - int numscan = sscanf(cmd, "t %u %f", &motor_number, &goal_point); - 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 { - Axis* axis = axes[motor_number]; - axis->controller_.move_to_pos(goal_point); - axis->watchdog_feed(); - } - - } else if (cmd[0] == 'f') { // feedback - unsigned motor_number; - int numscan = sscanf(cmd, "f %u", &motor_number); - if (numscan < 1) { - 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 { - respond(response_channel, use_checksum, "%f %f", - (double)axes[motor_number]->encoder_.pos_estimate_, - (double)axes[motor_number]->encoder_.vel_estimate_); - } - - } else if (cmd[0] == 'h') { // Help - respond(response_channel, use_checksum, "Please see documentation for more details"); - respond(response_channel, use_checksum, ""); - respond(response_channel, use_checksum, "Available commands syntax reference:"); - respond(response_channel, use_checksum, "Position: q axis pos vel-lim I-lim"); - respond(response_channel, use_checksum, "Position: p axis pos vel-ff I-ff"); - respond(response_channel, use_checksum, "Velocity: v axis vel I-ff"); - respond(response_channel, use_checksum, "Current: c axis I"); - respond(response_channel, use_checksum, ""); - respond(response_channel, use_checksum, "Properties start at odrive root, such as axis0.requested_state"); - respond(response_channel, use_checksum, "Read: r property"); - respond(response_channel, use_checksum, "Write: w property value"); - respond(response_channel, use_checksum, ""); - respond(response_channel, use_checksum, "Save config: ss"); - respond(response_channel, use_checksum, "Erase config: se"); - respond(response_channel, use_checksum, "Reboot: sr"); - - } 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, "Hardware version: %d.%d-%dV", HW_VERSION_MAJOR, HW_VERSION_MINOR, HW_VERSION_VOLTAGE); - respond(response_channel, use_checksum, "Firmware version: %d.%d.%d", FW_VERSION_MAJOR, FW_VERSION_MINOR, FW_VERSION_REVISION); - respond(response_channel, use_checksum, "Serial number: %s", serial_number_str); - - } else if (cmd[0] == 's'){ // System - if(cmd[1] == 's') { // Save config - save_configuration(); - } else if (cmd[1] == 'e'){ // Erase config - erase_configuration(); - } else if (cmd[1] == 'r'){ // Reboot - NVIC_SystemReset(); - } - - } else if (cmd[0] == 'r') { // read property - char name[MAX_LINE_LENGTH]; - int numscan = sscanf(cmd, "r %" TO_STR(MAX_LINE_LENGTH) "s", name); - if (numscan < 1) { - respond(response_channel, use_checksum, "invalid command format"); - } else { - Endpoint* endpoint = application_endpoints_->get_by_name(name, sizeof(name)); - if (!endpoint) { - respond(response_channel, use_checksum, "invalid property"); - } else { - char response[10]; - bool success = endpoint->get_string(response, sizeof(response)); - if (!success) - respond(response_channel, use_checksum, "not implemented"); - else - respond(response_channel, use_checksum, response); - } - } - - } else if (cmd[0] == 'w') { // write property - char name[MAX_LINE_LENGTH]; - char value[MAX_LINE_LENGTH]; - int numscan = sscanf(cmd, "w %" TO_STR(MAX_LINE_LENGTH) "s %" TO_STR(MAX_LINE_LENGTH) "s", name, value); - if (numscan < 1) { - respond(response_channel, use_checksum, "invalid command format"); - } else { - Endpoint* endpoint = application_endpoints_->get_by_name(name, sizeof(name)); - if (!endpoint) { - respond(response_channel, use_checksum, "invalid property"); - } else { - bool success = endpoint->set_string(value, sizeof(value)); - if (!success) - respond(response_channel, use_checksum, "not implemented"); - } - } - - }else if (cmd[0] == 'u') { // Update axis watchdog. - unsigned motor_number; - int numscan = sscanf(cmd, "u %u", &motor_number); - if(numscan < 1){ - 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]->watchdog_feed(); - } - - } else if (cmd[0] != 0) { - respond(response_channel, use_checksum, "unknown command"); + switch(cmd[0]) + { + case 'p': setPosition(cmd, response_channel, use_checksum); break; // position control + case 'q': setPositionWL(cmd, response_channel, use_checksum); break; // position control with limits + case 'v': setVelocity(cmd, response_channel, use_checksum); break; // velocity control + case 'c': setCurrent(cmd, response_channel, use_checksum); break; // current control + case 't': setTrapezoidTrajectory(cmd, response_channel, use_checksum); break; // trapezoidal trajectory + case 'f': getFeedback(cmd, response_channel, use_checksum); break; // feedback + case 'h': help(cmd, response_channel, use_checksum); break; // Help + case 'i': infoDump(cmd, response_channel, use_checksum); break; // Dump device info + case 's': systemCTRL(cmd, response_channel, use_checksum); break; // System + case 'r': readProperty(cmd, response_channel, use_checksum); break; // read property + case 'w': writeProperty(cmd, response_channel, use_checksum); break; // write property + case 'u': updateAxisWDG(cmd, response_channel, use_checksum); break; // Update axis watchdog. + default : unknownCMD(nullptr, response_channel, use_checksum); break; } } +// @brief Executes the set position command +// @param pStr buffer of ASCII encoded values +// @param response_channel reference to the stream to respond on +// @param use_checksum bool to indicate whether a checksum is required on response +void setPosition(char * pStr, StreamSink& response_channel, bool use_checksum) +{ + unsigned motor_number; + float pos_setpoint, vel_feed_forward, current_feed_forward; + + int numscan = sscanf(pStr, "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 { + Axis* axis = axes[motor_number]; + axis->controller_.config_.control_mode = Controller::CTRL_MODE_POSITION_CONTROL; + axis->controller_.input_pos_ = pos_setpoint; + if (numscan >= 3) + axis->controller_.input_vel_ = vel_feed_forward; + if (numscan >= 4) + axis->controller_.input_current_ = current_feed_forward; + axis->controller_.input_pos_updated(); + axis->watchdog_feed(); + } +} + +// @brief Executes the set position with current and velocity limit command +// @param pStr buffer of ASCII encoded values +// @param response_channel reference to the stream to respond on +// @param use_checksum bool to indicate whether a checksum is required on response +void setPositionWL(char * pStr, StreamSink& response_channel, bool use_checksum) +{ + unsigned motor_number; + float pos_setpoint, vel_limit, current_lim; + + int numscan = sscanf(pStr, "q %u %f %f %f", &motor_number, &pos_setpoint, &vel_limit, ¤t_lim); + 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 { + Axis* axis = axes[motor_number]; + axis->controller_.config_.control_mode = Controller::CTRL_MODE_POSITION_CONTROL; + axis->controller_.input_pos_ = pos_setpoint; + if (numscan >= 3) + axis->controller_.config_.vel_limit = vel_limit; + if (numscan >= 4) + axis->motor_.config_.current_lim = current_lim; + axis->controller_.input_pos_updated(); + axis->watchdog_feed(); + } +} + +// @brief Executes the set velocity command +// @param pStr buffer of ASCII encoded values +// @param response_channel reference to the stream to respond on +// @param use_checksum bool to indicate whether a checksum is required on response +void setVelocity(char * pStr, StreamSink& response_channel, bool use_checksum) +{ + unsigned motor_number; + float vel_setpoint, current_feed_forward; + int numscan = sscanf(pStr, "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 { + Axis* axis = axes[motor_number]; + axis->controller_.config_.control_mode = Controller::CTRL_MODE_VELOCITY_CONTROL; + axis->controller_.input_vel_ = vel_setpoint; + if (numscan >= 3) + axis->controller_.input_current_ = current_feed_forward; + axis->watchdog_feed(); + } +} + +// @brief Executes the set current limit command +// @param pStr buffer of ASCII encoded values +// @param response_channel reference to the stream to respond on +// @param use_checksum bool to indicate whether a checksum is required on response +void setCurrent(char * pStr, StreamSink& response_channel, bool use_checksum) +{ + unsigned motor_number; + float current_setpoint; + + if (sscanf(pStr, "c %u %f", &motor_number, ¤t_setpoint) < 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 { + Axis* axis = axes[motor_number]; + axis->controller_.config_.control_mode = Controller::CTRL_MODE_CURRENT_CONTROL; + axis->controller_.input_current_ = current_setpoint; + axis->watchdog_feed(); + } +} + +// @brief Executes the set trapezoid trajectory command +// @param pStr buffer of ASCII encoded values +// @param response_channel reference to the stream to respond on +// @param use_checksum bool to indicate whether a checksum is required on response +void setTrapezoidTrajectory(char * pStr, StreamSink& response_channel, bool use_checksum) +{ + unsigned motor_number; + float goal_point; + + if (sscanf(pStr, "t %u %f", &motor_number, &goal_point) < 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 { + Axis* axis = axes[motor_number]; + axis->controller_.config_.input_mode = Controller::INPUT_MODE_TRAP_TRAJ; + axis->controller_.move_to_pos(goal_point); + axis->watchdog_feed(); + } +} + +// @brief Executes the get position and velocity feedback command +// @param pStr buffer of ASCII encoded values +// @param response_channel reference to the stream to respond on +// @param use_checksum bool to indicate whether a checksum is required on response +void getFeedback(char * pStr, StreamSink& response_channel, bool use_checksum) +{ + unsigned motor_number; + + if (sscanf(pStr, "f %u", &motor_number) < 1) { + 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 { + Axis* axis = axes[motor_number]; + respond(response_channel, use_checksum, "%f %f", + (double)axis->encoder_.pos_estimate_, + (double)axis->encoder_.vel_estimate_); + } +} + +// @brief Shows help text +// @param pStr buffer of ASCII encoded values +// @param response_channel reference to the stream to respond on +// @param use_checksum bool to indicate whether a checksum is required on response +void help(char * pStr, StreamSink& response_channel, bool use_checksum) +{ + (void)pStr; + respond(response_channel, use_checksum, "Please see documentation for more details"); + respond(response_channel, use_checksum, ""); + respond(response_channel, use_checksum, "Available commands syntax reference:"); + respond(response_channel, use_checksum, "Position: q axis pos vel-lim I-lim"); + respond(response_channel, use_checksum, "Position: p axis pos vel-ff I-ff"); + respond(response_channel, use_checksum, "Velocity: v axis vel I-ff"); + respond(response_channel, use_checksum, "Current: c axis I"); + respond(response_channel, use_checksum, ""); + respond(response_channel, use_checksum, "Properties start at odrive root, such as axis0.requested_state"); + respond(response_channel, use_checksum, "Read: r property"); + respond(response_channel, use_checksum, "Write: w property value"); + respond(response_channel, use_checksum, ""); + respond(response_channel, use_checksum, "Save config: ss"); + respond(response_channel, use_checksum, "Erase config: se"); + respond(response_channel, use_checksum, "Reboot: sr"); +} + +// @brief Gets the hardware, firmware and serial details +// @param pStr buffer of ASCII encoded values +// @param response_channel reference to the stream to respond on +// @param use_checksum bool to indicate whether a checksum is required on response +void infoDump(char * pStr, StreamSink& response_channel, bool use_checksum) +{ + // 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, "Hardware version: %d.%d-%dV", HW_VERSION_MAJOR, HW_VERSION_MINOR, HW_VERSION_VOLTAGE); + respond(response_channel, use_checksum, "Firmware version: %d.%d.%d", FW_VERSION_MAJOR, FW_VERSION_MINOR, FW_VERSION_REVISION); + respond(response_channel, use_checksum, "Serial number: %s", serial_number_str); +} + +// @brief Executes the system control command +// @param pStr buffer of ASCII encoded values +// @param response_channel reference to the stream to respond on +// @param use_checksum bool to indicate whether a checksum is required on response +void systemCTRL(char * pStr, StreamSink& response_channel, bool use_checksum) +{ + switch (pStr[1]) + { + case 's': save_configuration(); break; // Save config + case 'e': erase_configuration(); break; // Erase config + case 'r': NVIC_SystemReset(); break; // Reboot + default: /* default */ break; + } +} + +// @brief Executes the read parameter command +// @param pStr buffer of ASCII encoded values +// @param response_channel reference to the stream to respond on +// @param use_checksum bool to indicate whether a checksum is required on response +void readProperty(char * pStr, StreamSink& response_channel, bool use_checksum) +{ + char name[MAX_LINE_LENGTH]; + + if (sscanf(pStr, "r %255s", name) < 1) { + respond(response_channel, use_checksum, "invalid command format"); + } else { + Endpoint* endpoint = application_endpoints_->get_by_name(name, sizeof(name)); + if (!endpoint) { + respond(response_channel, use_checksum, "invalid property"); + } else { + char response[10]; + respond(response_channel, use_checksum, (endpoint->get_string(response, sizeof(response))) ? response : "not implemented"); + } + } +} + +// @brief Executes the set write position command +// @param pStr buffer of ASCII encoded values +// @param response_channel reference to the stream to respond on +// @param use_checksum bool to indicate whether a checksum is required on response +void writeProperty(char * pStr, StreamSink& response_channel, bool use_checksum) +{ + char name[MAX_LINE_LENGTH]; + char value[MAX_LINE_LENGTH]; + + if (sscanf(pStr, "w %255s %255s", name, value) < 1) { + respond(response_channel, use_checksum, "invalid command format"); + } else { + Endpoint* endpoint = application_endpoints_->get_by_name(name, sizeof(name)); + if (!endpoint) { + respond(response_channel, use_checksum, "invalid property"); + } else { + if (!endpoint->set_string(value, sizeof(value))) { + respond(response_channel, use_checksum, "not implemented"); + } + } + } +} + +// @brief Executes the motor watchdog update command +// @param pStr buffer of ASCII encoded values +// @param response_channel reference to the stream to respond on +// @param use_checksum bool to indicate whether a checksum is required on response +void updateAxisWDG(char * pStr, StreamSink& response_channel, bool use_checksum) +{ + unsigned motor_number; + + if(sscanf(pStr, "u %u", &motor_number) < 1) { + 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]->watchdog_feed(); + } +} + +// @brief Sends the unknown command response +// @param pStr buffer of ASCII encoded values +// @param response_channel reference to the stream to respond on +// @param use_checksum bool to indicate whether a checksum is required on response +void unknownCMD(char * pStr, StreamSink& response_channel, bool use_checksum) +{ + (void)pStr; + respond(response_channel, use_checksum, "unknown command"); +} + +// @brief Parses the received ASCII char stream +// @param buffer buffer of ASCII encoded values +// @param response_channel reference to the stream to respond on +// @param use_checksum bool to indicate whether a checksum is required on response 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;