🎍 Updates feature flags and adds BNO055

esp32/lib/ESP32-sveltekit/peripherals/imu.h

@@ -6,11 +6,22 @@
 #include <ArduinoJson.h>
 #include <utils/math_utils.h>
 
+#if FT_ENABLED(USE_MPU6050)
 #include <MPU6050_6Axis_MotionApps612.h>
+#endif
+
+#if FT_ENABLED(USE_BNO055)
+#include <Adafruit_BNO055.h>
+#endif
 
 class IMU {
   public:
-    IMU() {}
+    IMU()
+#if FT_ENABLED(USE_BNO055)
+        : _imu(55, 0x29)
+#endif
+    {
+    }
     bool initialize() {
 #if FT_ENABLED(USE_MPU6050)
         _imu.initialize();
@@ -21,6 +32,13 @@ class IMU {
         _imu.setI2CMasterModeEnabled(false);
         _imu.setI2CBypassEnabled(true);
         _imu.setSleepEnabled(false);
+#endif
+#if FT_ENABLED(USE_BNO055)
+        imu_success = _imu.begin();
+        if (!imu_success) {
+            return false;
+        }
+        _imu.setExtCrystalUse(true);
 #endif
         return true;
     }
@@ -32,17 +50,28 @@ class IMU {
         _imu.dmpGetQuaternion(&q, fifoBuffer);
         _imu.dmpGetGravity(&gravity, &q);
         _imu.dmpGetYawPitchRoll(ypr, &q, &gravity);
+        ypr[0] *= 180 / M_PI;
+        ypr[1] *= 180 / M_PI;
+        ypr[2] *= 180 / M_PI;
         return updated;
 #endif
+#if FT_ENABLED(USE_BNO055)
+        sensors_event_t event;
+        _imu.getEvent(&event);
+        ypr[0] = (float)event.orientation.x;
+        ypr[1] = (float)event.orientation.y;
+        ypr[2] = (float)event.orientation.z;
+#endif
+        return true;
     }
 
     float getTemperature() { return imu_success ? imu_temperature : -1; }
 
-    float getAngleX() { return imu_success ? ypr[0] * 180 / M_PI : 0; }
+    float getAngleX() { return imu_success ? ypr[0] : 0; }
 
-    float getAngleY() { return imu_success ? ypr[1] * 180 / M_PI : 0; }
+    float getAngleY() { return imu_success ? ypr[1] : 0; }
 
-    float getAngleZ() { return imu_success ? ypr[2] * 180 / M_PI : 0; }
+    float getAngleZ() { return imu_success ? ypr[2] : 0; }
 
     void readIMU(JsonObject& root) {
         if (!imu_success) return;
@@ -60,6 +89,9 @@ class IMU {
     Quaternion q;
     uint8_t fifoBuffer[64];
     VectorFloat gravity;
+#endif
+#if FT_ENABLED(USE_BNO055)
+    Adafruit_BNO055 _imu;
 #endif
     bool imu_success {false};
     float ypr[3];

esp32/lib/ESP32-sveltekit/peripherals/peripherals.h

@@ -67,13 +67,13 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
 
         updatePins();
 
-#if FT_ENABLED(USE_MPU6050)
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
         if (!_imu.initialize()) ESP_LOGE("IMUService", "IMU initialize failed");
 #endif
-#if FT_ENABLED(USE_MAG)
+#if FT_ENABLED(USE_HMC5883)
         if (!_mag.initialize()) ESP_LOGE("IMUService", "MAG initialize failed");
 #endif
-#if FT_ENABLED(USE_BMP)
+#if FT_ENABLED(USE_BMP180)
         if (!_bmp.initialize()) ESP_LOGE("IMUService", "BMP initialize failed");
 #endif
 #if FT_ENABLED(USE_USS)
@@ -137,7 +137,7 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
     /* IMU FUNCTIONS */
     bool readIMU() {
         bool updated = false;
-#if FT_ENABLED(USE_MPU6050)
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
         beginTransaction();
         updated = _imu.readIMU();
         endTransaction();
@@ -147,7 +147,7 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
 
     bool readMag() {
         bool updated = false;
-#if FT_ENABLED(USE_MAG)
+#if FT_ENABLED(USE_HMC5883)
         beginTransaction();
         updated = _mag.readMagnetometer();
         endTransaction();
@@ -157,7 +157,7 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
 
     bool readBMP() {
         bool updated = false;
-#if FT_ENABLED(USE_BMP)
+#if FT_ENABLED(USE_BMP180)
         beginTransaction();
         updated = _bmp.readBarometer();
         endTransaction();
@@ -181,13 +181,13 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
     void emitIMU() {
         doc.clear();
         JsonObject root = doc.to<JsonObject>();
-#if FT_ENABLED(USE_MPU6050)
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
         _imu.readIMU(root);
 #endif
-#if FT_ENABLED(USE_MAG)
+#if FT_ENABLED(USE_HMC5883)
         _mag.readMagnetometer(root);
 #endif
-#if FT_ENABLED(USE_BMP)
+#if FT_ENABLED(USE_BMP180)
         _bmp.readBarometer(root);
 #endif
         serializeJson(doc, message);
@@ -196,7 +196,6 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
 
     void emitSonar() {
 #if FT_ENABLED(USE_USS)
-
         char output[16];
         snprintf(output, sizeof(output), "[%.1f,%.1f]", _left_distance, _right_distance);
         socket.emit("sonar", output);
@@ -214,13 +213,13 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
 
     JsonDocument doc;
     char message[MAX_ESP_IMU_SIZE];
-#if FT_ENABLED(USE_MPU6050)
+#if FT_ENABLED(USE_MPU6050 || USE_BNO055)
     IMU _imu;
 #endif
-#if FT_ENABLED(USE_MAG)
+#if FT_ENABLED(USE_HMC5883)
     Magnetometer _mag;
 #endif
-#if FT_ENABLED(USE_BMP)
+#if FT_ENABLED(USE_BMP180)
     Barometer _bmp;
 #endif
 #if FT_ENABLED(USE_USS)

esp32/lib/ESP32-sveltekit/peripherals/servo_controller.h

@@ -111,7 +111,6 @@ class ServoController : public StatefulService<ServoSettings> {
             float angle = servo.direction * angles[i] + servo.centerAngle;
             uint16_t pwm = angle * servo.conversion + servo.centerPwm;
             if (pwm < 125 || pwm > 600) {
-                ESP_LOGE("ServoController", "Servo %d, Invalid PWM value %d", i, pwm);
                 continue;
             }
             pwms[i] = pwm;