From 6058e8ecabf4394844bf67a4e304d85a3898b096 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Fri, 28 Feb 2025 22:49:37 +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 Remove parameters table functionality to simplify the code --- flix/cli.ino | 96 ++++++++------------------------------------ flix/control.ino | 4 -- flix/flix.ino | 2 +- flix/motors.ino | 15 +++++++ flix/util.h | 10 +++++ gazebo/Arduino.h | 8 ++++ gazebo/flix.h | 3 +- gazebo/simulator.cpp | 2 +- 8 files changed, 54 insertions(+), 86 deletions(-) diff --git a/flix/cli.ino b/flix/cli.ino index 8c9ffcd..5114be5 100644 --- a/flix/cli.ino +++ b/flix/cli.ino @@ -21,8 +21,6 @@ const char* motd = "|__| |_______||__| /__/ \\__\\\n\n" "Commands:\n\n" "help - show help\n" -"show - show all parameters\n" -" - set parameter\n" "ps - show pitch/roll/yaw\n" "psq - show attitude quaternion\n" "imu - show IMU data\n" @@ -35,36 +33,14 @@ const char* motd = "mfr, mfl, mrr, mrl - test motor (remove props)\n" "reset - reset drone's state\n"; -const struct Param { - const char* name; - float* value; - float* value2; -} params[] = { - {"rp", &rollRatePID.p, &pitchRatePID.p}, - {"ri", &rollRatePID.i, &pitchRatePID.i}, - {"rd", &rollRatePID.d, &pitchRatePID.d}, +void doCommand(String str) { + // parse command + String command, arg0, arg1; + splitString(str, command, arg0, arg1); - {"ap", &rollPID.p, &pitchPID.p}, - {"ai", &rollPID.i, &pitchPID.i}, - {"ad", &rollPID.d, &pitchPID.d}, - - {"yp", &yawRatePID.p, nullptr}, - {"yi", &yawRatePID.i, nullptr}, - {"yd", &yawRatePID.d, nullptr}, - - {"lpr", &ratesFilter.alpha, nullptr}, - {"lpd", &rollRatePID.lpf.alpha, &pitchRatePID.lpf.alpha}, - - {"ss", &loopRate, nullptr}, - {"dt", &dt, nullptr}, - {"t", &t, nullptr}, -}; - -void doCommand(String& command, String& value) { + // execute command if (command == "help" || command == "motd") { Serial.println(motd); - } else if (command == "show") { - showTable(); } else if (command == "ps") { Vector a = attitude.toEulerZYX(); Serial.printf("roll: %f pitch: %f yaw: %f\n", a.x * RAD_TO_DEG, a.y * RAD_TO_DEG, a.z * RAD_TO_DEG); @@ -96,76 +72,38 @@ void doCommand(String& command, String& value) { } 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 == "") { + // do nothing } else { - float val = value.toFloat(); - // TODO: on error returns 0, check invalid value - - for (uint8_t i = 0; i < sizeof(params) / sizeof(params[0]); i++) { - if (command == params[i].name) { - *params[i].value = val; - if (params[i].value2 != nullptr) *params[i].value2 = val; - Serial.print(command); - Serial.print(" = "); - Serial.println(val, 4); - return; - } - } Serial.println("Invalid command: " + command); } } -void showTable() { - for (uint8_t i = 0; i < sizeof(params) / sizeof(params[0]); i++) { - Serial.print(params[i].name); - Serial.print(" "); - Serial.println(*params[i].value, 5); - } -} - -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 command; - static String value; - static bool parsingCommand = true; + static String input; if (showMotd) { - Serial.println(motd); + Serial.printf("%s\n", motd); showMotd = false; } while (Serial.available()) { char c = Serial.read(); if (c == '\n') { - parsingCommand = true; - if (!command.isEmpty()) { - doCommand(command, value); - } - command.clear(); - value.clear(); - } else if (c == ' ') { - parsingCommand = false; + doCommand(input); + input.clear(); } else { - (parsingCommand ? command : value) += c; + input += c; } } } diff --git a/flix/control.ino b/flix/control.ino index 8b31cbf..dd92d7d 100644 --- a/flix/control.ino +++ b/flix/control.ino @@ -165,10 +165,6 @@ void controlTorque() { motors[3] = constrain(motors[3], 0, 1); } -bool motorsActive() { - return motors[0] > 0 || motors[1] > 0 || motors[2] > 0 || motors[3] > 0; -} - const char* getModeName() { switch (mode) { case MANUAL: return "MANUAL"; diff --git a/flix/flix.ino b/flix/flix.ino index ba32e10..f07619a 100644 --- a/flix/flix.ino +++ b/flix/flix.ino @@ -50,7 +50,7 @@ void loop() { estimate(); control(); sendMotors(); - parseInput(); + handleInput(); #if WIFI_ENABLED processMavlink(); #endif diff --git a/flix/motors.ino b/flix/motors.ino index ac442b0..998af2c 100644 --- a/flix/motors.ino +++ b/flix/motors.ino @@ -46,3 +46,18 @@ void sendMotors() { ledcWrite(MOTOR_2_PIN, getDutyCycle(motors[2])); ledcWrite(MOTOR_3_PIN, getDutyCycle(motors[3])); } + +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 9f61df8..d791ac7 100644 --- a/flix/util.h +++ b/flix/util.h @@ -42,3 +42,13 @@ void printArray(T arr[], int size) { 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 f443ae9..5674cfb 100644 --- a/gazebo/Arduino.h +++ b/gazebo/Arduino.h @@ -33,9 +33,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 ? 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 b96c6e4..fa53012 100644 --- a/gazebo/flix.h +++ b/gazebo/flix.h @@ -34,7 +34,8 @@ void controlTorque(); void showTable(); void sendMotors(); bool motorsActive(); -void cliTestMotor(uint8_t n); +void doCommand(String str); +void normalizeRC(); void printRCCal(); void processMavlink(); void sendMavlink(); diff --git a/gazebo/simulator.cpp b/gazebo/simulator.cpp index ea2d353..dd3105b 100644 --- a/gazebo/simulator.cpp +++ b/gazebo/simulator.cpp @@ -79,7 +79,7 @@ public: attitude.setYaw(this->model->WorldPose().Yaw()); control(); - parseInput(); + handleInput(); processMavlink(); applyMotorForces();