diff --git a/app/src/routes/peripherals/i2c/i2c.svelte b/app/src/routes/peripherals/i2c/i2c.svelte index 99522a9..ec42e28 100644 --- a/app/src/routes/peripherals/i2c/i2c.svelte +++ b/app/src/routes/peripherals/i2c/i2c.svelte @@ -8,6 +8,7 @@ const i2cDevices = [ { address: 30, part_number: 'HMC5883', name: '3-Axis Digital Compass/Magnetometer IC' }, + { address: 41, part_number: 'BNO055', name: '9-Axis Absolute Orientation Sensor' }, { address: 64, part_number: 'PCA9685', name: '16-channel PWM driver default address' }, { address: 72, part_number: 'ADS1115', name: '4-channel 16-bit ADC' }, { diff --git a/esp32/features.ini b/esp32/features.ini index b30ad57..cffea60 100644 --- a/esp32/features.ini +++ b/esp32/features.ini @@ -1,15 +1,16 @@ [features] build_flags = - -D USE_SLEEP=0 - -D USE_UPLOAD_FIRMWARE=1 + -D USE_SLEEP=1 + -D USE_UPLOAD_FIRMWARE=0 -D USE_DOWNLOAD_FIRMWARE=0 -D USE_MOTION=1 -D USE_MDNS=1 ; Hardware specific - -D USE_MAG=0 - -D USE_BMP=0 + -D USE_HMC5883=0 + -D USE_BMP180=0 -D USE_MPU6050=0 -D USE_WS2812=1 + -D USE_BNO055=1 -D USE_USS=0 - -D USE_SERVO=1 \ No newline at end of file + -D USE_PCA9685=1 \ No newline at end of file diff --git a/esp32/include/spot.h b/esp32/include/spot.h index b2b542b..f4c81fc 100644 --- a/esp32/include/spot.h +++ b/esp32/include/spot.h @@ -61,6 +61,7 @@ class Spot { // act void updateActuators() { if (updatedMotion) _servoController.setAngles(_motionService.getAngles()); + updatedMotion = false; _servoController.updateServoState(); #if FT_ENABLED(USE_WS2812) @@ -71,7 +72,7 @@ class Spot { // communicate void emitTelemetry() { if (updatedMotion) EXECUTE_EVERY_N_MS(100, { _motionService.emitAngles(); }); - EXECUTE_EVERY_N_MS(1000, { _peripherals.emitIMU(); }); + EXECUTE_EVERY_N_MS(250, { _peripherals.emitIMU(); }); // _peripherals.emitSonar(); } @@ -102,7 +103,7 @@ class Spot { bool updatedMotion = false; const char *_appName = APP_NAME; - const u_int16_t _numberEndpoints = 115; + const u_int16_t _numberEndpoints = 116; const u_int32_t _maxFileUpload = 2300000; // 2.3 MB const uint16_t _port = 80; @@ -110,7 +111,6 @@ class Spot { void loop(); static void _loopImpl(void *_this) { static_cast(_this)->loop(); } void setupServer(); - void setupMDNS(); void startServices(); }; diff --git a/esp32/lib/ESP32-sveltekit/features.cpp b/esp32/lib/ESP32-sveltekit/features.cpp index b10982b..1265fc6 100644 --- a/esp32/lib/ESP32-sveltekit/features.cpp +++ b/esp32/lib/ESP32-sveltekit/features.cpp @@ -15,14 +15,15 @@ void printFeatureConfiguration() { ESP_LOGI("Features", "USE_MOTION: %s", USE_MOTION ? "enabled" : "disabled"); // Sensors + ESP_LOGI("Features", "USE_BNO055: %s", USE_BNO055 ? "enabled" : "disabled"); ESP_LOGI("Features", "USE_MPU6050: %s", USE_MPU6050 ? "enabled" : "disabled"); - ESP_LOGI("Features", "USE_MAG: %s", USE_MAG ? "enabled" : "disabled"); - ESP_LOGI("Features", "USE_BMP: %s", USE_BMP ? "enabled" : "disabled"); + ESP_LOGI("Features", "USE_HMC5883: %s", USE_HMC5883 ? "enabled" : "disabled"); + ESP_LOGI("Features", "USE_BMP180: %s", USE_BMP180 ? "enabled" : "disabled"); ESP_LOGI("Features", "USE_USS: %s", USE_USS ? "enabled" : "disabled"); ESP_LOGI("Features", "USE_GPS: %s", USE_GPS ? "enabled" : "disabled"); // Peripherals - ESP_LOGI("Features", "USE_SERVO: %s", USE_SERVO ? "enabled" : "disabled"); + ESP_LOGI("Features", "USE_PCA9685: %s", USE_PCA9685 ? "enabled" : "disabled"); ESP_LOGI("Features", "USE_WS2812: %s", USE_WS2812 ? "enabled" : "disabled"); // Web services @@ -38,12 +39,12 @@ void features(JsonObject &root) { root["download_firmware"] = USE_DOWNLOAD_FIRMWARE; root["sleep"] = USE_SLEEP; root["camera"] = USE_CAMERA; - root["imu"] = USE_MPU6050; - root["mag"] = USE_MAG; - root["bmp"] = USE_BMP; + root["imu"] = USE_MPU6050 || USE_BNO055; + root["mag"] = USE_HMC5883 || USE_BNO055; + root["bmp"] = USE_BMP180; root["sonar"] = USE_USS; root["motion"] = USE_MOTION; - root["servo"] = USE_SERVO; + root["servo"] = USE_PCA9685; root["ws2812"] = USE_WS2812; root["mdns"] = USE_MDNS; root["embed_www"] = EMBED_WWW; diff --git a/esp32/lib/ESP32-sveltekit/features.h b/esp32/lib/ESP32-sveltekit/features.h index dd8d908..a98df9f 100644 --- a/esp32/lib/ESP32-sveltekit/features.h +++ b/esp32/lib/ESP32-sveltekit/features.h @@ -29,17 +29,22 @@ // ESP32 IMU on by default #ifndef USE_MPU6050 -#define USE_MPU6050 1 +#define USE_MPU6050 0 +#endif + +// ESP32 IMU on by default +#ifndef USE_BNO055 +#define USE_BNO055 1 #endif // ESP32 magnetometer on by default -#ifndef USE_MAG -#define USE_MAG 0 +#ifndef USE_HMC5883 +#define USE_HMC5883 0 #endif // ESP32 barometer off by default -#ifndef USE_BMP -#define USE_BMP 0 +#ifndef USE_BMP180 +#define USE_BMP180 0 #endif // ESP32 SONAR off by default @@ -47,6 +52,11 @@ #define USE_USS 0 #endif +// PCA9685 Servo controller on by default +#ifndef USE_PCA9685 +#define USE_PCA9685 1 +#endif + // ESP32 GPS off by default #ifndef USE_GPS #define USE_GPS 0 diff --git a/esp32/lib/ESP32-sveltekit/peripherals/imu.h b/esp32/lib/ESP32-sveltekit/peripherals/imu.h index 5d39be9..f4c6f9a 100644 --- a/esp32/lib/ESP32-sveltekit/peripherals/imu.h +++ b/esp32/lib/ESP32-sveltekit/peripherals/imu.h @@ -6,11 +6,22 @@ #include #include +#if FT_ENABLED(USE_MPU6050) #include +#endif + +#if FT_ENABLED(USE_BNO055) +#include +#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]; diff --git a/esp32/lib/ESP32-sveltekit/peripherals/peripherals.h b/esp32/lib/ESP32-sveltekit/peripherals/peripherals.h index 4790241..671fae0 100644 --- a/esp32/lib/ESP32-sveltekit/peripherals/peripherals.h +++ b/esp32/lib/ESP32-sveltekit/peripherals/peripherals.h @@ -67,13 +67,13 @@ class Peripherals : public StatefulService { 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 { /* 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 { 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 { 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 { void emitIMU() { doc.clear(); JsonObject root = doc.to(); -#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 { 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 { 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) diff --git a/esp32/lib/ESP32-sveltekit/peripherals/servo_controller.h b/esp32/lib/ESP32-sveltekit/peripherals/servo_controller.h index 5b68a33..88ad13a 100644 --- a/esp32/lib/ESP32-sveltekit/peripherals/servo_controller.h +++ b/esp32/lib/ESP32-sveltekit/peripherals/servo_controller.h @@ -111,7 +111,6 @@ class ServoController : public StatefulService { 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; diff --git a/platformio.ini b/platformio.ini index a4d92d5..b7f68fd 100644 --- a/platformio.ini +++ b/platformio.ini @@ -14,6 +14,7 @@ data_dir = esp32/data src_dir = esp32/src include_dir = esp32/include lib_dir = esp32/lib +test_dir = esp32/test extra_configs = esp32/factory_settings.ini esp32/features.ini @@ -64,8 +65,6 @@ build_flags = ${env.build_flags} -D USE_CAMERA=1 -D CAMERA_MODEL_XIAO_ESP32S3=1 - ;-D USS_LEFT_PIN=1 - ;-D USS_RIGHT_PIN=14 -D SDA_PIN=5 -D SCL_PIN=6