diff --git a/src/bno080sensor.cpp b/src/bno080sensor.cpp index 6e594ab..b01a8c4 100644 --- a/src/bno080sensor.cpp +++ b/src/bno080sensor.cpp @@ -36,6 +36,10 @@ namespace { digitalWrite(LOADING_LED, HIGH); } } + + void sendResetReason(uint8_t reason) { + sendByte(reason, PACKET_RESET_REASON); + } } void BNO080Sensor::motionSetup(DeviceConfig * config) @@ -67,19 +71,30 @@ void BNO080Sensor::motionSetup(DeviceConfig * config) imu.enableARVRStabilizedGameRotationVector(10); else imu.enableGameRotationVector(10); + uint8_t lastReset = imu.resetReason(); + sendResetReason(lastReset); } +unsigned long lastData = 0; + void BNO080Sensor::motionLoop() { //Look for reports from the IMU if (imu.dataAvailable()) { + lastData = millis(); quaternion.x = imu.getQuatI(); quaternion.y = imu.getQuatJ(); quaternion.z = imu.getQuatK(); quaternion.w = imu.getQuatReal(); quaternion *= sensorOffset; newData = true; + } else { + if(lastData + 5000 < millis()) { + lastData = millis(); + uint8_t lastReset = imu.resetReason(); + sendResetReason(lastReset); + } } } diff --git a/src/defines.h b/src/defines.h index 8b13746..9fd0ea6 100644 --- a/src/defines.h +++ b/src/defines.h @@ -75,4 +75,6 @@ #define useFullCalibrationMatrix true #define sensorIdTime 1000 -#define sensorIdInterval 100 \ No newline at end of file +#define sensorIdInterval 100 + +#define batteryADCMultiplier 1.0 / 1024.0 * 5.0 \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 92b9972..321b233 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -148,7 +148,7 @@ void loop() } if(now_ms - last_battery_sample >= batterySampleRate) { last_battery_sample = now_ms; - float battery = ((float) analogRead(A0)) / 1024.0 * 6.0; + float battery = ((float) analogRead(A0)) * batteryADCMultiplier; sendFloat(battery, PACKET_BATTERY_LEVEL); } } diff --git a/src/udpclient.h b/src/udpclient.h index 6e4a7d1..0cba4ca 100644 --- a/src/udpclient.h +++ b/src/udpclient.h @@ -43,6 +43,7 @@ #define PACKET_SERIAL 11 #define PACKET_BATTERY_LEVEL 12 #define PACKET_TAP 13 +#define PACKET_RESET_REASON 14 #define PACKET_RECIEVE_HEARTBEAT 1 #define PACKET_RECIEVE_VIBRATE 2 @@ -67,6 +68,7 @@ void sendByte(char const value, int type); void sendRawCalibrationData(int * const data, int type); void setConfigRecievedCallback(configRecievedCallback); void setCommandRecievedCallback(commandRecievedCallback); +void sendSerial(uint8_t *const data, int length, int type); void setUpWiFi(DeviceConfig * const config); bool isConnected();