Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
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.mdindex 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 QGroundControlQGroundControl 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 QYou 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 orientationThe 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.inoindex 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 anglesVector 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 modeprint("1/6 Place level [8 sec]\n");pause(8);@@ -94,9 +112,9 @@ void calibrateAccelOnce() {// Compute the average of the accelerometer readingsacc = 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.inoindex 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.hindex 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;// declarationsvoid step();