|
@@ -0,0 +1,277 @@
|
|
|
+#include "imu.h"
|
|
|
+
|
|
|
+//Storage
|
|
|
+
|
|
|
+struct Calib {
|
|
|
+ float magoffsetX;
|
|
|
+ float magoffsetY;
|
|
|
+ float magoffsetZ;
|
|
|
+ float magscaleX;
|
|
|
+ float magscaleY;
|
|
|
+ float magscaleZ;
|
|
|
+};
|
|
|
+const char *calibFile = "/calib.txt"; // <- SD library uses 8.3 filenames
|
|
|
+Calib calib;
|
|
|
+
|
|
|
+#define MADGWICK
|
|
|
+//#define MAHONY
|
|
|
+
|
|
|
+BMM150class bmm150;
|
|
|
+
|
|
|
+float accX = 0.0F;
|
|
|
+float accY = 0.0F;
|
|
|
+float accZ = 0.0F;
|
|
|
+
|
|
|
+float gyroX = 0.0F;
|
|
|
+float gyroY = 0.0F;
|
|
|
+float gyroZ = 0.0F;
|
|
|
+
|
|
|
+int AVERAGENUM_GY = 256;
|
|
|
+float init_gyroX = 0.0F;
|
|
|
+float init_gyroY = 0.0F;
|
|
|
+float init_gyroZ = 0.0F;
|
|
|
+
|
|
|
+float magnetX = 0.0F;
|
|
|
+float magnetY = 0.0F;
|
|
|
+float magnetZ = 0.0F;
|
|
|
+
|
|
|
+//for hard iron correction
|
|
|
+float magoffsetX = 0.0F;
|
|
|
+float magoffsetY = 0.0F;
|
|
|
+float magoffsetZ = 0.0F;
|
|
|
+
|
|
|
+//for soft iron correction
|
|
|
+float magscaleX = 0.0F;
|
|
|
+float magscaleY = 0.0F;
|
|
|
+float magscaleZ = 0.0F;
|
|
|
+
|
|
|
+float pitch = 0.0F;
|
|
|
+float roll = 0.0F;
|
|
|
+float yaw = 0.0F;
|
|
|
+
|
|
|
+float pitch_ahrs = 0.0F;
|
|
|
+float roll_ahrs = 0.0F;
|
|
|
+float yaw_ahrs = 0.0F;
|
|
|
+
|
|
|
+float temp = 0.0F;
|
|
|
+
|
|
|
+uint32_t Now = 0;
|
|
|
+uint32_t lastUpdate = 0;
|
|
|
+float deltat = 0.0f;
|
|
|
+
|
|
|
+//moved to global
|
|
|
+float magnetX1, magnetY1, magnetZ1;
|
|
|
+float head_dir;
|
|
|
+
|
|
|
+Smoothed<float> yaw_smooth;
|
|
|
+Smoothed<float> pitch_smooth;
|
|
|
+Smoothed<float> roll_smooth;
|
|
|
+
|
|
|
+void loadCalibration(const char *calibFile, Calib &calib) {
|
|
|
+ // Open file for reading
|
|
|
+ SPIFFS.begin();
|
|
|
+ File file = SPIFFS.open(calibFile);
|
|
|
+
|
|
|
+ // Allocate a temporary JsonDocument
|
|
|
+ // Don't forget to change the capacity to match your requirements.
|
|
|
+ // Use arduinojson.org/v6/assistant to compute the capacity.
|
|
|
+ StaticJsonDocument<512> doc;
|
|
|
+
|
|
|
+ // Deserialize the JSON document
|
|
|
+ DeserializationError error = deserializeJson(doc, file);
|
|
|
+ if (error){Serial.println(F("Failed to read file"));}
|
|
|
+
|
|
|
+ else{
|
|
|
+ // Copy values from the JsonDocument to the Config
|
|
|
+ calib.magoffsetX = doc["magoffsetX"];
|
|
|
+ calib.magoffsetY = doc["magoffsetY"];
|
|
|
+ calib.magoffsetZ = doc["magoffsetZ"];
|
|
|
+ calib.magscaleX = doc["magscaleX"];
|
|
|
+ calib.magscaleY = doc["magscaleY"];
|
|
|
+ calib.magscaleZ = doc["magscaleZ"];
|
|
|
+
|
|
|
+
|
|
|
+ // Close the file (Curiously, File's destructor doesn't close the file)
|
|
|
+ file.close();
|
|
|
+ }
|
|
|
+ SPIFFS.end();
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+// Saves the configuration to a file
|
|
|
+void saveCalibration(const char *calibFile, const Calib &calib) {
|
|
|
+ // Delete existing file, otherwise the configuration is appended to the file
|
|
|
+ SPIFFS.begin();
|
|
|
+ SPIFFS.remove(calibFile);
|
|
|
+
|
|
|
+ // Open file for writing
|
|
|
+ File file = SPIFFS.open(calibFile, FILE_WRITE);
|
|
|
+ if (!file) {
|
|
|
+ Serial.println(F("Failed to create file"));
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ // Allocate a temporary JsonDocument
|
|
|
+ // Don't forget to change the capacity to match your requirements.
|
|
|
+ // Use arduinojson.org/assistant to compute the capacity.
|
|
|
+ StaticJsonDocument<256> doc;
|
|
|
+
|
|
|
+ // Set the values in the document
|
|
|
+ doc["magoffsetX"] = calib.magoffsetX;
|
|
|
+ doc["magoffsetY"] = calib.magoffsetY;
|
|
|
+ doc["magoffsetZ"] = calib.magoffsetZ;
|
|
|
+ doc["magscaleX"] = calib.magscaleX;
|
|
|
+ doc["magscaleY"] = calib.magscaleY;
|
|
|
+ doc["magscaleZ"] = calib.magscaleZ;
|
|
|
+
|
|
|
+
|
|
|
+ // Serialize JSON to file
|
|
|
+ if (serializeJson(doc, file) == 0) {
|
|
|
+ Serial.println(F("Failed to write to file"));
|
|
|
+ }
|
|
|
+
|
|
|
+ // Close the file
|
|
|
+ file.close();
|
|
|
+ SPIFFS.end();
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+void initGyro()
|
|
|
+{
|
|
|
+ M5.Lcd.fillScreen(BLACK);
|
|
|
+ M5.Lcd.setTextColor(WHITE);
|
|
|
+ M5.Lcd.setTextSize(1);
|
|
|
+ M5.Lcd.setCursor(0, 0);
|
|
|
+ M5.Lcd.print("begin gyro calibration");
|
|
|
+
|
|
|
+ for (int i = 0; i < AVERAGENUM_GY; i++)
|
|
|
+ {
|
|
|
+ M5.IMU.getGyroData(&gyroX, &gyroY, &gyroZ);
|
|
|
+ init_gyroX += gyroX;
|
|
|
+ init_gyroY += gyroY;
|
|
|
+ init_gyroZ += gyroZ;
|
|
|
+ delay(5);
|
|
|
+ }
|
|
|
+ init_gyroX /= AVERAGENUM_GY;
|
|
|
+ init_gyroY /= AVERAGENUM_GY;
|
|
|
+ init_gyroZ /= AVERAGENUM_GY;
|
|
|
+ loadCalibration(calibFile, calib);
|
|
|
+
|
|
|
+ magoffsetX = calib.magoffsetX;
|
|
|
+ magoffsetY = calib.magoffsetY;
|
|
|
+ magoffsetZ = calib.magoffsetZ;
|
|
|
+ magscaleX = calib.magscaleX;
|
|
|
+ magscaleY = calib.magscaleY;
|
|
|
+ magscaleZ = calib.magscaleZ;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+void calibGyro()
|
|
|
+{
|
|
|
+ M5.Lcd.fillScreen(BLACK);
|
|
|
+ M5.Lcd.setTextColor(WHITE);
|
|
|
+ M5.Lcd.setTextSize(1);
|
|
|
+ M5.Lcd.setCursor(0, 0);
|
|
|
+ M5.Lcd.print("begin calibration in 3 seconds");
|
|
|
+ delay(3000);
|
|
|
+ M5.Lcd.setCursor(0, 10);
|
|
|
+ M5.Lcd.print("Flip + rotate core calibration");
|
|
|
+ bmm150.bmm150_calibrate(15000);
|
|
|
+ delay(100);
|
|
|
+
|
|
|
+ bmm150.Init();
|
|
|
+ bmm150.getMagnetOffset(&magoffsetX, &magoffsetY, &magoffsetZ);
|
|
|
+ bmm150.getMagnetScale(&magscaleX, &magscaleY, &magscaleZ);
|
|
|
+
|
|
|
+ calib.magoffsetX = magoffsetX;
|
|
|
+ calib.magoffsetY = magoffsetY;
|
|
|
+ calib.magoffsetZ = magoffsetZ;
|
|
|
+ calib.magscaleX = magscaleX;
|
|
|
+ calib.magscaleY = magscaleY;
|
|
|
+ calib.magscaleZ = magscaleZ;
|
|
|
+
|
|
|
+ saveCalibration(calibFile, calib);
|
|
|
+
|
|
|
+ M5.Lcd.fillScreen(BLACK);
|
|
|
+ M5.Lcd.setTextColor(GREEN, BLACK);
|
|
|
+ M5.Lcd.setTextSize(2);
|
|
|
+}
|
|
|
+
|
|
|
+void setup_imu()
|
|
|
+{
|
|
|
+ yaw_smooth.begin(SMOOTHED_AVERAGE, 100);
|
|
|
+ pitch_smooth.begin(SMOOTHED_EXPONENTIAL, 10);
|
|
|
+ roll_smooth.begin(SMOOTHED_EXPONENTIAL, 10);
|
|
|
+
|
|
|
+ M5.IMU.Init();
|
|
|
+ initGyro();
|
|
|
+ bmm150.Init();
|
|
|
+
|
|
|
+ bmm150.getMagnetOffset(&magoffsetX, &magoffsetY, &magoffsetZ);
|
|
|
+ bmm150.getMagnetScale(&magscaleX, &magscaleY, &magscaleZ);
|
|
|
+}
|
|
|
+
|
|
|
+void handle_imu()
|
|
|
+{
|
|
|
+
|
|
|
+ M5.IMU.getGyroData(&gyroX, &gyroY, &gyroZ);
|
|
|
+ gyroX -= init_gyroX;
|
|
|
+ gyroY -= init_gyroY;
|
|
|
+ gyroZ -= init_gyroZ;
|
|
|
+ M5.IMU.getAccelData(&accX, &accY, &accZ);
|
|
|
+ M5.IMU.getAhrsData(&pitch_ahrs, &roll_ahrs, &yaw_ahrs);
|
|
|
+ M5.IMU.getTempData(&temp);
|
|
|
+ bmm150.getMagnetData(&magnetX, &magnetY, &magnetZ);
|
|
|
+
|
|
|
+ magnetX1 = (magnetX - magoffsetX) * magscaleX;
|
|
|
+ magnetY1 = (magnetY - magoffsetY) * magscaleY;
|
|
|
+ magnetZ1 = (magnetZ - magoffsetZ) * magscaleZ;
|
|
|
+
|
|
|
+ head_dir = atan2(magnetX1, magnetY1);
|
|
|
+ if (head_dir < 0)
|
|
|
+ head_dir += 2 * M_PI;
|
|
|
+ if (head_dir > 2 * M_PI)
|
|
|
+ head_dir -= 2 * M_PI;
|
|
|
+ head_dir *= RAD_TO_DEG;
|
|
|
+ Now = micros();
|
|
|
+ deltat = ((Now - lastUpdate) / 1000000.0f); //0.09
|
|
|
+ lastUpdate = Now;
|
|
|
+
|
|
|
+#ifdef MADGWICK
|
|
|
+ MadgwickQuaternionUpdate(accX, accY, accZ, gyroX * DEG_TO_RAD,
|
|
|
+ gyroY * DEG_TO_RAD, gyroZ * DEG_TO_RAD,
|
|
|
+ -magnetX1, magnetY1, -magnetZ1, deltat);
|
|
|
+#endif
|
|
|
+
|
|
|
+#ifdef MAHONY
|
|
|
+ MahonyQuaternionUpdate(accX, accY, accZ, gyroX * DEG_TO_RAD,
|
|
|
+ gyroY * DEG_TO_RAD, gyroZ * DEG_TO_RAD,
|
|
|
+ -magnetX1, magnetY1, -magnetZ1, deltat);
|
|
|
+ //delay(10); // adjust sampleFreq = 50Hz
|
|
|
+#endif
|
|
|
+
|
|
|
+ yaw = atan2(2.0f * (*(getQ() + 1) * *(getQ() + 2) + *getQ() *
|
|
|
+ *(getQ() + 3)),
|
|
|
+ *getQ() * *getQ() + *(getQ() + 1) * *(getQ() + 1) - *(getQ() + 2) * *(getQ() + 2) - *(getQ() + 3) * *(getQ() + 3));
|
|
|
+ pitch = -asin(2.0f * (*(getQ() + 1) * *(getQ() + 3) - *getQ() *
|
|
|
+ *(getQ() + 2)));
|
|
|
+ roll = atan2(2.0f * (*getQ() * *(getQ() + 1) + *(getQ() + 2) *
|
|
|
+ *(getQ() + 3)),
|
|
|
+ *getQ() * *getQ() - *(getQ() + 1) * *(getQ() + 1) - *(getQ() + 2) * *(getQ() + 2) + *(getQ() + 3) * *(getQ() + 3));
|
|
|
+ yaw = -0.5 * M_PI - yaw;
|
|
|
+ if (yaw < 0)
|
|
|
+ yaw += 2 * M_PI;
|
|
|
+ if (yaw > 2 * M_PI)
|
|
|
+ yaw -= 2 * M_PI;
|
|
|
+ pitch *= RAD_TO_DEG;
|
|
|
+ yaw *= RAD_TO_DEG;
|
|
|
+ roll *= RAD_TO_DEG;
|
|
|
+
|
|
|
+ yaw_smooth.add(yaw);
|
|
|
+ pitch_smooth.add(pitch);
|
|
|
+ roll_smooth.add(roll);
|
|
|
+
|
|
|
+
|
|
|
+}
|