Skip to content

armgit / flix

Public Imported from GitHub
0

Making an ESP32-based quadcopter from scratch

Use FlixPeriph library for IMU, implement own IMU calibration

Oleg Kalachev2cf1c7abb3d6
diff --git a/Makefile b/Makefile
index 58e5f7d..f1434a8 100644
--- a/Makefile
+++ b/Makefile
@@ -16,7 +16,7 @@ dependencies .dependencies:
arduino-cli core install esp32:[email protected] --config-file arduino-cli.yaml
arduino-cli lib update-index
arduino-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.1
touch .dependencies
diff --git a/docs/build.md b/docs/build.md
index 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* set
2. 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.ino
index 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.ino
index 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.h
index 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;
}