Skip to content

armgit / flix

Public Imported from GitHub
0

Making an ESP32-based quadcopter from scratch

Configure imu using parameters instead of editing the source code Choose the model and optionally custom pin numbers.

Oleg Kalachevf79c444f0774
diff --git a/docs/usage.md b/docs/usage.md
index 5e7a0a8..c6914c3 100644
--- a/docs/usage.md
+++ b/docs/usage.md
@@ -71,15 +71,6 @@ See other available Make commands in [Makefile](../Makefile).
## Before first flight
-### Choose the IMU model
-
-In case if using different IMU model than MPU9250, change `imu` variable declaration in the `imu.ino`:
-
-```cpp
-ICM20948 imu(SPI); // For ICM-20948
-MPU6050 imu(Wire); // For MPU-6050
-```
-
### Connect using QGroundControl
QGroundControl is a ground control station software that can be used to monitor and control the drone.
@@ -120,6 +111,17 @@ The drone is configured using parameters. To access and modify them, go to the Q
You can also work with parameters using `p` command in the console. Parameter names are case-insensitive.
+### Configure the IMU
+
+1. Configure the following parameters for the IMU:
+ * `IMU_MODEL` — IMU model (1 for MPU-9250/MPU-6500, 2 for ICM-20948, 3 for MPU-6050, 4 for ICM-40609-D).
+ * `IMU_BUS` — communication bus (0 for SPI, 1 for I²C).
+ * `IMU_PIN_SCK`, `IMU_PIN_MISO`, `IMU_PIN_MOSI`, `IMU_PIN_CS` — SPI pin numbers.
+ * `IMU_PIN_SCL`, `IMU_PIN_SDA` — I²C pin numbers.
+ * `IMU_PIN_INT` — IMU data ready pin number (-1 if not used).
+2. Reboot the drone.
+3. Check the IMU is working using `imu` command in the console (should print `status: OK`).
+
### Define IMU orientation
The IMU orientation (relative to the drone's axes) is defined using the parameters: `IMU_ROT_ROLL`, `IMU_ROT_PITCH`, and `IMU_ROT_YAW`.
diff --git a/flix/imu.ino b/flix/imu.ino
index b6a84b5..0aafb13 100644
--- a/flix/imu.ino
+++ b/flix/imu.ino
@@ -4,12 +4,17 @@
// Work with the IMU sensor
#include <SPI.h>
+#include <Wire.h>
#include <FlixPeriph.h>
#include "vector.h"
#include "lpf.h"
#include "util.h"
-MPU9250 imu(SPI);
+IMU *imu;
+int imuModel = -1; // 1 - MPU9250, 2 - ICM20948, 3 - MPU6050, 4 - ICM40609D
+int imuBus = 0; // 0 - SPI, 1 - I2C
+int imuSckPin = SCK, imuMisoPin = MISO, imuMosiPin = MOSI, imuCsPin = SS, imuIntPin = -1;
+int imuSdaPin = SDA, imuSclPin = SCL;
Vector imuRotation(0, 0, PI / 2); // imu orientation as Euler angles
Vector gyro; // gyroscope output, rad/s
@@ -23,22 +28,35 @@ LowPassFilter<Vector> gyroBiasFilter(0.001);
void setupIMU() {
print("Setup IMU\n");
- imu.begin();
+ free(imu);
+ if (imuModel == 3) imuBus = 1; // MPU6050 is I2C only
+
+ if (imuBus == 0) {
+ // SPI connection
+ SPI.begin(imuSckPin, imuMisoPin, imuMosiPin);
+ imu = IMU::create(imuModel, SPI, imuCsPin, imuIntPin);
+ } else {
+ // I2C connection
+ Wire.setPins(imuSdaPin, imuSclPin);
+ imu = IMU::create(imuModel, Wire, imuIntPin);
+ }
+
+ imu->begin();
configureIMU();
}
void configureIMU() {
- imu.setAccelRange(imu.ACCEL_RANGE_4G);
- imu.setGyroRange(imu.GYRO_RANGE_2000DPS);
- imu.setDLPF(imu.DLPF_MAX);
- imu.setRate(imu.RATE_1KHZ_APPROX);
- imu.setupInterrupt();
+ imu->setAccelRange(IMU::ACCEL_RANGE_4G);
+ imu->setGyroRange(IMU::GYRO_RANGE_2000DPS);
+ imu->setDLPF(IMU::DLPF_MAX);
+ imu->setRate(IMU::RATE_1KHZ_APPROX);
+ imu->setupInterrupt();
}
void readIMU() {
- imu.waitForData();
- imu.getGyro(gyro.x, gyro.y, gyro.z);
- imu.getAccel(acc.x, acc.y, acc.z);
+ imu->waitForData();
+ imu->getGyro(gyro.x, gyro.y, gyro.z);
+ imu->getAccel(acc.x, acc.y, acc.z);
calibrateGyroOnce();
// Apply scale and bias
@@ -60,7 +78,7 @@ void calibrateGyroOnce() {
void calibrateAccel() {
print("Calibrating accelerometer\n");
- imu.setAccelRange(imu.ACCEL_RANGE_2G); // the most sensitive mode
+ imu->setAccelRange(IMU::ACCEL_RANGE_2G); // the most sensitive mode
print("1/6 Place level [8 sec]\n");
pause(8);
@@ -94,9 +112,9 @@ void calibrateAccelOnce() {
// Compute the average of the accelerometer readings
acc = Vector(0, 0, 0);
for (int i = 0; i < samples; i++) {
- imu.waitForData();
+ imu->waitForData();
Vector sample;
- imu.getAccel(sample.x, sample.y, sample.z);
+ imu->getAccel(sample.x, sample.y, sample.z);
acc = acc + sample;
}
acc = acc / samples;
@@ -121,17 +139,18 @@ void printIMUCalibration() {
}
void printIMUInfo() {
- imu.status() ? print("status: ERROR %d\n", imu.status()) : print("status: OK\n");
- print("model: %s\n", imu.getModel());
- print("who am I: 0x%02X\n", imu.whoAmI());
+ imu->status() ? print("status: ERROR %d\n", imu->status()) : print("status: OK\n");
+ print("model: %s\n", imu->getModel());
+ print("who am I: 0x%02X\n", imu->whoAmI());
print("rate: %.0f\n", loopRate);
- print("temperature: %.1f °C\n", imu.getTemp());
+ print("interrupt mode: %s\n", imuIntPin != -1 ? "pin" : "timer");
+ print("temperature: %.1f °C\n", imu->getTemp());
print("gyro: %f %f %f\n", gyro.x, gyro.y, gyro.z);
print("acc: %f %f %f\n", acc.x, acc.y, acc.z);
- imu.waitForData();
+ imu->waitForData();
Vector rawGyro, rawAcc;
- imu.getGyro(rawGyro.x, rawGyro.y, rawGyro.z);
- imu.getAccel(rawAcc.x, rawAcc.y, rawAcc.z);
+ imu->getGyro(rawGyro.x, rawGyro.y, rawGyro.z);
+ imu->getAccel(rawAcc.x, rawAcc.y, rawAcc.z);
print("raw gyro: %f %f %f\n", rawGyro.x, rawGyro.y, rawGyro.z);
print("raw acc: %f %f %f\n", rawAcc.x, rawAcc.y, rawAcc.z);
}
diff --git a/flix/parameters.ino b/flix/parameters.ino
index acac658..daed501 100644
--- a/flix/parameters.ino
+++ b/flix/parameters.ino
@@ -61,6 +61,15 @@ Parameter parameters[] = {
{"CTL_FLT_MODE_1", &flightModes[1]},
{"CTL_FLT_MODE_2", &flightModes[2]},
// imu
+ {"IMU_MODEL", &imuModel},
+ {"IMU_BUS", &imuBus},
+ {"IMU_PIN_SCK", &imuSckPin},
+ {"IMU_PIN_MISO", &imuMisoPin},
+ {"IMU_PIN_MOSI", &imuMosiPin},
+ {"IMU_PIN_CS", &imuCsPin},
+ {"IMU_PIN_SDA", &imuSdaPin},
+ {"IMU_PIN_SCL", &imuSclPin},
+ {"IMU_PIN_INT", &imuIntPin},
{"IMU_ROT_ROLL", &imuRotation.x},
{"IMU_ROT_PITCH", &imuRotation.y},
{"IMU_ROT_YAW", &imuRotation.z},
diff --git a/gazebo/flix.h b/gazebo/flix.h
index 1d88343..62593e7 100644
--- a/gazebo/flix.h
+++ b/gazebo/flix.h
@@ -21,6 +21,9 @@ extern float motors[4];
Vector gyro, acc, imuRotation;
Vector accBias, gyroBias, accScale(1, 1, 1);
LowPassFilter<Vector> gyroBiasFilter(0);
+int imuModel = 1, imuBus = 0;
+int imuSckPin = 0, imuMisoPin = 0, imuMosiPin = 0, imuCsPin = -1, imuIntPin = -1;
+int imuSdaPin = 0, imuSclPin = 0;
// declarations
void step();