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:esp32@2.0.11 --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 #include -#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); + +MPU9250 IMU(SPI, 4); +Vector gyroBias; void setupIMU() { - Serial.println("Setup IMU, stand still"); - - auto status = IMU.begin(); - if (status < 0) { + 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); } } - - if (LOAD_GYRO_CAL) loadGyroCal(); - loadAccelCal(); - - IMU.setSrd(0); // set sample rate to 1000 Hz - // NOTE: very important, without the above the rate would be terrible 50 Hz + calibrateGyro(); } -bool readIMU() { - if (IMU.readSensor() < 0) { - Serial.println("IMU read error"); - return false; - } +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 +} - 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 calibrateAccelOnce() { + const int samples = 100; + static Vector accMax(-INFINITY, -INFINITY, -INFINITY); + static Vector accMin(INFINITY, INFINITY, INFINITY); -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); + // 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; }