From 4c6fe56a5d99aa4442aabda6b1acca7f52051af5 Mon Sep 17 00:00:00 2001 From: sgilbert182 <45004203+sgilbert182@users.noreply.github.com> Date: Thu, 12 Dec 2019 10:12:07 +0000 Subject: [PATCH 1/2] Split up menu function to individual functions, changed if else tree to switch statement. --- Firmware/communication/ascii_protocol.cpp | 493 ++++++++++++++-------- 1 file changed, 308 insertions(+), 185 deletions(-) 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; From b56f7c1ff2180a3c0e16b6bc41423b070c93b3dc Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Wed, 5 Aug 2020 18:49:48 +0200 Subject: [PATCH 2/2] harmonize coding style --- Firmware/communication/ascii_protocol.cpp | 103 +++++++++------------- 1 file changed, 44 insertions(+), 59 deletions(-) diff --git a/Firmware/communication/ascii_protocol.cpp b/Firmware/communication/ascii_protocol.cpp index a87c7388..7e413ebe 100644 --- a/Firmware/communication/ascii_protocol.cpp +++ b/Firmware/communication/ascii_protocol.cpp @@ -27,19 +27,19 @@ /* 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); +void cmd_set_position(char * pStr, StreamSink& response_channel, bool use_checksum); +void cmd_set_position_wl(char * pStr, StreamSink& response_channel, bool use_checksum); +void cmd_set_velocity(char * pStr, StreamSink& response_channel, bool use_checksum); +void cmd_set_current(char * pStr, StreamSink& response_channel, bool use_checksum); +void cmd_set_trapezoid_trajectory(char * pStr, StreamSink& response_channel, bool use_checksum); +void cmd_get_feedback(char * pStr, StreamSink& response_channel, bool use_checksum); +void cmd_help(char * pStr, StreamSink& response_channel, bool use_checksum); +void cmd_info_dump(char * pStr, StreamSink& response_channel, bool use_checksum); +void cmd_system_ctrl(char * pStr, StreamSink& response_channel, bool use_checksum); +void cmd_read_property(char * pStr, StreamSink& response_channel, bool use_checksum); +void cmd_write_property(char * pStr, StreamSink& response_channel, bool use_checksum); +void cmd_update_axis_wdg(char * pStr, StreamSink& response_channel, bool use_checksum); +void cmd_unknown(char * pStr, StreamSink& response_channel, bool use_checksum); /* Function implementations --------------------------------------------------*/ @@ -50,12 +50,12 @@ void respond(StreamSink& output, bool include_checksum, const char * fmt, TArgs& 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]; - output.process_bytes((uint8_t*)response, snprintf(response, sizeof(response), "*%u", checksum), nullptr); + len = snprintf(response, sizeof(response), "*%u", checksum); + output.process_bytes((uint8_t*)response, len, nullptr); } output.process_bytes((const uint8_t*)"\r\n", 2, nullptr); } @@ -75,8 +75,7 @@ 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 { @@ -104,21 +103,20 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& // check incoming packet type - 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; + switch(cmd[0]) { + case 'p': cmd_set_position(cmd, response_channel, use_checksum); break; // position control + case 'q': cmd_set_position_wl(cmd, response_channel, use_checksum); break; // position control with limits + case 'v': cmd_set_velocity(cmd, response_channel, use_checksum); break; // velocity control + case 'c': cmd_set_current(cmd, response_channel, use_checksum); break; // current control + case 't': cmd_set_trapezoid_trajectory(cmd, response_channel, use_checksum); break; // trapezoidal trajectory + case 'f': cmd_get_feedback(cmd, response_channel, use_checksum); break; // feedback + case 'h': cmd_help(cmd, response_channel, use_checksum); break; // Help + case 'i': cmd_info_dump(cmd, response_channel, use_checksum); break; // Dump device info + case 's': cmd_system_ctrl(cmd, response_channel, use_checksum); break; // System + case 'r': cmd_read_property(cmd, response_channel, use_checksum); break; // read property + case 'w': cmd_write_property(cmd, response_channel, use_checksum); break; // write property + case 'u': cmd_update_axis_wdg(cmd, response_channel, use_checksum); break; // Update axis watchdog. + default : cmd_unknown(nullptr, response_channel, use_checksum); break; } } @@ -126,8 +124,7 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& // @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) -{ +void cmd_set_position(char * pStr, StreamSink& response_channel, bool use_checksum) { unsigned motor_number; float pos_setpoint, vel_feed_forward, current_feed_forward; @@ -153,8 +150,7 @@ void setPosition(char * pStr, StreamSink& response_channel, bool use_checksum) // @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) -{ +void cmd_set_position_wl(char * pStr, StreamSink& response_channel, bool use_checksum) { unsigned motor_number; float pos_setpoint, vel_limit, current_lim; @@ -180,8 +176,7 @@ void setPositionWL(char * pStr, StreamSink& response_channel, bool use_checksum) // @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) -{ +void cmd_set_velocity(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); @@ -203,8 +198,7 @@ void setVelocity(char * pStr, StreamSink& response_channel, bool use_checksum) // @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) -{ +void cmd_set_current(char * pStr, StreamSink& response_channel, bool use_checksum) { unsigned motor_number; float current_setpoint; @@ -224,8 +218,7 @@ void setCurrent(char * pStr, StreamSink& response_channel, bool use_checksum) // @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) -{ +void cmd_set_trapezoid_trajectory(char * pStr, StreamSink& response_channel, bool use_checksum) { unsigned motor_number; float goal_point; @@ -245,8 +238,7 @@ void setTrapezoidTrajectory(char * pStr, StreamSink& response_channel, bool use_ // @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) -{ +void cmd_get_feedback(char * pStr, StreamSink& response_channel, bool use_checksum) { unsigned motor_number; if (sscanf(pStr, "f %u", &motor_number) < 1) { @@ -265,8 +257,7 @@ void getFeedback(char * pStr, StreamSink& response_channel, bool use_checksum) // @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 cmd_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, ""); @@ -289,8 +280,7 @@ void help(char * pStr, StreamSink& response_channel, bool use_checksum) // @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) -{ +void cmd_info_dump(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()); @@ -303,8 +293,7 @@ void infoDump(char * pStr, StreamSink& response_channel, bool use_checksum) // @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) -{ +void cmd_system_ctrl(char * pStr, StreamSink& response_channel, bool use_checksum) { switch (pStr[1]) { case 's': save_configuration(); break; // Save config @@ -318,8 +307,7 @@ void systemCTRL(char * pStr, StreamSink& response_channel, bool use_checksum) // @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) -{ +void cmd_read_property(char * pStr, StreamSink& response_channel, bool use_checksum) { char name[MAX_LINE_LENGTH]; if (sscanf(pStr, "r %255s", name) < 1) { @@ -339,8 +327,7 @@ void readProperty(char * pStr, StreamSink& response_channel, bool use_checksum) // @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) -{ +void cmd_write_property(char * pStr, StreamSink& response_channel, bool use_checksum) { char name[MAX_LINE_LENGTH]; char value[MAX_LINE_LENGTH]; @@ -362,8 +349,7 @@ void writeProperty(char * pStr, StreamSink& response_channel, bool use_checksum) // @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) -{ +void cmd_update_axis_wdg(char * pStr, StreamSink& response_channel, bool use_checksum) { unsigned motor_number; if(sscanf(pStr, "u %u", &motor_number) < 1) { @@ -379,8 +365,7 @@ void updateAxisWDG(char * pStr, StreamSink& response_channel, bool use_checksum) // @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 cmd_unknown(char * pStr, StreamSink& response_channel, bool use_checksum) { (void)pStr; respond(response_channel, use_checksum, "unknown command"); }