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

@@ -21,8 +21,6 @@ const char* motd =
 "|__|     |_______||__| /__/ \\__\\\n\n"
 "Commands:\n\n"
 "help - show help\n"
-"show - show all parameters\n"
-"<name> <value> - 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 {
+void doCommand(String str) {
-	const char* name;
+	// parse command
-	float* value;
+	String command, arg0, arg1;
-	float* value2;
+	splitString(str, command, arg0, arg1);
-} params[] = {
-	{"rp", &rollRatePID.p, &pitchRatePID.p},
-	{"ri", &rollRatePID.i, &pitchRatePID.i},
-	{"rd", &rollRatePID.d, &pitchRatePID.d},
 
-	{"ap", &rollPID.p, &pitchPID.p},
+	// execute command
-	{"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) {
 	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() {
+void handleInput() {
-	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() {
 	static bool showMotd = true;
-	static String command;
+	static String input;
-	static String value;
-	static bool parsingCommand = true;
 
 	if (showMotd) {
-		Serial.println(motd);
+		Serial.printf("%s\n", motd);
 		showMotd = false;
 	}
 
 	while (Serial.available()) {
 		char c = Serial.read();
 		if (c == '\n') {
-			parsingCommand = true;
+			doCommand(input);
-			if (!command.isEmpty()) {
+			input.clear();
-				doCommand(command, value);
-			}
-			command.clear();
-			value.clear();
-		} else if (c == ' ') {
-			parsingCommand = false;
 		} else {
-			(parsingCommand ? command : value) += c;
+			input += c;
 		}
 	}
 }

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";

flix/flix.ino

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

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

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

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;

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

gazebo/simulator.cpp

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