From 149c62568faab36719c05272559bd2da787cc640 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Mon, 17 Feb 2025 15:51:58 +0300 Subject: [PATCH] Refactor CLI submodule Move command parsing to doCommand Parse command with splitString instead of stringToken Trim commands Move cliTestMotor to the bottom Rename parseInput to handleInput, which is more clear Move motor test function to motors.ino --- flix/cli.ino | 41 ++++++++++++----------------------------- flix/flix.ino | 2 +- flix/motors.ino | 11 +++++++++++ flix/util.h | 10 ++++++++++ gazebo/Arduino.h | 6 +++++- gazebo/flix.h | 2 +- gazebo/simulator.cpp | 2 +- 7 files changed, 41 insertions(+), 33 deletions(-) diff --git a/flix/cli.ino b/flix/cli.ino index 0165b02..79d002b 100644 --- a/flix/cli.ino +++ b/flix/cli.ino @@ -39,7 +39,12 @@ const char* motd = "reset - reset drone's state\n" "reboot - reboot the drone\n"; -void doCommand(const String& command, const String& arg0, const String& arg1) { +void doCommand(String str) { + // parse command + String command, arg0, arg1; + splitString(str, command, arg0, arg1); + + // execute command if (command == "help" || command == "motd") { Serial.println(motd); } else if (command == "p" && arg0 == "") { @@ -90,13 +95,13 @@ void doCommand(const String& command, const String& arg0, const String& arg1) { } else if (command == "ca") { calibrateAccel(); } else if (command == "mfr") { - cliTestMotor(MOTOR_FRONT_RIGHT); + testMotor(MOTOR_FRONT_RIGHT); } else if (command == "mfl") { - cliTestMotor(MOTOR_FRONT_LEFT); + testMotor(MOTOR_FRONT_LEFT); } else if (command == "mrr") { - cliTestMotor(MOTOR_REAR_RIGHT); + testMotor(MOTOR_REAR_RIGHT); } else if (command == "mrl") { - cliTestMotor(MOTOR_REAR_LEFT); + testMotor(MOTOR_REAR_LEFT); } else if (command == "reset") { attitude = Quaternion(); } else if (command == "reboot") { @@ -108,18 +113,7 @@ void doCommand(const String& command, const String& arg0, const String& arg1) { } } -void cliTestMotor(uint8_t n) { - Serial.printf("Testing motor %d\n", n); - motors[n] = 1; - delay(50); // ESP32 may need to wait until the end of the current cycle to change duty https://github.com/espressif/arduino-esp32/issues/5306 - sendMotors(); - delay(3000); - motors[n] = 0; - sendMotors(); - Serial.println("Done"); -} - -void parseInput() { +void handleInput() { static bool showMotd = true; static String input; @@ -131,21 +125,10 @@ void parseInput() { while (Serial.available()) { char c = Serial.read(); if (c == '\n') { - char chars[input.length() + 1]; - input.toCharArray(chars, input.length() + 1); - String command = stringToken(chars, " "); - String arg0 = stringToken(NULL, " "); - String arg1 = stringToken(NULL, ""); - doCommand(command, arg0, arg1); + doCommand(input); input.clear(); } else { input += c; } } } - -// Helper function for parsing input -String stringToken(char* str, const char* delim) { - char* token = strtok(str, delim); - return token == NULL ? "" : token; -} diff --git a/flix/flix.ino b/flix/flix.ino index 61e9689..b33f1db 100644 --- a/flix/flix.ino +++ b/flix/flix.ino @@ -44,7 +44,7 @@ void loop() { estimate(); control(); sendMotors(); - parseInput(); + handleInput(); #if WIFI_ENABLED processMavlink(); #endif diff --git a/flix/motors.ino b/flix/motors.ino index aea72e8..5c28aea 100644 --- a/flix/motors.ino +++ b/flix/motors.ino @@ -54,3 +54,14 @@ void sendMotors() { bool motorsActive() { return motors[0] != 0 || motors[1] != 0 || motors[2] != 0 || motors[3] != 0; } + +void testMotor(uint8_t n) { + Serial.printf("Testing motor %d\n", n); + motors[n] = 1; + delay(50); // ESP32 may need to wait until the end of the current cycle to change duty https://github.com/espressif/arduino-esp32/issues/5306 + sendMotors(); + delay(3000); + motors[n] = 0; + sendMotors(); + Serial.printf("Done\n"); +} diff --git a/flix/util.h b/flix/util.h index a46c034..a889a5c 100644 --- a/flix/util.h +++ b/flix/util.h @@ -42,3 +42,13 @@ void printArray(const T (&arr)[N]) { void disableBrownOut() { REG_CLR_BIT(RTC_CNTL_BROWN_OUT_REG, RTC_CNTL_BROWN_OUT_ENA); } + +// Trim and split string by spaces +void splitString(String& str, String& token0, String& token1, String& token2) { + str.trim(); + char chars[str.length() + 1]; + str.toCharArray(chars, str.length() + 1); + token0 = strtok(chars, " "); + token1 = strtok(NULL, " "); // String(NULL) creates empty string + token2 = strtok(NULL, ""); +} diff --git a/gazebo/Arduino.h b/gazebo/Arduino.h index 509b574..77841cf 100644 --- a/gazebo/Arduino.h +++ b/gazebo/Arduino.h @@ -41,13 +41,17 @@ class __FlashStringHelper; // https://www.arduino.cc/reference/en/language/variables/data-types/stringobject/ class String: public std::string { public: - String(const char *str = "") : std::string(str) {} + String(const char *str = "") : std::string(str ? str : "") {} long toInt() const { return atol(this->c_str()); } float toFloat() const { return atof(this->c_str()); } bool isEmpty() const { return this->empty(); } void toCharArray(char *buf, unsigned int bufsize, unsigned int index = 0) const { strlcpy(buf, this->c_str() + index, bufsize); } + void trim() { + this->erase(0, this->find_first_not_of(" \t\n\r")); + this->erase(this->find_last_not_of(" \t\n\r") + 1); + } }; class Print; diff --git a/gazebo/flix.h b/gazebo/flix.h index 6ae749d..1437623 100644 --- a/gazebo/flix.h +++ b/gazebo/flix.h @@ -34,8 +34,8 @@ void controlTorque(); void showTable(); void sendMotors(); bool motorsActive(); +void doCommand(String str); void cliTestMotor(uint8_t n); -String stringToken(char* str, const char* delim); void normalizeRC(); void printRCCal(); void processMavlink(); diff --git a/gazebo/simulator.cpp b/gazebo/simulator.cpp index c520b6e..5986343 100644 --- a/gazebo/simulator.cpp +++ b/gazebo/simulator.cpp @@ -81,7 +81,7 @@ public: attitude.setYaw(this->model->WorldPose().Yaw()); control(); - parseInput(); + handleInput(); processMavlink(); applyMotorForces();