Update battery scaling for latest board

Fix MPU6050 address for latest board
Added more debug
Fix BNO085 offset

lib/mpu9250/MPU9250.cpp

@@ -53,6 +53,10 @@ MPU9250::MPU9250(uint8_t address) {
     devAddr = address;
 }
 
+uint8_t MPU9250::getAddr() {
+    return devAddr;
+}
+
 /** Power on and prepare for general usage.
  * This will activate the device and take it out of sleep mode (which must be done
  * after start-up). This function also sets both the accelerometer and the gyroscope

lib/mpu9250/MPU9250.h

@@ -417,6 +417,8 @@ class MPU9250 {
         void initialize();
         bool testConnection();
 
+        uint8_t getAddr();
+
         // AUX_VDDIO register
         uint8_t getAuxVDDIOLevel();
         void setAuxVDDIOLevel(uint8_t level);

src/main.cpp

@@ -144,7 +144,7 @@ void loop()
         }
         if(now_ms - last_battery_sample >= batterySampleRate) {
             last_battery_sample = now_ms;
-            float battery = ((float) analogRead(A0)) / 1024.0 * 14.0;
+            float battery = ((float) analogRead(A0)) / 1024.0 * 6.0;
             sendFloat(battery, PACKET_BATTERY_LEVEL);
         }
     }

src/mpu6050sensor.cpp

@@ -28,8 +28,14 @@
 void gatherCalibrationData(MPU9250 &imu);
 
 void MPU6050Sensor::motionSetup(DeviceConfig * config) {
+    Serial.print("IMU I2C address: ");
+    Serial.println(imu.getAddr(), HEX);
     // initialize device
     imu.initialize();
+    if(!imu.testConnection()) {
+        Serial.print("Can't communicate with MPU9250, response ");
+        Serial.println(imu.getDeviceID(), HEX);
+    }
     devStatus = imu.dmpInitialize();
 
     imu.setXGyroOffset(config->calibration.G_off[0]);

src/mpu9250sensor.cpp

@@ -43,6 +43,10 @@ void MPU9250Sensor::motionSetup(DeviceConfig * config) {
     calibration = &config->calibration;
     // initialize device
     imu.initialize();
+    if(!imu.testConnection()) {
+        Serial.print("Can't communicate with MPU9250, response ");
+        Serial.println(imu.getDeviceID(), HEX);
+    }
 }
 
 void MPU9250Sensor::motionLoop() {

src/sensor.h

@@ -54,6 +54,7 @@ class BNO080Sensor : public Sensor {
     private:
         BNO080 imu {};
         bool newData {false};
+        Quat sensorOffset {Quat(Vector3(0, 0, 1), PI / 2.0)};
 };
 
 class MPUSensor : public Sensor {
@@ -61,7 +62,7 @@ class MPUSensor : public Sensor {
         MPUSensor() = default;
         ~MPUSensor() override = default;
     protected:
-        MPU9250 imu {};
+        MPU9250 imu {MPU9250(MPU9250_ADDRESS_AD0_HIGH)};
         float q[4] {1.0, 0.0, 0.0, 0.0};
 };