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