Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
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.inoindex 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 commandif (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.inoindex 61e9689..b33f1db 100644--- a/flix/flix.ino+++ b/flix/flix.ino@@ -44,7 +44,7 @@ void loop() {estimate();control();sendMotors();- parseInput();+ handleInput();#if WIFI_ENABLEDprocessMavlink();#endifdiff --git a/flix/motors.ino b/flix/motors.inoindex 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.hindex 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.hindex 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.hindex 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.cppindex 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();