Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
Use FlixPeriph library for IMU, implement own IMU calibration
Oleg Kalachev2cf1c7abb3d6
diff --git a/Makefile b/Makefileindex 58e5f7d..f1434a8 100644--- a/Makefile+++ b/Makefile@@ -16,7 +16,7 @@ dependencies .dependencies:arduino-cli core install esp32:[email protected] --config-file arduino-cli.yamlarduino-cli lib update-indexarduino-cli lib install "Bolder Flight Systems SBUS"@1.0.1- arduino-cli lib install "Bolder Flight Systems MPU9250"@1.0.2+ arduino-cli lib install "FlixPeriph"arduino-cli lib install "MAVLink"@2.0.1touch .dependenciesdiff --git a/docs/build.md b/docs/build.mdindex 34b3319..12e1bfd 100644--- a/docs/build.md+++ b/docs/build.md@@ -83,7 +83,7 @@ Use USB remote control or QGroundControl mobile app (with *Virtual Joystick* set2. Install ESP32 core using [Boards Manager](https://docs.arduino.cc/learn/starting-guide/cores).3. Install the following libraries using [Library Manager](https://docs.arduino.cc/software/ide-v2/tutorials/ide-v2-installing-a-library) (**versions are significant**):* `Bolder Flight Systems SBUS`, version 1.0.1.- * `Bolder Flight Systems MPU9250`, version 1.0.2.+ * `FlixPeriph`, version 1.0.0.* `MAVLink`, version 2.0.1.4. Clone the project using git or [download the source code as a ZIP archive](https://codeload.github.com/okalachev/flix/zip/refs/heads/master).5. Open the downloaded Arduino sketch `flix/flix.ino` in Arduino IDE.diff --git a/flix/flix.ino b/flix/flix.inoindex e01da35..5e4358b 100644--- a/flix/flix.ino+++ b/flix/flix.ino@@ -51,8 +51,7 @@ void setup() {}void loop() {- if (!readIMU()) return;-+ readIMU();step();readRC();estimate();diff --git a/flix/imu.ino b/flix/imu.inoindex f5754ff..5576e80 100644--- a/flix/imu.ino+++ b/flix/imu.ino@@ -6,86 +6,116 @@#include <SPI.h>#include <MPU9250.h>-#define LOAD_GYRO_CAL false+#define ONE_G 9.80665-MPU9250 IMU(SPI, SS);+// NOTE: use 'ca' command to calibrate the accelerometer and put the values here+Vector accBias(0, 0, 0);+Vector accScale(1, 1, 1);-void setupIMU() {- Serial.println("Setup IMU, stand still");+MPU9250 IMU(SPI, 4);+Vector gyroBias;- auto status = IMU.begin();- if (status < 0) {+void setupIMU() {+ Serial.println("Setup IMU");+ bool status = IMU.begin();+ if (!status) {while (true) {- Serial.printf("IMU begin error: %d\n", status);+ Serial.println("IMU begin error");delay(1000);}}+ calibrateGyro();+}- if (LOAD_GYRO_CAL) loadGyroCal();- loadAccelCal();-+void configureIMU() {+ IMU.setAccelRange(IMU.ACCEL_RANGE_4G);+ IMU.setGyroRange(IMU.GYRO_RANGE_500DPS);+ IMU.setDlpfBandwidth(IMU.DLPF_BANDWIDTH_184HZ);IMU.setSrd(0); // set sample rate to 1000 Hz- // NOTE: very important, without the above the rate would be terrible 50 Hz}-bool readIMU() {- if (IMU.readSensor() < 0) {- Serial.println("IMU read error");- return false;- }-- auto lastRates = rates;-- rates.x = IMU.getGyroX_rads();- rates.y = IMU.getGyroY_rads();- rates.z = IMU.getGyroZ_rads();- acc.x = IMU.getAccelX_mss();- acc.y = IMU.getAccelY_mss();- acc.z = IMU.getAccelZ_mss();-- return rates != lastRates;+void readIMU() {+ IMU.waitForData();+ IMU.getGyro(rates.x, rates.y, rates.z);+ IMU.getAccel(acc.x, acc.y, acc.z);+ // apply scale and bias+ acc = (acc - accBias) / accScale;+ rates = rates - gyroBias;}void calibrateGyro() {+ const int samples = 1000;Serial.println("Calibrating gyro, stand still");- int status = IMU.calibrateGyro();- Serial.printf("Calibration status: %d\n", status);- IMU.setSrd(0);+ IMU.setGyroRange(IMU.GYRO_RANGE_250DPS); // the most sensitive mode++ gyroBias = Vector(0, 0, 0);+ for (int i = 0; i < samples; i++) {+ IMU.waitForData();+ IMU.getGyro(rates.x, rates.y, rates.z);+ gyroBias = gyroBias + rates;+ }+ gyroBias = gyroBias / samples;+printIMUCal();+ configureIMU();}void calibrateAccel() {- Serial.println("Cal accel: place level"); delay(3000);- IMU.calibrateAccel();- Serial.println("Cal accel: place nose up"); delay(3000);- IMU.calibrateAccel();- Serial.println("Cal accel: place nose down"); delay(3000);- IMU.calibrateAccel();- Serial.println("Cal accel: place on right side"); delay(3000);- IMU.calibrateAccel();- Serial.println("Cal accel: place on left side"); delay(3000);- IMU.calibrateAccel();- Serial.println("Cal accel: upside down"); delay(3000);- IMU.calibrateAccel();+ Serial.println("Calibrating accelerometer");+ IMU.setAccelRange(IMU.ACCEL_RANGE_2G);+ IMU.setDlpfBandwidth(IMU.DLPF_BANDWIDTH_20HZ);+ IMU.setSrd(19);+ Serial.setTimeout(60000);+ Serial.print("Place level [enter] "); Serial.readStringUntil('\n');+ calibrateAccelOnce();+ Serial.print("Place nose up [enter] "); Serial.readStringUntil('\n');+ calibrateAccelOnce();+ Serial.print("Place nose down [enter] "); Serial.readStringUntil('\n');+ calibrateAccelOnce();+ Serial.print("Place on right side [enter] "); Serial.readStringUntil('\n');+ calibrateAccelOnce();+ Serial.print("Place on left side [enter] "); Serial.readStringUntil('\n');+ calibrateAccelOnce();+ Serial.print("Place upside down [enter] "); Serial.readStringUntil('\n');+ calibrateAccelOnce();printIMUCal();+ IMU.setAccelRange(IMU.ACCEL_RANGE_16G);+ IMU.setDlpfBandwidth(IMU.DLPF_BANDWIDTH_184HZ);+ IMU.setSrd(0);}-void loadAccelCal() {- // NOTE: this should be changed to the actual values- IMU.setAccelCalX(-0.0048542023, 1.0008112192);- IMU.setAccelCalY(0.0521845818, 0.9985780716);- IMU.setAccelCalZ(0.5754694939, 1.0045746565);-}--void loadGyroCal() {- // NOTE: this should be changed to the actual values- IMU.setGyroBiasX_rads(-0.0185128022);- IMU.setGyroBiasY_rads(-0.0262369743);- IMU.setGyroBiasZ_rads(0.0163032326);+void calibrateAccelOnce() {+ const int samples = 100;+ static Vector accMax(-INFINITY, -INFINITY, -INFINITY);+ static Vector accMin(INFINITY, INFINITY, INFINITY);++ // Compute the average of the accelerometer readings+ acc = Vector(0, 0, 0);+ for (int i = 0; i < samples; i++) {+ IMU.waitForData();+ Vector sample;+ IMU.getAccel(sample.x, sample.y, sample.z);+ acc = acc + sample;+ }+ acc = acc / samples;++ // Update the maximum and minimum values+ if (acc.x > accMax.x) accMax.x = acc.x;+ if (acc.y > accMax.y) accMax.y = acc.y;+ if (acc.z > accMax.z) accMax.z = acc.z;+ if (acc.x < accMin.x) accMin.x = acc.x;+ if (acc.y < accMin.y) accMin.y = acc.y;+ if (acc.z < accMin.z) accMin.z = acc.z;+ Serial.printf("acc %f %f %f\n", acc.x, acc.y, acc.z);+ Serial.printf("max %f %f %f\n", accMax.x, accMax.y, accMax.z);+ Serial.printf("min %f %f %f\n", accMin.x, accMin.y, accMin.z);+ // Compute scale and bias+ accScale = (accMax - accMin) / 2 / ONE_G;+ accBias = (accMax + accMin) / 2;}void printIMUCal() {- Serial.printf("gyro bias: %f %f %f\n", IMU.getGyroBiasX_rads(), IMU.getGyroBiasY_rads(), IMU.getGyroBiasZ_rads());- Serial.printf("accel bias: %f %f %f\n", IMU.getAccelBiasX_mss(), IMU.getAccelBiasY_mss(), IMU.getAccelBiasZ_mss());- Serial.printf("accel scale: %f %f %f\n", IMU.getAccelScaleFactorX(), IMU.getAccelScaleFactorY(), IMU.getAccelScaleFactorZ());+ Serial.printf("gyro bias: %f %f %f\n", gyroBias.x, gyroBias.y, gyroBias.z);+ Serial.printf("accel bias: %f %f %f\n", accBias.x, accBias.y, accBias.z);+ Serial.printf("accel scale: %f %f %f\n", accScale.x, accScale.y, accScale.z);}diff --git a/flix/vector.h b/flix/vector.hindex 2f25e42..7602ac5 100644--- a/flix/vector.h+++ b/flix/vector.h@@ -44,6 +44,16 @@ public:return Vector(x - b.x, y - b.y, z - b.z);}+ // Element-wise multiplication+ Vector operator * (const Vector& b) const {+ return Vector(x * b.x, y * b.y, z * b.z);+ }++ // Element-wise division+ Vector operator / (const Vector& b) const {+ return Vector(x / b.x, y / b.y, z / b.z);+ }+inline bool operator == (const Vector& b) const {return x == b.x && y == b.y && z == b.z;}