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

@@ -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;
-}

flix/flix.ino

@@ -44,7 +44,7 @@ void loop() {
 	estimate();
 	control();
 	sendMotors();
-	parseInput();
+	handleInput();
 #if WIFI_ENABLED
 	processMavlink();
 #endif

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");
+}

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, "");
+}

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;

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();

gazebo/simulator.cpp

@@ -81,7 +81,7 @@ public:
 		attitude.setYaw(this->model->WorldPose().Yaw());
 
 		control();
-		parseInput();
+		handleInput();
 		processMavlink();
 
 		applyMotorForces();