Skip to content

armgit / flix

Public Imported from GitHub
0

Making an ESP32-based quadcopter from scratch

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

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