Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
Move all global variable declarations to the appropriate subsystems As it makes the subsystems code easier to understand. Declare the most used variables in main sketch file as forward declarations. Make all control input zero by default (except controlMode). Minor changes.
Oleg Kalachevc8e5e08b03b2
diff --git a/flix/cli.ino b/flix/cli.inoindex b92f56d..145b80e 100644--- a/flix/cli.ino+++ b/flix/cli.ino@@ -11,7 +11,7 @@ extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FROextern const int RAW, ACRO, STAB, AUTO;extern float t, dt, loopRate;extern uint16_t channels[16];-extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;+extern float controlTime;extern int mode;extern bool armed;@@ -132,6 +132,7 @@ void doCommand(String str, bool echo = false) {}print("\nroll: %g pitch: %g yaw: %g throttle: %g mode: %g\n",controlRoll, controlPitch, controlYaw, controlThrottle, controlMode);+ print("time: %.1f\n", controlTime);print("mode: %s\n", getModeName());print("armed: %d\n", armed);} else if (command == "wifi") {diff --git a/flix/estimate.ino b/flix/estimate.inoindex cd8dcb9..a873b9a 100644--- a/flix/estimate.ino+++ b/flix/estimate.ino@@ -8,6 +8,10 @@#include "lpf.h"#include "util.h"+Vector rates; // estimated angular rates, rad/s+Quaternion attitude; // estimated attitude+bool landed;+float accWeight = 0.003;LowPassFilter<Vector> ratesFilter(0.2); // cutoff frequency ~ 40 Hzdiff --git a/flix/flix.ino b/flix/flix.inoindex 8c0f260..8036b60 100644--- a/flix/flix.ino+++ b/flix/flix.ino@@ -9,16 +9,13 @@#define WIFI_ENABLED 1-float t = NAN; // current step time, s-float dt; // time delta from previous step, s-float controlRoll, controlPitch, controlYaw, controlThrottle; // pilot's inputs, range [-1, 1]-float controlMode = NAN;-Vector gyro; // gyroscope data-Vector acc; // accelerometer data, m/s/s-Vector rates; // filtered angular rates, rad/s-Quaternion attitude; // estimated attitude-bool landed; // are we landed and stationary-float motors[4]; // normalized motors thrust in range [0..1]+extern float t, dt;+extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode;+extern Vector gyro, acc;+extern Vector rates;+extern Quaternion attitude;+extern bool landed;+extern float motors[4];void setup() {Serial.begin(115200);diff --git a/flix/imu.ino b/flix/imu.inoindex 7c482f1..22cf3e3 100644--- a/flix/imu.ino+++ b/flix/imu.ino@@ -12,9 +12,12 @@MPU9250 imu(SPI);Vector imuRotation(0, 0, -PI / 2); // imu orientation as Euler angles+Vector gyro; // gyroscope output, rad/s+Vector gyroBias;++Vector acc; // accelerometer output, m/s/sVector accBias;Vector accScale(1, 1, 1);-Vector gyroBias;void setupIMU() {print("Setup IMU\n");diff --git a/flix/mavlink.ino b/flix/mavlink.inoindex 4436c92..cb122fc 100644--- a/flix/mavlink.ino+++ b/flix/mavlink.ino@@ -12,12 +12,11 @@#define MAVLINK_RATE_SLOW 1#define MAVLINK_RATE_FAST 10+extern float controlTime;+bool mavlinkConnected = false;String mavlinkPrintBuffer;-extern float controlTime;-extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlMode;-void processMavlink() {sendMavlink();receiveMavlink();diff --git a/flix/motors.ino b/flix/motors.inoindex 6b2af65..3dde15c 100644--- a/flix/motors.ino+++ b/flix/motors.ino@@ -17,7 +17,8 @@#define PWM_MIN 0#define PWM_MAX 1000000 / PWM_FREQUENCY-// Motors array indexes:+float motors[4]; // normalized motor thrusts in range [0..1]+const int MOTOR_REAR_LEFT = 0;const int MOTOR_REAR_RIGHT = 1;const int MOTOR_FRONT_RIGHT = 2;diff --git a/flix/rc.ino b/flix/rc.inoindex 21c03ae..5702316 100644--- a/flix/rc.ino+++ b/flix/rc.ino@@ -6,13 +6,16 @@#include <SBUS.h>#include "util.h"-SBUS rc(Serial2); // NOTE: Use RC(Serial2, 16, 17) if you use the old UART2 pins+SBUS rc(Serial2);uint16_t channels[16]; // raw rc channels-float controlTime; // time of the last controls updatefloat channelZero[16]; // calibration zero valuesfloat channelMax[16]; // calibration max values+float controlRoll, controlPitch, controlYaw, controlThrottle; // pilot's inputs, range [-1, 1]+float controlMode = NAN; //+float controlTime; // time of the last controls update (0 when no RC)+// Channels mapping (using float to store in parameters):float rollChannel = NAN, pitchChannel = NAN, throttleChannel = NAN, yawChannel = NAN, modeChannel = NAN;@@ -38,11 +41,11 @@ void normalizeRC() {controls[i] = mapf(channels[i], channelZero[i], channelMax[i], 0, 1);}// Update control values- controlRoll = rollChannel >= 0 ? controls[(int)rollChannel] : NAN;- controlPitch = pitchChannel >= 0 ? controls[(int)pitchChannel] : NAN;- controlYaw = yawChannel >= 0 ? controls[(int)yawChannel] : NAN;- controlThrottle = throttleChannel >= 0 ? controls[(int)throttleChannel] : NAN;- controlMode = modeChannel >= 0 ? controls[(int)modeChannel] : NAN;+ controlRoll = rollChannel >= 0 ? controls[(int)rollChannel] : 0;+ controlPitch = pitchChannel >= 0 ? controls[(int)pitchChannel] : 0;+ controlYaw = yawChannel >= 0 ? controls[(int)yawChannel] : 0;+ controlThrottle = throttleChannel >= 0 ? controls[(int)throttleChannel] : 0;+ controlMode = modeChannel >= 0 ? controls[(int)modeChannel] : NAN; // mode switch should not have affect if not set}void calibrateRC() {diff --git a/flix/time.ino b/flix/time.inoindex b57b5bc..f3ba3d1 100644--- a/flix/time.ino+++ b/flix/time.ino@@ -3,6 +3,8 @@// Time related functions+float t = NAN; // current time, s+float dt; // time delta with the previous step, sfloat loopRate; // Hzvoid step() {diff --git a/flix/vector.h b/flix/vector.hindex 272e1db..efc3be0 100644--- a/flix/vector.h+++ b/flix/vector.h@@ -35,7 +35,6 @@ public:z = NAN;}-float norm() const {return sqrt(x * x + y * y + z * z);}diff --git a/gazebo/flix.h b/gazebo/flix.hindex e7d3a2c..91d72c4 100644--- a/gazebo/flix.h+++ b/gazebo/flix.h@@ -12,17 +12,15 @@#define WIFI_ENABLED 1-float t = NAN;-float dt;-float motors[4];-float controlRoll, controlPitch, controlYaw, controlThrottle = NAN;-float controlMode = NAN;-Vector acc;-Vector gyro;-Vector rates;-Quaternion attitude;-bool landed;-Vector imuRotation;+extern float t, dt;+extern float controlRoll, controlPitch, controlYaw, controlThrottle, controlMode;+extern Vector rates;+extern Quaternion attitude;+extern bool landed;+extern float motors[4];++Vector gyro, acc, imuRotation;+Vector accBias, gyroBias, accScale(1, 1, 1);// declarationsvoid step();@@ -75,4 +73,3 @@ void calibrateAccel() { print("Skip accel calibrating\n"); };void printIMUCalibration() { print("cal: N/A\n"); };void printIMUInfo() {};void printWiFiInfo() {};-Vector accBias, gyroBias, accScale(1, 1, 1);