Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
Rename motor indexes constant names The original names are overly verbose. Make motor pins disabled by default on all platforms except classic esp32. Make config more clear.
Oleg Kalachev647d68f1b765
diff --git a/flix/cli.ino b/flix/cli.inoindex d26f6c6..0ca5fc0 100644--- a/flix/cli.ino+++ b/flix/cli.ino@@ -8,7 +8,7 @@#include "util.h"#include "filter.h"-extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;+extern const int MOT_RL, MOT_RR, MOT_FR, MOT_FL;extern const int RAW, ACRO, STAB, AUTO;extern const int W_AP, W_STA, W_ESPNOW;extern float t, dt, loopRate;@@ -152,7 +152,7 @@ void doCommand(String str, bool echo = false) {configWiFi(W_ESPNOW, arg0.c_str(), arg1.c_str());} else if (command == "mot") {print("front-right %g front-left %g rear-right %g rear-left %g\n",- motors[MOTOR_FRONT_RIGHT], motors[MOTOR_FRONT_LEFT], motors[MOTOR_REAR_RIGHT], motors[MOTOR_REAR_LEFT]);+ motors[MOT_FR], motors[MOT_FL], motors[MOT_RR], motors[MOT_RL]);} else if (command == "log") {printLogHeader();if (arg0 == "dump") printLogData();@@ -161,13 +161,13 @@ void doCommand(String str, bool echo = false) {} else if (command == "ca") {calibrateAccel();} else if (command == "mfr") {- testMotor(MOTOR_FRONT_RIGHT, arg0.isEmpty() ? 0.2 : arg0.toFloat());+ testMotor(MOT_FR, arg0.isEmpty() ? 0.2 : arg0.toFloat());} else if (command == "mfl") {- testMotor(MOTOR_FRONT_LEFT, arg0.isEmpty() ? 0.2 : arg0.toFloat());+ testMotor(MOT_FL, arg0.isEmpty() ? 0.2 : arg0.toFloat());} else if (command == "mrr") {- testMotor(MOTOR_REAR_RIGHT, arg0.isEmpty() ? 0.2 : arg0.toFloat());+ testMotor(MOT_RR, arg0.isEmpty() ? 0.2 : arg0.toFloat());} else if (command == "mrl") {- testMotor(MOTOR_REAR_LEFT, arg0.isEmpty() ? 0.2 : arg0.toFloat());+ testMotor(MOT_RL, arg0.isEmpty() ? 0.2 : arg0.toFloat());} else if (command == "sys") {#ifdef ESP32print("Chip: %s\n", ESP.getChipModel());diff --git a/flix/config.h b/flix/config.hindex badea19..c2c290e 100644--- a/flix/config.h+++ b/flix/config.h@@ -12,16 +12,22 @@ void setDefaults() {pwmFrequency = 38000;#endif+ #ifdef CONFIG_IDF_TARGET_ESP32+ // classic esp32 configuration+ motorPins[MOT_RL] = 12;+ motorPins[MOT_RR] = 13;+ motorPins[MOT_FR] = 14;+ motorPins[MOT_FL] = 15;+ #endif+#ifdef FLIX2imuModel = 4; // ICM-40609-DimuIntPin = 10;imuCsPin = 14;-- motorPins[MOTOR_REAR_LEFT] = 41;- motorPins[MOTOR_REAR_RIGHT] = 7;- motorPins[MOTOR_FRONT_RIGHT] = 18;- motorPins[MOTOR_FRONT_LEFT] = 38;-voltagePin = 3;+ motorPins[MOT_RL] = 41;+ motorPins[MOT_RR] = 7;+ motorPins[MOT_FR] = 18;+ motorPins[MOT_FL] = 38;#endif}diff --git a/flix/control.ino b/flix/control.inoindex 43a2a4a..1164a2d 100644--- a/flix/control.ino+++ b/flix/control.ino@@ -29,7 +29,7 @@ Vector maxRate(radians(360), radians(360), radians(360));float tiltMax = radians(30);int flightModes[] = {STAB, STAB, STAB}; // map for rc mode switch-extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;+extern const int MOT_RL, MOT_RR, MOT_FR, MOT_FL;extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;void control() {@@ -119,13 +119,13 @@ void controlTorque() {return;}- motors[MOTOR_FRONT_LEFT] = thrustTarget + torqueTarget.x - torqueTarget.y + torqueTarget.z;- motors[MOTOR_FRONT_RIGHT] = thrustTarget - torqueTarget.x - torqueTarget.y - torqueTarget.z;- motors[MOTOR_REAR_LEFT] = thrustTarget + torqueTarget.x + torqueTarget.y - torqueTarget.z;- motors[MOTOR_REAR_RIGHT] = thrustTarget - torqueTarget.x + torqueTarget.y + torqueTarget.z;+ motors[MOT_FL] = thrustTarget + torqueTarget.x - torqueTarget.y + torqueTarget.z;+ motors[MOT_FR] = thrustTarget - torqueTarget.x - torqueTarget.y - torqueTarget.z;+ motors[MOT_RL] = thrustTarget + torqueTarget.x + torqueTarget.y - torqueTarget.z;+ motors[MOT_RR] = thrustTarget - torqueTarget.x + torqueTarget.y + torqueTarget.z;// Prioritize angle control over thrust control- desaturate(motors[MOTOR_FRONT_LEFT], motors[MOTOR_FRONT_RIGHT], motors[MOTOR_REAR_LEFT], motors[MOTOR_REAR_RIGHT]);+ desaturate(motors[MOT_FL], motors[MOT_FR], motors[MOT_RL], motors[MOT_RR]);motors[0] = constrain(motors[0], 0, 1);motors[1] = constrain(motors[1], 0, 1);diff --git a/flix/motors.ino b/flix/motors.inoindex 6b808b1..40699f0 100644--- a/flix/motors.ino+++ b/flix/motors.ino@@ -7,14 +7,14 @@float motors[4]; // normalized motor thrusts in range [0..1]-int motorPins[4] = {12, 13, 14, 15}; // default pin numbers+int motorPins[4] = {-1, -1, -1, -1}; // default pin numbersint pwmFrequency = 78000;int pwmResolution = 10;int pwmStop = 0;int pwmMin = 0;int pwmMax = -1; // -1 means duty cycle mode-const int MOTOR_REAR_LEFT = 0, MOTOR_REAR_RIGHT = 1, MOTOR_FRONT_RIGHT = 2, MOTOR_FRONT_LEFT = 3;+const int MOT_RL = 0, MOT_RR = 1, MOT_FR = 2, MOT_FL = 3;void setupMotors() {print("Setup motors\n");diff --git a/flix/parameters.ino b/flix/parameters.inoindex ff3daa3..32c874d 100644--- a/flix/parameters.ino+++ b/flix/parameters.ino@@ -87,10 +87,10 @@ Parameter parameters[] = {{"EST_LVL_WEIGHT", &levelWeight},{"EST_RATES_LPF_A", &ratesFilter.alpha},// motors- {"MOT_PIN_FL", &motorPins[MOTOR_FRONT_LEFT], setupMotors},- {"MOT_PIN_FR", &motorPins[MOTOR_FRONT_RIGHT], setupMotors},- {"MOT_PIN_RL", &motorPins[MOTOR_REAR_LEFT], setupMotors},- {"MOT_PIN_RR", &motorPins[MOTOR_REAR_RIGHT], setupMotors},+ {"MOT_PIN_FL", &motorPins[MOT_FL], setupMotors},+ {"MOT_PIN_FR", &motorPins[MOT_FR], setupMotors},+ {"MOT_PIN_RL", &motorPins[MOT_RL], setupMotors},+ {"MOT_PIN_RR", &motorPins[MOT_RR], setupMotors},{"MOT_PWM_FREQ", &pwmFrequency, setupMotors},{"MOT_PWM_RES", &pwmResolution, setupMotors},{"MOT_PWM_STOP", &pwmStop},diff --git a/gazebo/simulator.cpp b/gazebo/simulator.cppindex 63bcec7..65a4138 100644--- a/gazebo/simulator.cpp+++ b/gazebo/simulator.cpp@@ -98,10 +98,10 @@ public:const double maxThrust = 0.03 * ONE_G; // ~30 g, https://youtu.be/VtKI4Pjx8Sk?&t=78const float scale0 = 1.0, scale1 = 1.1, scale2 = 0.9, scale3 = 1.05; // imitating motors asymmetry- float mfl = scale0 * maxThrust * motors[MOTOR_FRONT_LEFT];- float mfr = scale1 * maxThrust * motors[MOTOR_FRONT_RIGHT];- float mrl = scale2 * maxThrust * motors[MOTOR_REAR_LEFT];- float mrr = scale3 * maxThrust * motors[MOTOR_REAR_RIGHT];+ float mfl = scale0 * maxThrust * motors[MOT_FL];+ float mfr = scale1 * maxThrust * motors[MOT_FR];+ float mrl = scale2 * maxThrust * motors[MOT_RL];+ float mrr = scale3 * maxThrust * motors[MOT_RR];body->AddLinkForce(Vector3d(0.0, 0.0, mfl), Vector3d(dist, dist, 0.0));body->AddLinkForce(Vector3d(0.0, 0.0, mfr), Vector3d(dist, -dist, 0.0));@@ -110,10 +110,10 @@ public:// torqueconst double maxTorque = 0.0024 * ONE_G; // ~24 g*cm- body->AddRelativeTorque(Vector3d(0.0, 0.0, scale0 * maxTorque * motors[MOTOR_FRONT_LEFT]));- body->AddRelativeTorque(Vector3d(0.0, 0.0, scale1 * -maxTorque * motors[MOTOR_FRONT_RIGHT]));- body->AddRelativeTorque(Vector3d(0.0, 0.0, scale2 * -maxTorque * motors[MOTOR_REAR_LEFT]));- body->AddRelativeTorque(Vector3d(0.0, 0.0, scale3 * maxTorque * motors[MOTOR_REAR_RIGHT]));+ body->AddRelativeTorque(Vector3d(0.0, 0.0, scale0 * maxTorque * motors[MOT_FL]));+ body->AddRelativeTorque(Vector3d(0.0, 0.0, scale1 * -maxTorque * motors[MOT_FR]));+ body->AddRelativeTorque(Vector3d(0.0, 0.0, scale2 * -maxTorque * motors[MOT_RL]));+ body->AddRelativeTorque(Vector3d(0.0, 0.0, scale3 * maxTorque * motors[MOT_RR]));}void initNode() {