aleks/flix
1
0
Fork 0
mirror of https://github.com/okalachev/flix.git synced 2025-07-27 09:39:33 +00:00
Code Issues Packages Projects Releases Wiki Activity

Use FlixPeriph library for IMU, implement own IMU calibration

Browse Source
This commit is contained in:
Oleg Kalachev 2024-03-15 10:38:48 +03:00
parent d752cce0cc
commit 2cf1c7abb3
5 changed files with 100 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Makefile
View File

@ -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

2
docs/build.md
View File

@ -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.

3
flix/flix.ino
View File

@ -51,8 +51,7 @@ void setup() {
}
void loop() {
if (!readIMU()) return;
readIMU();
step();
readRC();
estimate();

144
flix/imu.ino
View File

@ -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);
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);
}

10
flix/vector.h
View File

@ -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;
}