diff --git a/.editorconfig b/.editorconfig new file mode 100644 index 0000000..980ef21 --- /dev/null +++ b/.editorconfig @@ -0,0 +1,12 @@ +# EditorConfig is awesome: https://EditorConfig.org + +# top-most EditorConfig file +root = true + +[*] +indent_style = tab +indent_size = 4 +end_of_line = lf +charset = utf-8 +trim_trailing_whitespace = true +insert_final_newline = true diff --git a/src/GlobalVars.h b/src/GlobalVars.h index 9e97163..45adde9 100644 --- a/src/GlobalVars.h +++ b/src/GlobalVars.h @@ -1,39 +1,44 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2022 TheDevMinerTV + SlimeVR Code is placed under the MIT license + Copyright (c) 2022 TheDevMinerTV - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #ifndef GLOBALVARS_H #define GLOBALVARS_H -#include "LEDManager.h" -#include "status/StatusManager.h" -#include "configuration/Configuration.h" -#include "sensors/SensorManager.h" #include +#include "LEDManager.h" +#include "configuration/Configuration.h" +#include "network/connection.h" +#include "network/manager.h" +#include "sensors/SensorManager.h" +#include "status/StatusManager.h" + +extern Timer<> globalTimer; extern SlimeVR::LEDManager ledManager; extern SlimeVR::Status::StatusManager statusManager; extern SlimeVR::Configuration::Configuration configuration; extern SlimeVR::Sensors::SensorManager sensorManager; -extern Timer<> globalTimer; +extern SlimeVR::Network::Manager networkManager; +extern SlimeVR::Network::Connection networkConnection; #endif diff --git a/src/batterymonitor.cpp b/src/batterymonitor.cpp index b9309ee..b7ff913 100644 --- a/src/batterymonitor.cpp +++ b/src/batterymonitor.cpp @@ -55,9 +55,9 @@ void BatteryMonitor::Loop() voltage = -1; #if ESP8266 && (BATTERY_MONITOR == BAT_INTERNAL || BATTERY_MONITOR == BAT_INTERNAL_MCP3021) // Find out what your max measurement is (voltage_3_3). - // Take the max measurement and check if it was less than 50mV + // Take the max measurement and check if it was less than 50mV // if yes output 5.0V - // if no output 3.3V - dropvoltage + 0.1V + // if no output 3.3V - dropvoltage + 0.1V auto ESPmV = ESP.getVcc(); if (ESPmV > voltage_3_3) { @@ -116,7 +116,7 @@ void BatteryMonitor::Loop() level = 1; else if (level < 0) level = 0; - Network::sendBatteryLevel(voltage, level); + networkConnection.sendBatteryLevel(voltage, level); #ifdef BATTERY_LOW_POWER_VOLTAGE if (voltage < BATTERY_LOW_POWER_VOLTAGE) { @@ -132,4 +132,4 @@ void BatteryMonitor::Loop() } } #endif -} \ No newline at end of file +} diff --git a/src/batterymonitor.h b/src/batterymonitor.h index e3e1681..62c642e 100644 --- a/src/batterymonitor.h +++ b/src/batterymonitor.h @@ -25,7 +25,6 @@ #include #include "globals.h" -#include "network/network.h" #include #include #include "logging/Logger.h" @@ -62,7 +61,7 @@ // Wemos D1 Mini has an internal Voltage Divider with R1=100K and R2=220K > this means, 3.3V analogRead input voltage results in 1023.0 // Wemos D1 Mini with Wemos Battery Shield v1.2.0 or higher: Battery Shield with J2 closed, has an additional 130K resistor. So the resulting Voltage Divider is R1=220K+100K=320K and R2=100K > this means, 4.5V analogRead input voltage results in 1023.0 // ESP32 Boards may have not the internal Voltage Divider. Also ESP32 has a 12bit ADC (0..4095). So R1 and R2 can be changed. - // Diagramm: + // Diagramm: // (Battery)--- [BATTERY_SHIELD_RESISTANCE] ---(INPUT_BOARD)--- [BATTERY_SHIELD_R2] ---(ESP_INPUT)--- [BATTERY_SHIELD_R1] --- (GND) // SlimeVR Board can handle max 5V > so analogRead of 5.0V input will result in 1023.0 #define batteryADCMultiplier ADCVoltageMax / ADCResulution * (BATTERY_SHIELD_R1 + BATTERY_SHIELD_R2 + BATTERY_SHIELD_RESISTANCE) / BATTERY_SHIELD_R1 diff --git a/src/main.cpp b/src/main.cpp index 8b993e0..bc6148d 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -24,7 +24,6 @@ #include "Wire.h" #include "ota.h" #include "GlobalVars.h" -#include "network/network.h" #include "globals.h" #include "credentials.h" #include @@ -32,12 +31,14 @@ #include "batterymonitor.h" #include "logging/Logger.h" +Timer<> globalTimer; SlimeVR::Logging::Logger logger("SlimeVR"); SlimeVR::Sensors::SensorManager sensorManager; SlimeVR::LEDManager ledManager(LED_PIN); SlimeVR::Status::StatusManager statusManager; SlimeVR::Configuration::Configuration configuration; -Timer<> globalTimer; +SlimeVR::Network::Manager networkManager; +SlimeVR::Network::Connection networkConnection; int sensorToCalibrate = -1; bool blinking = false; @@ -52,7 +53,7 @@ void setup() Serial.begin(serialBaudRate); globalTimer = timer_create_default(); -#ifdef ESP32C3 +#ifdef ESP32C3 // Wait for the Computer to be able to connect. delay(2000); #endif @@ -82,7 +83,7 @@ void setup() #endif // using `static_cast` here seems to be better, because there are 2 similar function signatures - Wire.begin(static_cast(PIN_IMU_SDA), static_cast(PIN_IMU_SCL)); + Wire.begin(static_cast(PIN_IMU_SDA), static_cast(PIN_IMU_SCL)); #ifdef ESP8266 Wire.setClockStretchLimit(150000L); // Default stretch limit 150mS @@ -94,10 +95,10 @@ void setup() // Wait for IMU to boot delay(500); - + sensorManager.setup(); - - Network::setUp(); + + networkManager.setup(); OTA::otaSetup(otaPassword); battery.Setup(); @@ -113,7 +114,7 @@ void loop() globalTimer.tick(); SerialCommands::update(); OTA::otaUpdate(); - Network::update(sensorManager.getFirst(), sensorManager.getSecond()); + networkManager.update(); sensorManager.update(); battery.Loop(); ledManager.update(); diff --git a/src/network/connection.cpp b/src/network/connection.cpp new file mode 100644 index 0000000..9bd2b69 --- /dev/null +++ b/src/network/connection.cpp @@ -0,0 +1,597 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2023 SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#include "connection.h" + +#include "GlobalVars.h" +#include "logging/Logger.h" +#include "packets.h" + +#define TIMEOUT 3000UL + +template +unsigned char* convert_to_chars(T src, unsigned char* target) { + union uwunion { + unsigned char c[sizeof(T)]; + T v; + } un; + un.v = src; + for (size_t i = 0; i < sizeof(T); i++) { + target[i] = un.c[sizeof(T) - i - 1]; + } + return target; +} + +template +T convert_chars(unsigned char* const src) { + union uwunion { + unsigned char c[sizeof(T)]; + T v; + } un; + for (size_t i = 0; i < sizeof(T); i++) { + un.c[i] = src[sizeof(T) - i - 1]; + } + return un.v; +} + +namespace SlimeVR { +namespace Network { + +#define MUST_TRANSFER_BOOL(b) \ + if (!b) \ + return false; + +#define MUST(b) \ + if (!b) \ + return; + +bool Connection::beginPacket() { + int r = m_UDP.beginPacket(m_ServerHost, m_ServerPort); + if (r == 0) { + // This *technically* should *never* fail, since the underlying UDP + // library just returns 1. + + m_Logger.warn("UDP beginPacket() failed"); + } + + return r > 0; +} + +bool Connection::endPacket() { + int r = m_UDP.endPacket(); + if (r == 0) { + // This is usually just `ERR_ABRT` but the UDP client doesn't expose + // the full error code to us, so we just have to live with it. + + // m_Logger.warn("UDP endPacket() failed"); + } + + return r > 0; +} + +bool Connection::sendFloat(float f) { + convert_to_chars(f, m_Buf); + + return m_UDP.write(m_Buf, sizeof(f)) != 0; +} + +bool Connection::sendByte(uint8_t c) { return m_UDP.write(&c, 1) != 0; } + +bool Connection::sendInt(int i) { + convert_to_chars(i, m_Buf); + + return m_UDP.write(m_Buf, sizeof(i)) != 0; +} + +bool Connection::sendLong(uint64_t l) { + convert_to_chars(l, m_Buf); + + return m_UDP.write(m_Buf, sizeof(l)) != 0; +} + +bool Connection::sendBytes(const uint8_t* c, size_t length) { + return m_UDP.write(c, length) != 0; +} + +bool Connection::sendPacketNumber() { + uint64_t pn = m_PacketNumber++; + + return sendLong(pn); +} + +bool Connection::sendShortString(const char* str) { + uint8_t size = strlen(str); + + MUST_TRANSFER_BOOL(sendByte(size)); + MUST_TRANSFER_BOOL(sendBytes((const uint8_t*)str, size)); + + return true; +} + +bool Connection::sendPacketType(uint8_t type) { + MUST_TRANSFER_BOOL(sendByte(0)); + MUST_TRANSFER_BOOL(sendByte(0)); + MUST_TRANSFER_BOOL(sendByte(0)); + + return sendByte(type); +} + +bool Connection::sendLongString(const char* str) { + int size = strlen(str); + + MUST_TRANSFER_BOOL(sendInt(size)); + + return sendBytes((const uint8_t*)str, size); +} + +int Connection::getWriteError() { return m_UDP.getWriteError(); } + +// PACKET_HEARTBEAT 0 +void Connection::sendHeartbeat() { + MUST(m_Connected); + + MUST(beginPacket()); + + MUST(sendPacketType(PACKET_HEARTBEAT)); + MUST(sendPacketNumber()); + + MUST(endPacket()); +} + +// PACKET_ACCEL 4 +void Connection::sendSensorAcceleration(uint8_t sensorId, float* vector) { + MUST(m_Connected); + + MUST(beginPacket()); + + MUST(sendPacketType(PACKET_ACCEL)); + MUST(sendPacketNumber()); + MUST(sendFloat(vector[0])); + MUST(sendFloat(vector[1])); + MUST(sendFloat(vector[2])); + MUST(sendByte(sensorId)); + + MUST(endPacket()); +} + +// PACKET_BATTERY_LEVEL 12 +void Connection::sendBatteryLevel(float batteryVoltage, float batteryPercentage) { + MUST(m_Connected); + + MUST(beginPacket()); + + MUST(sendPacketType(PACKET_BATTERY_LEVEL)); + MUST(sendPacketNumber()); + MUST(sendFloat(batteryVoltage)); + MUST(sendFloat(batteryPercentage)); + + MUST(endPacket()); +} + +// PACKET_TAP 13 +void Connection::sendSensorTap(uint8_t sensorId, uint8_t value) { + MUST(m_Connected); + + MUST(beginPacket()); + + MUST(sendPacketType(PACKET_TAP)); + MUST(sendPacketNumber()); + MUST(sendByte(sensorId)); + MUST(sendByte(value)); + + MUST(endPacket()); +} + +// PACKET_ERROR 14 +void Connection::sendSensorError(uint8_t sensorId, uint8_t error) { + MUST(m_Connected); + + MUST(beginPacket()); + + MUST(sendPacketType(PACKET_ERROR)); + MUST(sendPacketNumber()); + MUST(sendByte(sensorId)); + MUST(sendByte(error)); + + MUST(endPacket()); +} + +// PACKET_SENSOR_INFO 15 +void Connection::sendSensorInfo(Sensor* sensor) { + MUST(m_Connected); + + MUST(beginPacket()); + + MUST(sendPacketType(PACKET_SENSOR_INFO)); + MUST(sendPacketNumber()); + MUST(sendByte(sensor->getSensorId())); + MUST(sendByte((uint8_t)sensor->getSensorState())); + MUST(sendByte(sensor->getSensorType())); + + MUST(endPacket()); +} + +// PACKET_ROTATION_DATA 17 +void Connection::sendRotationData( + uint8_t sensorId, + Quat* const quaternion, + uint8_t dataType, + uint8_t accuracyInfo +) { + MUST(m_Connected); + + MUST(beginPacket()); + + MUST(sendPacketType(PACKET_ROTATION_DATA)); + MUST(sendPacketNumber()); + MUST(sendByte(sensorId)); + MUST(sendByte(dataType)); + MUST(sendFloat(quaternion->x)); + MUST(sendFloat(quaternion->y)); + MUST(sendFloat(quaternion->z)); + MUST(sendFloat(quaternion->w)); + MUST(sendByte(accuracyInfo)); + + MUST(endPacket()); +} + +// PACKET_MAGNETOMETER_ACCURACY 18 +void Connection::sendMagnetometerAccuracy(uint8_t sensorId, float accuracyInfo) { + MUST(m_Connected); + + MUST(beginPacket()); + + MUST(sendPacketType(PACKET_MAGNETOMETER_ACCURACY)); + MUST(sendPacketNumber()); + MUST(sendByte(sensorId)); + MUST(sendFloat(accuracyInfo)); + + MUST(endPacket()); +} + +// PACKET_SIGNAL_STRENGTH 19 +void Connection::sendSignalStrength(uint8_t signalStrength) { + MUST(m_Connected); + + MUST(beginPacket()); + + MUST(sendPacketType(PACKET_SIGNAL_STRENGTH)); + MUST(sendPacketNumber()); + MUST(sendByte(255)); + MUST(sendByte(signalStrength)); + + MUST(endPacket()); +} + +// PACKET_TEMPERATURE 20 +void Connection::sendTemperature(uint8_t sensorId, float temperature) { + MUST(m_Connected); + + MUST(beginPacket()); + + MUST(sendPacketType(PACKET_TEMPERATURE)); + MUST(sendPacketNumber()); + MUST(sendByte(sensorId)); + MUST(sendFloat(temperature)); + + MUST(endPacket()); +} + +void Connection::sendTrackerDiscovery() { + MUST(!m_Connected); + + uint8_t mac[6]; + WiFi.macAddress(mac); + + MUST(beginPacket()); + + MUST(sendPacketType(PACKET_HANDSHAKE)); + // Packet number is always 0 for handshake + MUST(sendLong(0)); + MUST(sendInt(BOARD)); + // This is kept for backwards compatibility, + // but the latest SlimeVR server will not initialize trackers + // with firmware build > 8 until it recieves a sensor info packet + MUST(sendInt(IMU)); + MUST(sendInt(HARDWARE_MCU)); + MUST(sendInt(0)); + MUST(sendInt(0)); + MUST(sendInt(0)); + MUST(sendInt(FIRMWARE_BUILD_NUMBER)); + MUST(sendShortString(FIRMWARE_VERSION)); + // MAC address string + MUST(sendBytes(mac, 6)); + + MUST(endPacket()); +} + +#if ENABLE_INSPECTION +void Connection::sendInspectionRawIMUData( + uint8_t sensorId, + int16_t rX, + int16_t rY, + int16_t rZ, + uint8_t rA, + int16_t aX, + int16_t aY, + int16_t aZ, + uint8_t aA, + int16_t mX, + int16_t mY, + int16_t mZ, + uint8_t mA +) { + MUST(m_Connected); + + MUST(beginPacket()); + + MUST(sendPacketType(PACKET_INSPECTION)); + MUST(sendPacketNumber()); + + MUST(sendByte(PACKET_INSPECTION_PACKETTYPE_RAW_IMU_DATA)); + + MUST(sendByte(sensorId)); + MUST(sendByte(PACKET_INSPECTION_DATATYPE_INT)); + + MUST(sendInt(rX)); + MUST(sendInt(rY)); + MUST(sendInt(rZ)); + MUST(sendByte(rA)); + + MUST(sendInt(aX)); + MUST(sendInt(aY)); + MUST(sendInt(aZ)); + MUST(sendByte(aA)); + + MUST(sendInt(mX)); + MUST(sendInt(mY)); + MUST(sendInt(mZ)); + MUST(sendByte(mA)); + + MUST(endPacket()); +} + +void Connection::sendInspectionRawIMUData( + uint8_t sensorId, + float rX, + float rY, + float rZ, + uint8_t rA, + float aX, + float aY, + float aZ, + uint8_t aA, + float mX, + float mY, + float mZ, + uint8_t mA +) { + MUST(m_Connected); + + MUST(beginPacket()); + + MUST(sendPacketType(PACKET_INSPECTION)); + MUST(sendPacketNumber()); + + MUST(sendByte(PACKET_INSPECTION_PACKETTYPE_RAW_IMU_DATA)); + + MUST(sendByte(sensorId)); + MUST(sendByte(PACKET_INSPECTION_DATATYPE_FLOAT)); + + MUST(sendFloat(rX)); + MUST(sendFloat(rY)); + MUST(sendFloat(rZ)); + MUST(sendByte(rA)); + + MUST(sendFloat(aX)); + MUST(sendFloat(aY)); + MUST(sendFloat(aZ)); + MUST(sendByte(aA)); + + MUST(sendFloat(mX)); + MUST(sendFloat(mY)); + MUST(sendFloat(mZ)); + MUST(sendByte(mA)); + + MUST(endPacket()); +} +#endif + +void Connection::returnLastPacket(int len) { + MUST(m_Connected); + + MUST(beginPacket()); + + MUST(sendBytes(m_Packet, len)); + + MUST(endPacket()); +} + +void Connection::updateSensorState(Sensor* const sensor1, Sensor* const sensor2) { + if (millis() - m_LastSensorInfoPacketTimestamp <= 1000) { + return; + } + + m_LastSensorInfoPacketTimestamp = millis(); + + if (m_AckedSensorState1 != sensor1->getSensorState()) { + sendSensorInfo(sensor1); + } + + if (m_AckedSensorState2 != sensor2->getSensorState()) { + sendSensorInfo(sensor2); + } +} + +void Connection::searchForServer() { + while (true) { + int packetSize = m_UDP.parsePacket(); + if (!packetSize) { + break; + } + + // receive incoming UDP packets + int len = m_UDP.read(m_Packet, sizeof(m_Packet)); + +#ifdef DEBUG_NETWORK + m_Logger.trace( + "Received %d bytes from %s, port %d", + packetSize, + m_UDP.remoteIP().toString().c_str(), + m_UDP.remotePort() + ); + m_Logger.traceArray("UDP packet contents: ", m_Packet, len); +#endif + + // Handshake is different, it has 3 in the first byte, not the 4th, and data + // starts right after + if (m_Packet[0] == PACKET_HANDSHAKE) { + if (strncmp((char*)m_Packet + 1, "Hey OVR =D 5", 12) != 0) { + m_Logger.error("Received invalid handshake packet"); + continue; + } + + m_ServerHost = m_UDP.remoteIP(); + m_ServerPort = m_UDP.remotePort(); + m_LastPacketTimestamp = millis(); + m_Connected = true; + + statusManager.setStatus(SlimeVR::Status::SERVER_CONNECTING, false); + ledManager.off(); + + m_Logger.debug( + "Handshake successful, server is %s:%d", + m_UDP.remoteIP().toString().c_str(), + m_UDP.remotePort() + ); + + break; + } + } + + auto now = millis(); + + // This makes the LED blink for 20ms every second + if (m_lastConnectionAttemptTimestamp + 1000 < now) { + m_lastConnectionAttemptTimestamp = now; + m_Logger.info("Searching for the server on the local network..."); + Connection::sendTrackerDiscovery(); + ledManager.on(); + } else if (m_lastConnectionAttemptTimestamp + 20 < now) { + ledManager.off(); + } +} + +void Connection::reset() { + m_Connected = false; + m_AckedSensorState1 = SensorStatus::SENSOR_OFFLINE; + m_AckedSensorState2 = SensorStatus::SENSOR_OFFLINE; + + m_UDP.begin(m_ServerPort); + + statusManager.setStatus(SlimeVR::Status::SERVER_CONNECTING, true); +} + +void Connection::update() { + auto sensor1 = sensorManager.getFirst(); + auto sensor2 = sensorManager.getSecond(); + + updateSensorState(sensor1, sensor2); + + if (!m_Connected) { + searchForServer(); + return; + } + + if (m_LastPacketTimestamp + TIMEOUT < millis()) { + statusManager.setStatus(SlimeVR::Status::SERVER_CONNECTING, true); + + m_Connected = false; + m_AckedSensorState1 = SensorStatus::SENSOR_OFFLINE; + m_AckedSensorState2 = SensorStatus::SENSOR_OFFLINE; + m_Logger.warn("Connection to server timed out"); + + return; + } + + int packetSize = m_UDP.parsePacket(); + if (!packetSize) { + return; + } + + m_LastPacketTimestamp = millis(); + int len = m_UDP.read(m_Packet, sizeof(m_Packet)); + +#ifdef DEBUG_NETWORK + m_Logger.trace( + "Received %d bytes from %s, port %d", + packetSize, + m_UDP.remoteIP().toString().c_str(), + m_UDP.remotePort() + ); + m_Logger.traceArray("UDP packet contents: ", m_Packet, len); +#else + (void)packetSize; +#endif + + switch (convert_chars(m_Packet)) { + case PACKET_RECEIVE_HEARTBEAT: + sendHeartbeat(); + break; + + case PACKET_RECEIVE_VIBRATE: + break; + + case PACKET_RECEIVE_HANDSHAKE: + // Assume handshake successful + m_Logger.warn("Handshake received again, ignoring"); + break; + + case PACKET_RECEIVE_COMMAND: + break; + + case PACKET_CONFIG: + break; + + case PACKET_PING_PONG: + returnLastPacket(len); + break; + + case PACKET_SENSOR_INFO: + if (len < 6) { + m_Logger.warn("Wrong sensor info packet"); + break; + } + + if (m_Packet[4] == sensor1->getSensorId()) { + m_AckedSensorState1 = (SensorStatus)m_Packet[5]; + } else if (m_Packet[4] == sensor2->getSensorId()) { + m_AckedSensorState2 = (SensorStatus)m_Packet[5]; + } + + break; + } +} + +} // namespace Network +} // namespace SlimeVR diff --git a/src/network/connection.h b/src/network/connection.h new file mode 100644 index 0000000..3ca0156 --- /dev/null +++ b/src/network/connection.h @@ -0,0 +1,157 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2023 SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ +#ifndef SLIMEVR_NETWORK_CONNECTION_H_ +#define SLIMEVR_NETWORK_CONNECTION_H_ + +#include +#include + +#include "globals.h" +#include "quat.h" +#include "sensors/sensor.h" +#include "wifihandler.h" + +namespace SlimeVR { +namespace Network { + +class Connection { +public: + void searchForServer(); + void update(); + void reset(); + bool isConnected() const { return m_Connected; } + + // PACKET_ACCEL 4 + void sendSensorAcceleration(uint8_t sensorId, float* vector); + + // PACKET_BATTERY_LEVEL 12 + void sendBatteryLevel(float batteryVoltage, float batteryPercentage); + + // PACKET_TAP 13 + void sendSensorTap(uint8_t sensorId, uint8_t value); + + // PACKET_ERROR 14 + void sendSensorError(uint8_t sensorId, uint8_t error); + + // PACKET_ROTATION_DATA 17 + void sendRotationData( + uint8_t sensorId, + Quat* const quaternion, + uint8_t dataType, + uint8_t accuracyInfo + ); + + // PACKET_MAGNETOMETER_ACCURACY 18 + void sendMagnetometerAccuracy(uint8_t sensorId, float accuracyInfo); + + // PACKET_SIGNAL_STRENGTH 19 + void sendSignalStrength(uint8_t signalStrength); + + // PACKET_TEMPERATURE 20 + void sendTemperature(uint8_t sensorId, float temperature); + +#if ENABLE_INSPECTION + void sendInspectionRawIMUData( + uint8_t sensorId, + int16_t rX, + int16_t rY, + int16_t rZ, + uint8_t rA, + int16_t aX, + int16_t aY, + int16_t aZ, + uint8_t aA, + int16_t mX, + int16_t mY, + int16_t mZ, + uint8_t mA + ); + void sendInspectionRawIMUData( + uint8_t sensorId, + float rX, + float rY, + float rZ, + uint8_t rA, + float aX, + float aY, + float aZ, + uint8_t aA, + float mX, + float mY, + float mZ, + uint8_t mA + ); +#endif + +private: + void updateSensorState(Sensor* const sensor1, Sensor* const sensor2); + + bool beginPacket(); + bool endPacket(); + + bool sendPacketType(uint8_t type); + bool sendPacketNumber(); + bool sendFloat(float f); + bool sendByte(uint8_t c); + bool sendInt(int i); + bool sendLong(uint64_t l); + bool sendBytes(const uint8_t* c, size_t length); + bool sendShortString(const char* str); + bool sendLongString(const char* str); + + int getWriteError(); + + void returnLastPacket(int len); + + // PACKET_HEARTBEAT 0 + void sendHeartbeat(); + + // PACKET_HANDSHAKE 3 + void sendTrackerDiscovery(); + + // PACKET_SENSOR_INFO 15 + void sendSensorInfo(Sensor* sensor); + + bool m_Connected = false; + SlimeVR::Logging::Logger m_Logger = SlimeVR::Logging::Logger("UDPConnection"); + + WiFiUDP m_UDP; + unsigned char m_Packet[128]; // buffer for incoming packets + uint64_t m_PacketNumber = 0; + + int m_ServerPort = 6969; + IPAddress m_ServerHost = IPAddress(255, 255, 255, 255); + unsigned long m_lastConnectionAttemptTimestamp; + unsigned long m_LastPacketTimestamp; + + SensorStatus m_AckedSensorState1 = SensorStatus::SENSOR_OFFLINE; + SensorStatus m_AckedSensorState2 = SensorStatus::SENSOR_OFFLINE; + unsigned long m_LastSensorInfoPacketTimestamp = 0; + + unsigned char m_Buf[8]; +}; + +} // namespace Network +} // namespace SlimeVR + +#endif // SLIMEVR_NETWORK_CONNECTION_H_ diff --git a/src/network/manager.cpp b/src/network/manager.cpp new file mode 100644 index 0000000..c27329f --- /dev/null +++ b/src/network/manager.cpp @@ -0,0 +1,52 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2023 SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ +#include "manager.h" + +#include "GlobalVars.h" + +namespace SlimeVR { +namespace Network { + +void Manager::setup() { ::WiFiNetwork::setUp(); } + +void Manager::update() { + WiFiNetwork::upkeep(); + + auto wasConnected = m_IsConnected; + + m_IsConnected = ::WiFiNetwork::isConnected(); + + if (!m_IsConnected) { + return; + } + + if (!wasConnected) { + // WiFi was reconnected, rediscover the server and reconnect + networkConnection.reset(); + } + + networkConnection.update(); +} + +} // namespace Network +} // namespace SlimeVR diff --git a/src/network/manager.h b/src/network/manager.h new file mode 100644 index 0000000..d934c71 --- /dev/null +++ b/src/network/manager.h @@ -0,0 +1,46 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2023 SlimeVR Contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ +#ifndef SLIMEVR_NETWORK_MANAGER_H_ +#define SLIMEVR_NETWORK_MANAGER_H_ + +#include "globals.h" +#include "packets.h" +#include "wifihandler.h" +#include "wifiprovisioning.h" + +namespace SlimeVR { +namespace Network { + +class Manager { +public: + void setup(); + void update(); + +private: + bool m_IsConnected = false; +}; + +} // namespace Network +} // namespace SlimeVR + +#endif // SLIMEVR_NETWORK_MANAGER_H_ diff --git a/src/network/network.cpp b/src/network/network.cpp deleted file mode 100644 index 848ca7a..0000000 --- a/src/network/network.cpp +++ /dev/null @@ -1,42 +0,0 @@ -/* - SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. -*/ -#include "network.h" - -bool lastWifiConnected = false; - -void Network::setUp() { - WiFiNetwork::setUp(); -} - -void Network::update(Sensor * const sensor, Sensor * const sensor2) { - WiFiNetwork::upkeep(); - if(WiFiNetwork::isConnected()) { - if(lastWifiConnected == false) { - lastWifiConnected = true; - ServerConnection::resetConnection(); // WiFi was reconnected, reconnect to the server - } - ServerConnection::update(sensor, sensor2); - } else { - lastWifiConnected = false; - } -} diff --git a/src/network/network.h b/src/network/network.h deleted file mode 100644 index 29304b7..0000000 --- a/src/network/network.h +++ /dev/null @@ -1,37 +0,0 @@ -/* - SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. -*/ -#ifndef SLIMEVR_NETWORK_H_ -#define SLIMEVR_NETWORK_H_ - -#include "globals.h" -#include "wifihandler.h" -#include "udpclient.h" -#include "packets.h" -#include "wifiprovisioning.h" - -namespace Network { - void update(Sensor * const sensor, Sensor * const sensor2); - void setUp(); -} - -#endif // SLIMEVR_NETWORK_H_ \ No newline at end of file diff --git a/src/network/packets.h b/src/network/packets.h index a44649d..a01b91f 100644 --- a/src/network/packets.h +++ b/src/network/packets.h @@ -1,54 +1,52 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain + SlimeVR Code is placed under the MIT license + Copyright (c) 2021 Eiren Rain - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #ifndef SLIMEVR_PACKETS_H_ #define SLIMEVR_PACKETS_H_ -#include "sensors/sensor.h" - #define PACKET_HEARTBEAT 0 -//#define PACKET_ROTATION 1 // Deprecated -//#define PACKET_GYRO 2 // Deprecated +// #define PACKET_ROTATION 1 // Deprecated +// #define PACKET_GYRO 2 // Deprecated #define PACKET_HANDSHAKE 3 #define PACKET_ACCEL 4 -//#define PACKET_MAG 5 // Deprecated -#define PACKET_RAW_CALIBRATION_DATA 6 -#define PACKET_CALIBRATION_FINISHED 7 +// #define PACKET_MAG 5 // Deprecated +// #define PACKET_RAW_CALIBRATION_DATA 6 // Deprecated +// #define PACKET_CALIBRATION_FINISHED 7 // Deprecated #define PACKET_CONFIG 8 -//#define PACKET_RAW_MAGNETOMETER 9 // Deprecated +// #define PACKET_RAW_MAGNETOMETER 9 // Deprecated #define PACKET_PING_PONG 10 #define PACKET_SERIAL 11 #define PACKET_BATTERY_LEVEL 12 #define PACKET_TAP 13 #define PACKET_ERROR 14 #define PACKET_SENSOR_INFO 15 -//#define PACKET_ROTATION_2 16 // Deprecated +// #define PACKET_ROTATION_2 16 // Deprecated #define PACKET_ROTATION_DATA 17 #define PACKET_MAGNETOMETER_ACCURACY 18 #define PACKET_SIGNAL_STRENGTH 19 #define PACKET_TEMPERATURE 20 -#define PACKET_INSPECTION 105 // 0x69 +#define PACKET_INSPECTION 105 // 0x69 #define PACKET_RECEIVE_HEARTBEAT 1 #define PACKET_RECEIVE_VIBRATE 2 @@ -61,68 +59,4 @@ #define PACKET_INSPECTION_DATATYPE_INT 1 #define PACKET_INSPECTION_DATATYPE_FLOAT 2 -namespace Network { - // PACKET_HEARTBEAT 0 - void sendHeartbeat(); - - // PACKET_HANDSHAKE 3 - void sendHandshake(); - - // PACKET_ACCEL 4 - void sendAccel(float* vector, uint8_t sensorId); - - // PACKET_RAW_CALIBRATION_DATA 6 - void sendRawCalibrationData(float* vector, uint8_t calibrationType, uint8_t sensorId); - void sendRawCalibrationData(int* vector, uint8_t calibrationType, uint8_t sensorId); - - // PACKET_CALIBRATION_FINISHED 7 - void sendCalibrationFinished(uint8_t calibrationType, uint8_t sensorId); - - // PACKET_BATTERY_LEVEL 12 - void sendBatteryLevel(float batteryVoltage, float batteryPercentage); - - // PACKET_TAP 13 - void sendTap(uint8_t value, uint8_t sensorId); - - // PACKET_ERROR 14 - void sendError(uint8_t reason, uint8_t sensorId); - - // PACKET_SENSOR_INFO 15 - void sendSensorInfo(Sensor * sensor); - - // PACKET_ROTATION_DATA 17 - void sendRotationData(Quat * const quaternion, uint8_t dataType, uint8_t accuracyInfo, uint8_t sensorId); - - // PACKET_MAGNETOMETER_ACCURACY 18 - void sendMagnetometerAccuracy(float accuracyInfo, uint8_t sensorId); - - // PACKET_SIGNAL_STRENGTH 19 - void sendSignalStrength(uint8_t signalStrength); - - // PACKET_TEMPERATURE 20 - void sendTemperature(float temperature, uint8_t sensorId); - -#if ENABLE_INSPECTION - void sendInspectionRawIMUData(uint8_t sensorId, int16_t rX, int16_t rY, int16_t rZ, uint8_t rA, int16_t aX, int16_t aY, int16_t aZ, uint8_t aA, int16_t mX, int16_t mY, int16_t mZ, uint8_t mA); - void sendInspectionRawIMUData(uint8_t sensorId, float rX, float rY, float rZ, uint8_t rA, float aX, float aY, float aZ, uint8_t aA, float mX, float mY, float mZ, uint8_t mA); -#endif -} - -namespace DataTransfer { - bool beginPacket(); - bool endPacket(); - void sendPacketType(uint8_t type); - void sendPacketNumber(); - - void sendFloat(float f); - void sendByte(uint8_t c); - void sendInt(int i); - void sendLong(uint64_t l); - void sendBytes(const uint8_t * c, size_t length); - void sendShortString(const char * str); - void sendLongString(const char * str); - - int getWriteError(); -} - -#endif // SLIMEVR_PACKETS_H_ +#endif // SLIMEVR_PACKETS_H_ diff --git a/src/network/udpclient.cpp b/src/network/udpclient.cpp deleted file mode 100644 index 24422ec..0000000 --- a/src/network/udpclient.cpp +++ /dev/null @@ -1,633 +0,0 @@ -/* - SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain & SlimeVR contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. -*/ - -#include "udpclient.h" -#include "packets.h" -#include "logging/Logger.h" -#include "GlobalVars.h" - -#define TIMEOUT 3000UL - -WiFiUDP Udp; -unsigned char incomingPacket[128]; // buffer for incoming packets -uint64_t packetNumber = 0; -unsigned char handshake[12] = {0, 0, 0, 3, 0, 0, 0, 0, 0, 0, 0, 0}; - -int port = 6969; -IPAddress host = IPAddress(255, 255, 255, 255); -unsigned long lastConnectionAttemptMs; -unsigned long lastPacketMs; - -bool connected = false; - -uint8_t sensorStateNotified1 = 0; -uint8_t sensorStateNotified2 = 0; -unsigned long lastSensorInfoPacket = 0; - -uint8_t serialBuffer[128]; -size_t serialLength = 0; - -unsigned char buf[8]; - -// TODO: Cleanup with proper classes -SlimeVR::Logging::Logger udpClientLogger("UDPClient"); - -template -unsigned char * convert_to_chars(T src, unsigned char * target) -{ - union uwunion - { - unsigned char c[sizeof(T)]; - T v; - } un; - un.v = src; - for (size_t i = 0; i < sizeof(T); i++) - { - target[i] = un.c[sizeof(T) - i - 1]; - } - return target; -} - -template -T convert_chars(unsigned char * const src) -{ - union uwunion - { - unsigned char c[sizeof(T)]; - T v; - } un; - for (size_t i = 0; i < sizeof(T); i++) - { - un.c[i] = src[sizeof(T) - i - 1]; - } - return un.v; -} - -namespace DataTransfer { - - bool beginPacket() { - int r = Udp.beginPacket(host, port); - if(r == 0) { - // Print error - } - return r > 0; - } - - bool endPacket() { - int r = Udp.endPacket(); - if(r == 0) { - // Print error - } - return r > 0; - } - - void sendPacketType(uint8_t type) { - Udp.write(0); - Udp.write(0); - Udp.write(0); - Udp.write(type); - } - - void sendPacketNumber() { - uint64_t pn = packetNumber++; - sendLong(pn); - } - - void sendFloat(float f) { - Udp.write(convert_to_chars(f, buf), sizeof(f)); - } - - void sendByte(uint8_t c) { - Udp.write(&c, 1); - } - - void sendInt(int i) { - Udp.write(convert_to_chars(i, buf), sizeof(i)); - } - - void sendLong(uint64_t l) { - Udp.write(convert_to_chars(l, buf), sizeof(l)); - } - - void sendBytes(const uint8_t * c, size_t length) { - Udp.write(c, length); - } - - void sendShortString(const char * str) { - uint8_t size = strlen(str); - sendByte(size); // String size - sendBytes((const uint8_t *) str, size); // Firmware version string - } - - void sendLongString(const char * str) { - int size = strlen(str); - sendInt(size); // String size - sendBytes((const uint8_t *) str, size); // Firmware version string - } - - int getWriteError() { - return Udp.getWriteError(); - } -} - -// PACKET_HEARTBEAT 0 -void Network::sendHeartbeat() { - if(!connected) - { - return; - } - - if(DataTransfer::beginPacket()) { - DataTransfer::sendPacketType(PACKET_HEARTBEAT); - DataTransfer::sendPacketNumber(); - DataTransfer::endPacket(); - } -} - -// PACKET_ACCEL 4 -void Network::sendAccel(float* vector, uint8_t sensorId) { - if(!connected) - { - return; - } - - if(DataTransfer::beginPacket()) { - DataTransfer::sendPacketType(PACKET_ACCEL); - DataTransfer::sendPacketNumber(); - DataTransfer::sendFloat(vector[0]); - DataTransfer::sendFloat(vector[1]); - DataTransfer::sendFloat(vector[2]); - DataTransfer::sendByte(sensorId); - DataTransfer::endPacket(); - } -} - -// PACKET_RAW_CALIBRATION_DATA 6 -void Network::sendRawCalibrationData(float* vector, uint8_t calibrationType, uint8_t sensorId) { - if(!connected) - { - return; - } - - if(DataTransfer::beginPacket()) { - DataTransfer::sendPacketType(PACKET_RAW_CALIBRATION_DATA); - DataTransfer::sendPacketNumber(); - DataTransfer::sendByte(sensorId); - DataTransfer::sendInt(calibrationType); - DataTransfer::sendFloat(vector[0]); - DataTransfer::sendFloat(vector[1]); - DataTransfer::sendFloat(vector[2]); - DataTransfer::endPacket(); - } -} - -void Network::sendRawCalibrationData(int* vector, uint8_t calibrationType, uint8_t sensorId) { - if(!connected) - { - return; - } - - if(DataTransfer::beginPacket()) { - DataTransfer::sendPacketType(PACKET_RAW_CALIBRATION_DATA); - DataTransfer::sendPacketNumber(); - DataTransfer::sendByte(sensorId); - DataTransfer::sendInt(calibrationType); - DataTransfer::sendInt(vector[0]); - DataTransfer::sendInt(vector[1]); - DataTransfer::sendInt(vector[2]); - DataTransfer::endPacket(); - } -} - -// PACKET_CALIBRATION_FINISHED 7 -void Network::sendCalibrationFinished(uint8_t calibrationType, uint8_t sensorId) { - if(!connected) - { - return; - } - - if(DataTransfer::beginPacket()) { - DataTransfer::sendPacketType(PACKET_CALIBRATION_FINISHED); - DataTransfer::sendPacketNumber(); - DataTransfer::sendByte(sensorId); - DataTransfer::sendInt(calibrationType); - DataTransfer::endPacket(); - } -} - -// PACKET_BATTERY_LEVEL 12 -void Network::sendBatteryLevel(float batteryVoltage, float batteryPercentage) { - if(!connected) - { - return; - } - - if(DataTransfer::beginPacket()) { - DataTransfer::sendPacketType(PACKET_BATTERY_LEVEL); - DataTransfer::sendPacketNumber(); - DataTransfer::sendFloat(batteryVoltage); - DataTransfer::sendFloat(batteryPercentage); - DataTransfer::endPacket(); - } -} - -// PACKET_TAP 13 -void Network::sendTap(uint8_t value, uint8_t sensorId) { - if(!connected) - { - return; - } - - if(DataTransfer::beginPacket()) { - DataTransfer::sendPacketType(PACKET_TAP); - DataTransfer::sendPacketNumber(); - DataTransfer::sendByte(sensorId); - DataTransfer::sendByte(value); - DataTransfer::endPacket(); - } -} - -// PACKET_ERROR 14 -void Network::sendError(uint8_t reason, uint8_t sensorId) { - if(!connected) - { - return; - } - - if(DataTransfer::beginPacket()) { - DataTransfer::sendPacketType(PACKET_ERROR); - DataTransfer::sendPacketNumber(); - DataTransfer::sendByte(sensorId); - DataTransfer::sendByte(reason); - DataTransfer::endPacket(); - } -} - -// PACKET_SENSOR_INFO 15 -void Network::sendSensorInfo(Sensor * sensor) { - if(!connected) - { - return; - } - - if(DataTransfer::beginPacket()) { - DataTransfer::sendPacketType(PACKET_SENSOR_INFO); - DataTransfer::sendPacketNumber(); - DataTransfer::sendByte(sensor->getSensorId()); - DataTransfer::sendByte(sensor->getSensorState()); - DataTransfer::sendByte(sensor->getSensorType()); - DataTransfer::endPacket(); - } -} - -// PACKET_ROTATION_DATA 17 -void Network::sendRotationData(Quat * const quaternion, uint8_t dataType, uint8_t accuracyInfo, uint8_t sensorId) { - if(!connected) - { - return; - } - - if(DataTransfer::beginPacket()) { - DataTransfer::sendPacketType(PACKET_ROTATION_DATA); - DataTransfer::sendPacketNumber(); - DataTransfer::sendByte(sensorId); - DataTransfer::sendByte(dataType); - DataTransfer::sendFloat(quaternion->x); - DataTransfer::sendFloat(quaternion->y); - DataTransfer::sendFloat(quaternion->z); - DataTransfer::sendFloat(quaternion->w); - DataTransfer::sendByte(accuracyInfo); - DataTransfer::endPacket(); - } -} - -// PACKET_MAGNETOMETER_ACCURACY 18 -void Network::sendMagnetometerAccuracy(float accuracyInfo, uint8_t sensorId) { - if(!connected) - { - return; - } - - if(DataTransfer::beginPacket()) { - DataTransfer::sendPacketType(PACKET_MAGNETOMETER_ACCURACY); - DataTransfer::sendPacketNumber(); - DataTransfer::sendByte(sensorId); - DataTransfer::sendFloat(accuracyInfo); - DataTransfer::endPacket(); - } -} - -// PACKET_SIGNAL_STRENGTH 19 -void Network::sendSignalStrength(uint8_t signalStrength) { - if(!connected) - { - return; - } - - if(DataTransfer::beginPacket()) { - DataTransfer::sendPacketType(PACKET_SIGNAL_STRENGTH); - DataTransfer::sendPacketNumber(); - DataTransfer::sendByte(255); - DataTransfer::sendByte(signalStrength); - DataTransfer::endPacket(); - } -} - -// PACKET_TEMPERATURE 20 -void Network::sendTemperature(float temperature, uint8_t sensorId) { - if(!connected) - { - return; - } - - if(DataTransfer::beginPacket()) { - DataTransfer::sendPacketType(PACKET_TEMPERATURE); - DataTransfer::sendPacketNumber(); - DataTransfer::sendByte(sensorId); - DataTransfer::sendFloat(temperature); - DataTransfer::endPacket(); - } -} - -void Network::sendHandshake() { - if(DataTransfer::beginPacket()) { - DataTransfer::sendPacketType(PACKET_HANDSHAKE); - DataTransfer::sendLong(0); // Packet number is always 0 for handshake - DataTransfer::sendInt(BOARD); - // This is kept for backwards compatibility, - // but the latest SlimeVR server will not initialize trackers - // with firmware build > 8 until it recieves sensor info packet - DataTransfer::sendInt(IMU); - DataTransfer::sendInt(HARDWARE_MCU); - DataTransfer::sendInt(0); - DataTransfer::sendInt(0); - DataTransfer::sendInt(0); - DataTransfer::sendInt(FIRMWARE_BUILD_NUMBER); // Firmware build number - DataTransfer::sendShortString(FIRMWARE_VERSION); - uint8_t mac[6]; - WiFi.macAddress(mac); - DataTransfer::sendBytes(mac, 6); // MAC address string - if(!DataTransfer::endPacket()) { - udpClientLogger.error("Handshake write error: %d", Udp.getWriteError()); - } - } else { - udpClientLogger.error("Handshake write error: %d", Udp.getWriteError()); - } -} - -#if ENABLE_INSPECTION -void Network::sendInspectionRawIMUData(uint8_t sensorId, int16_t rX, int16_t rY, int16_t rZ, uint8_t rA, int16_t aX, int16_t aY, int16_t aZ, uint8_t aA, int16_t mX, int16_t mY, int16_t mZ, uint8_t mA) -{ - if (!connected) - { - return; - } - - if(!DataTransfer::beginPacket()) - { - udpClientLogger.error("RawIMUData write begin error: %d", Udp.getWriteError()); - return; - } - - DataTransfer::sendPacketType(PACKET_INSPECTION); - DataTransfer::sendPacketNumber(); - - DataTransfer::sendByte(PACKET_INSPECTION_PACKETTYPE_RAW_IMU_DATA); - - DataTransfer::sendByte(sensorId); - DataTransfer::sendByte(PACKET_INSPECTION_DATATYPE_INT); - - DataTransfer::sendInt(rX); - DataTransfer::sendInt(rY); - DataTransfer::sendInt(rZ); - DataTransfer::sendByte(rA); - - DataTransfer::sendInt(aX); - DataTransfer::sendInt(aY); - DataTransfer::sendInt(aZ); - DataTransfer::sendByte(aA); - - DataTransfer::sendInt(mX); - DataTransfer::sendInt(mY); - DataTransfer::sendInt(mZ); - DataTransfer::sendByte(mA); - - if(!DataTransfer::endPacket()) - { - udpClientLogger.error("RawIMUData write end error: %d", Udp.getWriteError()); - } -} - -void Network::sendInspectionRawIMUData(uint8_t sensorId, float rX, float rY, float rZ, uint8_t rA, float aX, float aY, float aZ, uint8_t aA, float mX, float mY, float mZ, uint8_t mA) -{ - if (!connected) - { - return; - } - - if (!DataTransfer::beginPacket()) - { - udpClientLogger.error("RawIMUData write begin error: %d", Udp.getWriteError()); - return; - } - - DataTransfer::sendPacketType(PACKET_INSPECTION); - DataTransfer::sendPacketNumber(); - - DataTransfer::sendByte(PACKET_INSPECTION_PACKETTYPE_RAW_IMU_DATA); - - DataTransfer::sendByte(sensorId); - DataTransfer::sendByte(PACKET_INSPECTION_DATATYPE_FLOAT); - - DataTransfer::sendFloat(rX); - DataTransfer::sendFloat(rY); - DataTransfer::sendFloat(rZ); - DataTransfer::sendByte(rA); - - DataTransfer::sendFloat(aX); - DataTransfer::sendFloat(aY); - DataTransfer::sendFloat(aZ); - DataTransfer::sendByte(aA); - - DataTransfer::sendFloat(mX); - DataTransfer::sendFloat(mY); - DataTransfer::sendFloat(mZ); - DataTransfer::sendByte(mA); - - if(!DataTransfer::endPacket()) - { - udpClientLogger.error("RawIMUData write end error: %d", Udp.getWriteError()); - } -} -#endif - -void returnLastPacket(int len) { - if(DataTransfer::beginPacket()) { - DataTransfer::sendBytes(incomingPacket, len); - DataTransfer::endPacket(); - } -} - -void updateSensorState(Sensor * const sensor, Sensor * const sensor2) { - if(millis() - lastSensorInfoPacket > 1000) { - lastSensorInfoPacket = millis(); - if(sensorStateNotified1 != sensor->getSensorState()) - Network::sendSensorInfo(sensor); - if(sensorStateNotified2 != sensor2->getSensorState()) - Network::sendSensorInfo(sensor2); - } -} - -bool ServerConnection::isConnected() { - return connected; -} - -void ServerConnection::connect() -{ - unsigned long now = millis(); - while(true) { - int packetSize = Udp.parsePacket(); - if (packetSize) - { - // receive incoming UDP packets - int len = Udp.read(incomingPacket, sizeof(incomingPacket)); - -#ifdef DEBUG_NETWORK - udpClientLogger.trace("Received %d bytes from %s, port %d", packetSize, Udp.remoteIP().toString().c_str(), Udp.remotePort()); - udpClientLogger.traceArray("UDP packet contents: ", incomingPacket, len); -#endif - - // Handshake is different, it has 3 in the first byte, not the 4th, and data starts right after - switch (incomingPacket[0]) - { - case PACKET_HANDSHAKE: - // Assume handshake successful, don't check it - // But proper handshake should contain "Hey OVR =D 5" ASCII string right after the packet number - // Starting on 14th byte (packet number, 12 bytes greetings, null-terminator) we can transfer SlimeVR handshake data - host = Udp.remoteIP(); - port = Udp.remotePort(); - lastPacketMs = now; - connected = true; - statusManager.setStatus(SlimeVR::Status::SERVER_CONNECTING, false); - ledManager.off(); - udpClientLogger.debug("Handshake successful, server is %s:%d", Udp.remoteIP().toString().c_str(), + Udp.remotePort()); - return; - default: - continue; - } - } - else - { - break; - } - } - if(lastConnectionAttemptMs + 1000 < now) - { - lastConnectionAttemptMs = now; - udpClientLogger.info("Looking for the server..."); - Network::sendHandshake(); - ledManager.on(); - } - else if(lastConnectionAttemptMs + 20 < now) - { - ledManager.off(); - } -} - -void ServerConnection::resetConnection() { - Udp.begin(port); - connected = false; - - statusManager.setStatus(SlimeVR::Status::SERVER_CONNECTING, true); -} - -void ServerConnection::update(Sensor * const sensor, Sensor * const sensor2) { - if(connected) { - int packetSize = Udp.parsePacket(); - if (packetSize) - { - lastPacketMs = millis(); - int len = Udp.read(incomingPacket, sizeof(incomingPacket)); - // receive incoming UDP packets - -#ifdef DEBUG_NETWORK - udpClientLogger.trace("Received %d bytes from %s, port %d", packetSize, Udp.remoteIP().toString().c_str(), Udp.remotePort()); - udpClientLogger.traceArray("UDP packet contents: ", incomingPacket, len); -#endif - - switch (convert_chars(incomingPacket)) - { - case PACKET_RECEIVE_HEARTBEAT: - Network::sendHeartbeat(); - break; - case PACKET_RECEIVE_VIBRATE: - - break; - case PACKET_RECEIVE_HANDSHAKE: - // Assume handshake successful - udpClientLogger.warn("Handshake received again, ignoring"); - break; - case PACKET_RECEIVE_COMMAND: - - break; - case PACKET_CONFIG: - - break; - case PACKET_PING_PONG: - returnLastPacket(len); - break; - case PACKET_SENSOR_INFO: - if(len < 6) { - udpClientLogger.warn("Wrong sensor info packet"); - break; - } - if(incomingPacket[4] == sensor->getSensorId()) { - sensorStateNotified1 = incomingPacket[5]; - } - else if(incomingPacket[4] == sensor2->getSensorId()) { - sensorStateNotified2 = incomingPacket[5]; - } - break; - } - } - //while(Serial.available()) { - // size_t bytesRead = Serial.readBytes(serialBuffer, min(Serial.available(), sizeof(serialBuffer))); - // sendSerial(serialBuffer, bytesRead, PACKET_SERIAL); - //} - if(lastPacketMs + TIMEOUT < millis()) - { - statusManager.setStatus(SlimeVR::Status::SERVER_CONNECTING, true); - - connected = false; - sensorStateNotified1 = false; - sensorStateNotified2 = false; - udpClientLogger.warn("Connection to server timed out"); - } - } - - if(!connected) { - connect(); - } else if(sensorStateNotified1 != sensor->isWorking() || sensorStateNotified2 != sensor2->isWorking()) { - updateSensorState(sensor, sensor2); - } -} diff --git a/src/network/udpclient.h b/src/network/udpclient.h deleted file mode 100644 index b569787..0000000 --- a/src/network/udpclient.h +++ /dev/null @@ -1,40 +0,0 @@ -/* - SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain & SlimeVR contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. -*/ -#ifndef SLIMEVR_UDP_CLIENT_H_ -#define SLIMEVR_UDP_CLIENT_H_ - -#include -#include -#include "quat.h" -#include "sensors/sensor.h" -#include "wifihandler.h" -#include "globals.h" - -namespace ServerConnection { - void connect(); - void update(Sensor * const sensor, Sensor * const sensor2); - void resetConnection(); - bool isConnected(); -} - -#endif // SLIMEVR_UDP_CLIENT_H_ \ No newline at end of file diff --git a/src/network/wifihandler.cpp b/src/network/wifihandler.cpp index 32509d2..e701a6a 100644 --- a/src/network/wifihandler.cpp +++ b/src/network/wifihandler.cpp @@ -21,7 +21,6 @@ THE SOFTWARE. */ #include "globals.h" -#include "network.h" #include "logging/Logger.h" #include "GlobalVars.h" #if !ESP8266 @@ -86,7 +85,7 @@ void WiFiNetwork::setUp() { wifiHandlerLogger.debug("Status: %d", status); wifiState = SLIME_WIFI_SAVED_ATTEMPT; wifiConnectionTimeout = millis(); - + #if ESP8266 #if POWERSAVING_MODE == POWER_SAVING_NONE WiFi.setSleepMode(WIFI_NONE_SLEEP); @@ -188,7 +187,7 @@ void WiFiNetwork::upkeep() { #endif wifiState = SLIME_WIFI_HARDCODE_G_ATTEMPT; return; - case SLIME_WIFI_SERVER_CRED_ATTEMPT: // Couldn't connect with server-sent credentials. + case SLIME_WIFI_SERVER_CRED_ATTEMPT: // Couldn't connect with server-sent credentials. #if ESP8266 // Try again silently but with 11G WiFi.setPhyMode(WIFI_PHY_MODE_11G); @@ -197,7 +196,7 @@ void WiFiNetwork::upkeep() { wifiConnectionTimeout = millis(); wifiState = SLIME_WIFI_SERVER_CRED_G_ATTEMPT; #endif - return; + return; case SLIME_WIFI_HARDCODE_G_ATTEMPT: // Couldn't connect with second set of credentials with PHY Mode G. case SLIME_WIFI_SERVER_CRED_G_ATTEMPT: // Or if couldn't connect with server-sent credentials // Return to the default PHY Mode N. @@ -224,7 +223,7 @@ void WiFiNetwork::upkeep() { if(millis() - last_rssi_sample >= 2000) { last_rssi_sample = millis(); uint8_t signalStrength = WiFi.RSSI(); - Network::sendSignalStrength(signalStrength); + networkConnection.sendSignalStrength(signalStrength); } } return; diff --git a/src/network/wifiprovisioning.cpp b/src/network/wifiprovisioning.cpp index 38d3075..bacf64d 100644 --- a/src/network/wifiprovisioning.cpp +++ b/src/network/wifiprovisioning.cpp @@ -20,7 +20,8 @@ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -#include "network.h" +#include "wifiprovisioning.h" +#include "wifihandler.h" #include "logging/Logger.h" // TODO Currently provisioning implemented via SmartConfig @@ -53,4 +54,4 @@ void WiFiNetwork::provideNeighbours() { bool WiFiNetwork::isProvisioning() { return provisioning && !WiFi.smartConfigDone(); -} \ No newline at end of file +} diff --git a/src/sensors/ErroneousSensor.cpp b/src/sensors/ErroneousSensor.cpp index 5303c5d..58bb3ab 100644 --- a/src/sensors/ErroneousSensor.cpp +++ b/src/sensors/ErroneousSensor.cpp @@ -22,7 +22,6 @@ */ #include "ErroneousSensor.h" -#include "network/network.h" #include "GlobalVars.h" namespace SlimeVR @@ -34,9 +33,9 @@ namespace SlimeVR m_Logger.error("IMU of type %s failed to initialize", getIMUNameByType(m_ExpectedType)); } - uint8_t ErroneousSensor::getSensorState() + SensorStatus ErroneousSensor::getSensorState() { - return SENSOR_ERROR; + return SensorStatus::SENSOR_ERROR; }; } } diff --git a/src/sensors/ErroneousSensor.h b/src/sensors/ErroneousSensor.h index 8e2e2e7..c298b7f 100644 --- a/src/sensors/ErroneousSensor.h +++ b/src/sensors/ErroneousSensor.h @@ -40,7 +40,7 @@ namespace SlimeVR void motionLoop() override final{}; void sendData() override{}; void startCalibration(int calibrationType) override final{}; - uint8_t getSensorState() override; + SensorStatus getSensorState() override; private: uint8_t m_ExpectedType; diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index afac9ff..fa5750e 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -23,7 +23,6 @@ #include "SensorManager.h" #include -#include "network/network.h" #include "bno055sensor.h" #include "bno080sensor.h" #include "mpu9250sensor.h" @@ -32,6 +31,7 @@ #include "icm20948sensor.h" #include "ErroneousSensor.h" #include "sensoraddresses.h" +#include "GlobalVars.h" namespace SlimeVR { @@ -128,7 +128,7 @@ namespace SlimeVR m_Sensor1->motionLoop(); m_Sensor2->motionLoop(); - if (!ServerConnection::isConnected()) + if (!networkConnection.isConnected()) { return; } diff --git a/src/sensors/bmi160sensor.cpp b/src/sensors/bmi160sensor.cpp index 35ee216..504160b 100644 --- a/src/sensors/bmi160sensor.cpp +++ b/src/sensors/bmi160sensor.cpp @@ -22,7 +22,6 @@ */ #include "bmi160sensor.h" -#include "network/network.h" #include "GlobalVars.h" #include #include @@ -205,7 +204,7 @@ void BMI160Sensor::motionSetup() { gscaleZ = BMI160_GSCALE * BMI160_CALCULATE_SENSITIVTY_MUL(offsets.z); m_Logger.debug("Custom sensitivity offset enabled: %s %s", offsets.mac, - offsets.sensorId == SENSORID_PRIMARY ? "primary" : "aux" + offsets.sensorId == SENSORID_PRIMARY ? "primary" : "aux" ); } } @@ -256,7 +255,7 @@ void BMI160Sensor::motionLoop() { getRemappedRotation(&rX, &rY, &rZ); getRemappedAcceleration(&aX, &aY, &aZ); - Network::sendInspectionRawIMUData(sensorId, rX, rY, rZ, 255, aX, aY, aZ, 255, 0, 0, 0, 255); + networkConnection.sendInspectionRawIMUData(sensorId, rX, rY, rZ, 255, aX, aY, aZ, 255, 0, 0, 0, 255); } #endif @@ -277,13 +276,13 @@ void BMI160Sensor::motionLoop() { sensorTime1 = rawSensorTime; if ((sensorTime0 > 0 || localTime0 > 0) && (sensorTime1 > 0 || sensorTime1 > 0)) { // handle 24 bit overflow - double remoteDt = + double remoteDt = sensorTime1 >= sensorTime0 ? sensorTime1 - sensorTime0 : (sensorTime1 + 0xFFFFFF) - sensorTime0; double localDt = localTime1 - localTime0; const double nextSensorTimeRatio = localDt / (remoteDt * BMI160_TIMESTAMP_RESOLUTION_MICROS); - + // handle sdk lags and time travel if (round(nextSensorTimeRatio) == 1.0) { sensorTimeRatio = nextSensorTimeRatio; @@ -307,7 +306,7 @@ void BMI160Sensor::motionLoop() { optimistic_yield(100); } } - + { uint32_t now = micros(); constexpr uint32_t BMI160_TARGET_POLL_INTERVAL_MICROS = 6000; @@ -368,9 +367,9 @@ void BMI160Sensor::motionLoop() { lastTemperaturePacketSent = now - (elapsed - sendInterval); #if BMI160_TEMPCAL_DEBUG uint32_t isCalibrating = gyroTempCalibrator->isCalibrating() ? 10000 : 0; - Network::sendTemperature(isCalibrating + 10000 + (gyroTempCalibrator->config.samplesTotal * 100) + temperature, sensorId); + networkConnection.sendTemperature(sensorId, isCalibrating + 10000 + (gyroTempCalibrator->config.samplesTotal * 100) + temperature); #else - Network::sendTemperature(temperature, sensorId); + networkConnection.sendTemperature(sensorId, temperature); #endif optimistic_yield(100); } @@ -391,7 +390,7 @@ void BMI160Sensor::motionLoop() { vqf.getQuat9D(qwxyz); #endif #endif - + if (isnan(qwxyz[0]) || isnan(qwxyz[1]) || isnan(qwxyz[2]) || isnan(qwxyz[3])) { qwxyz[0] = 1; qwxyz[1] = 0; @@ -469,11 +468,11 @@ void BMI160Sensor::readFIFO() { for (uint32_t i = 0; i < fifo.length;) { #define BMI160_FIFO_FRAME_ENSURE_BYTES_AVAILABLE(len) { if (i + len > fifo.length) break; } BMI160_FIFO_FRAME_ENSURE_BYTES_AVAILABLE(1); - + // ignore interrupt tags in header header = fifo.data[i] & 0b11111100; i++; - + if (header == BMI160_FIFO_HEADER_CTL_SKIP_FRAME) { BMI160_FIFO_FRAME_ENSURE_BYTES_AVAILABLE(BMI160_FIFO_SKIP_FRAME_LEN); break; @@ -502,7 +501,7 @@ void BMI160Sensor::readFIFO() { #endif i += BMI160_FIFO_M_LEN; } - + // bmi160 -> 0 lsb 1 msb // gyro if (header & BMI160_FIFO_HEADER_DATA_FRAME_FLAG_G) { @@ -539,7 +538,7 @@ void BMI160Sensor::readFIFO() { timestamp1 = (localTime1 - alignmentOffset - syncLatencyMicros) + (++samplesSinceClockSync) * sampleDtMicros; int32_t dtMicros = timestamp1 - timestamp0; - + constexpr float invPeriod = 1.0f / BMI160_ODR_GYR_MICROS; int32_t sampleOffset = round((float)dtMicros * invPeriod) - 1; if (abs(sampleOffset) > 3) { @@ -788,7 +787,7 @@ void BMI160Sensor::startCalibration(int calibrationType) { maybeCalibrateGyro(); maybeCalibrateAccel(); maybeCalibrateMag(); - + m_Logger.debug("Saving the calibration data"); SlimeVR::Configuration::CalibrationConfig calibration; @@ -950,7 +949,7 @@ void BMI160Sensor::maybeCalibrateAccel() { delayMicroseconds(BMI160_ODR_ACC_MICROS); continue; } - + if (calibrationRestDetection.getRestDetected()) { const uint16_t i = numCurrentPositionSamples * 3; accelCalibrationChunk[i + 0] = ax; @@ -1029,7 +1028,7 @@ void BMI160Sensor::maybeCalibrateMag() { delay(100); ledManager.on(); m_Logger.debug("Gathering magnetometer data..."); - + constexpr float SAMPLE_DELAY_MS = 100.0f; constexpr uint16_t magCalibrationSamples = MAG_CALIBRATION_DURATION_SEC / (SAMPLE_DELAY_MS / 1e3f); @@ -1108,4 +1107,4 @@ void BMI160Sensor::getMagnetometerXYZFromBuffer(uint8_t* data, int16_t* x, int16 *y = ((int16_t)data[3] << 8) | data[2]; *z = ((int16_t)data[5] << 8) | data[4]; #endif -} \ No newline at end of file +} diff --git a/src/sensors/bno055sensor.cpp b/src/sensors/bno055sensor.cpp index f5857cf..97787b3 100644 --- a/src/sensors/bno055sensor.cpp +++ b/src/sensors/bno055sensor.cpp @@ -21,7 +21,6 @@ THE SOFTWARE. */ #include "bno055sensor.h" -#include "network/network.h" #include "globals.h" #include "GlobalVars.h" @@ -56,7 +55,7 @@ void BNO055Sensor::motionLoop() { Vector3 accel = imu.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL); Vector3 mag = imu.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER); - Network::sendInspectionRawIMUData(sensorId, UNPACK_VECTOR(gyro), 255, UNPACK_VECTOR(accel), 255, UNPACK_VECTOR(mag), 255); + networkConnection.sendInspectionRawIMUData(sensorId, UNPACK_VECTOR(gyro), 255, UNPACK_VECTOR(accel), 255, UNPACK_VECTOR(mag), 255); } #endif @@ -83,4 +82,4 @@ void BNO055Sensor::motionLoop() { void BNO055Sensor::startCalibration(int calibrationType) { -} \ No newline at end of file +} diff --git a/src/sensors/bno080sensor.cpp b/src/sensors/bno080sensor.cpp index d33a433..3772d20 100644 --- a/src/sensors/bno080sensor.cpp +++ b/src/sensors/bno080sensor.cpp @@ -22,7 +22,6 @@ */ #include "sensors/bno080sensor.h" -#include "network/network.h" #include "utils.h" #include "GlobalVars.h" @@ -42,13 +41,13 @@ void BNO080Sensor::motionSetup() "SW Version Minor: 0x%02x " "SW Part Number: 0x%02x " "SW Build Number: 0x%02x " - "SW Version Patch: 0x%02x", - getIMUNameByType(sensorType), - addr, - imu.swMajor, - imu.swMinor, - imu.swPartNumber, - imu.swBuildNumber, + "SW Version Patch: 0x%02x", + getIMUNameByType(sensorType), + addr, + imu.swMajor, + imu.swMinor, + imu.swPartNumber, + imu.swBuildNumber, imu.swVersionPatch ); @@ -107,7 +106,7 @@ void BNO080Sensor::motionLoop() int16_t mZ = imu.getRawMagZ(); uint8_t mA = imu.getMagAccuracy(); - Network::sendInspectionRawIMUData(sensorId, rX, rY, rZ, rA, aX, aY, aZ, aA, mX, mY, mZ, mA); + networkConnection.sendInspectionRawIMUData(sensorId, rX, rY, rZ, rA, aX, aY, aZ, aA, mX, mY, mZ, mA); } #endif @@ -160,7 +159,7 @@ void BNO080Sensor::motionLoop() #if ENABLE_INSPECTION { - Network::sendInspectionCorrectionData(sensorId, quaternion); + networkConnection.sendInspectionCorrectionData(sensorId, quaternion); } #endif // ENABLE_INSPECTION @@ -192,15 +191,16 @@ void BNO080Sensor::motionLoop() if (rr != lastReset) { lastReset = rr; - Network::sendError(rr, this->sensorId); + networkConnection.sendSensorError(this->sensorId, rr); } + m_Logger.error("Sensor %d doesn't respond. Last reset reason:", sensorId, lastReset); m_Logger.error("Last error: %d, seq: %d, src: %d, err: %d, mod: %d, code: %d", lastError.severity, lastError.error_sequence_number, lastError.error_source, lastError.error, lastError.error_module, lastError.error_code); } } -uint8_t BNO080Sensor::getSensorState() { +SensorStatus BNO080Sensor::getSensorState() { return lastReset > 0 ? SensorStatus::SENSOR_ERROR : isWorking() ? SensorStatus::SENSOR_OK : SensorStatus::SENSOR_OFFLINE; } @@ -209,7 +209,7 @@ void BNO080Sensor::sendData() if (newFusedRotation) { newFusedRotation = false; - Network::sendRotationData(&fusedRotation, DATA_TYPE_NORMAL, calibrationAccuracy, sensorId); + networkConnection.sendRotationData(sensorId, &fusedRotation, DATA_TYPE_NORMAL, calibrationAccuracy); #ifdef DEBUG_SENSOR m_Logger.trace("Quaternion: %f, %f, %f, %f", UNPACK_QUATERNION(fusedRotation)); @@ -220,26 +220,26 @@ void BNO080Sensor::sendData() if(newAcceleration) { newAcceleration = false; - Network::sendAccel(this->acceleration, this->sensorId); + networkConnection.sendSensorAcceleration(this->sensorId, this->acceleration); } #endif #if !USE_6_AXIS - Network::sendMagnetometerAccuracy(magneticAccuracyEstimate, sensorId); + networkConnection.sendMagnetometerAccuracy(sensorId, magneticAccuracyEstimate); #endif #if USE_6_AXIS && BNO_USE_MAGNETOMETER_CORRECTION if (newMagData) { newMagData = false; - Network::sendRotationData(&magQuaternion, DATA_TYPE_CORRECTION, magCalibrationAccuracy, sensorId); - Network::sendMagnetometerAccuracy(magneticAccuracyEstimate, sensorId); + networkConnection.sendRotationData(sensorId, &magQuaternion, DATA_TYPE_CORRECTION, magCalibrationAccuracy); + networkConnection.sendMagnetometerAccuracy(sensorId, magneticAccuracyEstimate); } #endif if (tap != 0) { - Network::sendTap(tap, sensorId); + networkConnection.sendSensorTap(sensorId, tap); tap = 0; } } @@ -249,4 +249,4 @@ void BNO080Sensor::startCalibration(int calibrationType) // BNO does automatic calibration, // it's always enabled except accelerometer // that is disabled 30 seconds after startup -} \ No newline at end of file +} diff --git a/src/sensors/bno080sensor.h b/src/sensors/bno080sensor.h index e7cce64..9bb0864 100644 --- a/src/sensors/bno080sensor.h +++ b/src/sensors/bno080sensor.h @@ -41,7 +41,7 @@ public: void motionLoop() override final; void sendData() override final; void startCalibration(int calibrationType) override final; - uint8_t getSensorState() override final; + SensorStatus getSensorState() override final; private: BNO080 imu{}; diff --git a/src/sensors/icm20948sensor.cpp b/src/sensors/icm20948sensor.cpp index cc88c98..63d235f 100644 --- a/src/sensors/icm20948sensor.cpp +++ b/src/sensors/icm20948sensor.cpp @@ -23,7 +23,6 @@ #include "icm20948sensor.h" #include "calibration.h" #include -#include "network/network.h" #include "GlobalVars.h" // seconds after previous save (from start) when calibration (DMP Bias) data will be saved to NVS. Increments through the list then stops; to prevent unwelcome eeprom wear. @@ -61,7 +60,7 @@ void ICM20948Sensor::motionLoop() float mY = imu.magY(); float mZ = imu.magZ(); - Network::sendInspectionRawIMUData(sensorId, rX, rY, rZ, 255, aX, aY, aZ, 255, mX, mY, mZ, 255); + networkConnection.sendInspectionRawIMUData(sensorId, rX, rY, rZ, 255, aX, aY, aZ, 255, mX, mY, mZ, 255); } #endif @@ -96,11 +95,11 @@ void ICM20948Sensor::sendData() #if(USE_6_AXIS) { - Network::sendRotationData(&fusedRotation, DATA_TYPE_NORMAL, 0, sensorId); + networkConnection.sendRotationData(sensorId, &fusedRotation, DATA_TYPE_NORMAL, 0); } #else { - Network::sendRotationData(&fusedRotation, DATA_TYPE_NORMAL, dmpData.Quat9.Data.Accuracy, sensorId); + Network::sendRotationData(sensorId, &fusedRotation, DATA_TYPE_NORMAL, dmpData.Quat9.Data.Accuracy); } #endif } @@ -108,7 +107,7 @@ void ICM20948Sensor::sendData() #if SEND_ACCELERATION if(newAcceleration) { newAcceleration = false; - Network::sendAccel(acceleration, sensorId); + networkConnection.sendSensorAcceleration(sensorId, acceleration); } #endif } @@ -300,7 +299,7 @@ void ICM20948Sensor::checkSensorTimeout() working = false; lastData = millis(); m_Logger.error("Sensor timeout I2C Address 0x%02x", addr); - Network::sendError(1, this->sensorId); + networkConnection.sendSensorError(this->sensorId, 1); } } diff --git a/src/sensors/mpu6050sensor.cpp b/src/sensors/mpu6050sensor.cpp index e90b105..847a6eb 100644 --- a/src/sensors/mpu6050sensor.cpp +++ b/src/sensors/mpu6050sensor.cpp @@ -30,7 +30,6 @@ #endif #include "mpu6050sensor.h" -#include "network/network.h" #include #include "calibration.h" #include "GlobalVars.h" @@ -127,7 +126,7 @@ void MPU6050Sensor::motionLoop() imu.getRotation(&rX, &rY, &rZ); imu.getAcceleration(&aX, &aY, &aZ); - Network::sendInspectionRawIMUData(sensorId, rX, rY, rZ, 255, aX, aY, aZ, 255, 0, 0, 0, 255); + networkConnection.sendInspectionRawIMUData(sensorId, rX, rY, rZ, 255, aX, aY, aZ, 255, 0, 0, 0, 255); } #endif @@ -176,17 +175,6 @@ void MPU6050Sensor::startCalibration(int calibrationType) { #ifdef IMU_MPU6050_RUNTIME_CALIBRATION m_Logger.info("MPU is using automatic runtime calibration. Place down the device and it should automatically calibrate after a few seconds"); - - // Lie to the server and say we've calibrated - switch (calibrationType) - { - case CALIBRATION_TYPE_INTERNAL_ACCEL: - Network::sendCalibrationFinished(CALIBRATION_TYPE_INTERNAL_ACCEL, 0); - break; - case CALIBRATION_TYPE_INTERNAL_GYRO: - Network::sendCalibrationFinished(CALIBRATION_TYPE_INTERNAL_GYRO, 0);//was CALIBRATION_TYPE_INTERNAL_GYRO for some reason? there wasn't a point to this switch - break; - } #else //!IMU_MPU6050_RUNTIME_CALIBRATION m_Logger.info("Put down the device and wait for baseline gyro reading calibration"); delay(2000); @@ -202,14 +190,12 @@ void MPU6050Sensor::startCalibration(int calibrationType) { { case CALIBRATION_TYPE_INTERNAL_ACCEL: imu.CalibrateAccel(10); - Network::sendCalibrationFinished(CALIBRATION_TYPE_INTERNAL_ACCEL, 0);//doesn't send calibration data anymore, has that been depricated in server? m_Calibration.A_B[0] = imu.getXAccelOffset(); m_Calibration.A_B[1] = imu.getYAccelOffset(); m_Calibration.A_B[2] = imu.getZAccelOffset(); break; case CALIBRATION_TYPE_INTERNAL_GYRO: imu.CalibrateGyro(10); - Network::sendCalibrationFinished(CALIBRATION_TYPE_INTERNAL_GYRO, 0);//doesn't send calibration data anymore m_Calibration.G_off[0] = imu.getXGyroOffset(); m_Calibration.G_off[1] = imu.getYGyroOffset(); m_Calibration.G_off[2] = imu.getZGyroOffset(); diff --git a/src/sensors/mpu9250sensor.cpp b/src/sensors/mpu9250sensor.cpp index 976fadc..64320c2 100644 --- a/src/sensors/mpu9250sensor.cpp +++ b/src/sensors/mpu9250sensor.cpp @@ -20,7 +20,6 @@ */ #include "mpu9250sensor.h" -#include "network/network.h" #include "globals.h" #include "helper_3dmath.h" #include @@ -148,7 +147,7 @@ void MPU9250Sensor::motionLoop() { imu.getAcceleration(&aX, &aY, &aZ); imu.getMagnetometer(&mX, &mY, &mZ); - Network::sendInspectionRawIMUData(sensorId, rX, rY, rZ, 255, aX, aY, aZ, 255, mX, mY, mZ, 255); + networkConnection.sendInspectionRawIMUData(sensorId, rX, rY, rZ, 255, aX, aY, aZ, 255, mX, mY, mZ, 255); } #endif @@ -229,7 +228,7 @@ void MPU9250Sensor::motionLoop() { madgwickQuaternionUpdate(q, Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], Mxyz[0], Mxyz[1], Mxyz[2], deltat * 1.0e-6); #endif } - + quaternion.set(-q[2], q[1], q[3], q[0]); #endif @@ -258,7 +257,6 @@ void MPU9250Sensor::startCalibration(int calibrationType) { magneto->sample(my, mx, -mz); float rawMagFloat[3] = { (float)mx, (float)my, (float)mz}; - Network::sendRawCalibrationData(rawMagFloat, CALIBRATION_TYPE_EXTERNAL_MAG, 0); ledManager.off(); delay(250); } @@ -293,7 +291,7 @@ void MPU9250Sensor::startCalibration(int calibrationType) { delay(2000); union fifo_sample_raw buf; - + imu.resetFIFO(); // fifo is sure to have filled up in the seconds of delay, don't try reading it. for (int i = 0; i < calibrationSamples; i++) { // wait for new sample @@ -311,7 +309,6 @@ void MPU9250Sensor::startCalibration(int calibrationType) { m_Logger.trace("Gyro calibration results: %f %f %f", Gxyz[0], Gxyz[1], Gxyz[2]); #endif - Network::sendRawCalibrationData(Gxyz, CALIBRATION_TYPE_EXTERNAL_GYRO, 0); // TODO: use offset registers? m_Calibration.G_off[0] = Gxyz[0]; m_Calibration.G_off[1] = Gxyz[1]; @@ -336,11 +333,9 @@ void MPU9250Sensor::startCalibration(int calibrationType) { // we could make the server run magneto for us. // TODO: consider moving the sample reporting into magneto itself? float rawAccFloat[3] = { (float)ax, (float)ay, (float)az }; - Network::sendRawCalibrationData(rawAccFloat, CALIBRATION_TYPE_EXTERNAL_ACCEL, 0); float rawMagFloat[3] = { (float)mx, (float)my, (float)-mz }; - Network::sendRawCalibrationData(rawMagFloat, CALIBRATION_TYPE_EXTERNAL_MAG, 0); - + ledManager.off(); delay(250); } @@ -387,7 +382,6 @@ void MPU9250Sensor::startCalibration(int calibrationType) { configuration.save(); ledManager.off(); - Network::sendCalibrationFinished(CALIBRATION_TYPE_EXTERNAL_ALL, 0); m_Logger.debug("Saved the calibration data"); m_Logger.info("Calibration data gathered"); @@ -488,4 +482,4 @@ bool MPU9250Sensor::getNextSample(union fifo_sample_raw *buffer, uint16_t *remai imu.getFIFOBytes(buffer->raw, sensor_data_len); swapFifoData(buffer); return true; -} \ No newline at end of file +} diff --git a/src/sensors/sensor.cpp b/src/sensors/sensor.cpp index 91c7f93..45e9b07 100644 --- a/src/sensors/sensor.cpp +++ b/src/sensors/sensor.cpp @@ -21,18 +21,18 @@ THE SOFTWARE. */ #include "sensor.h" -#include "network/network.h" +#include "GlobalVars.h" #include #include "calibration.h" -uint8_t Sensor::getSensorState() { +SensorStatus Sensor::getSensorState() { return isWorking() ? SensorStatus::SENSOR_OK : SensorStatus::SENSOR_OFFLINE; } void Sensor::sendData() { if(newFusedRotation) { newFusedRotation = false; - Network::sendRotationData(&fusedRotation, DATA_TYPE_NORMAL, calibrationAccuracy, sensorId); + networkConnection.sendRotationData(sensorId, &fusedRotation, DATA_TYPE_NORMAL, calibrationAccuracy); #ifdef DEBUG_SENSOR m_Logger.trace("Quaternion: %f, %f, %f, %f", UNPACK_QUATERNION(quaternion)); @@ -42,7 +42,7 @@ void Sensor::sendData() { #if SEND_ACCELERATION if(newAcceleration) { newAcceleration = false; - Network::sendAccel(acceleration, sensorId); + networkConnection.sendSensorAcceleration(sensorId, acceleration); } #endif } diff --git a/src/sensors/sensor.h b/src/sensors/sensor.h index 25b535e..b19a236 100644 --- a/src/sensors/sensor.h +++ b/src/sensors/sensor.h @@ -35,6 +35,12 @@ #define DATA_TYPE_NORMAL 1 #define DATA_TYPE_CORRECTION 2 +enum class SensorStatus : uint8_t { + SENSOR_OFFLINE = 0, + SENSOR_OK = 1, + SENSOR_ERROR = 2 +}; + class Sensor { public: @@ -52,7 +58,7 @@ public: virtual void motionLoop(){}; virtual void sendData(); virtual void startCalibration(int calibrationType){}; - virtual uint8_t getSensorState(); + virtual SensorStatus getSensorState(); virtual void printTemperatureCalibrationState(); virtual void printDebugTemperatureCalibrationState(); virtual void resetTemperatureCalibrationState(); @@ -96,11 +102,4 @@ private: const char * getIMUNameByType(int imuType); -enum SensorStatus { - SENSOR_OFFLINE = 0, - SENSOR_OK = 1, - SENSOR_ERROR = 2 -}; - - #endif // SLIMEVR_SENSOR_H_ diff --git a/src/serial/serialcommands.cpp b/src/serial/serialcommands.cpp index cd7095b..b82421c 100644 --- a/src/serial/serialcommands.cpp +++ b/src/serial/serialcommands.cpp @@ -22,7 +22,6 @@ */ #include "serialcommands.h" -#include "network/network.h" #include "logging/Logger.h" #include #include "GlobalVars.h" @@ -88,14 +87,14 @@ namespace SerialCommands { if (parser->getParamCount() < 2) { return; } - + if (parser->equalCmdParam(1, "INFO")) { printState(); } if (parser->equalCmdParam(1, "CONFIG")) { String str = - "BOARD=%d\n" + "BOARD=%d\n" "IMU=%d\n" "SECOND_IMU=%d\n" "IMU_ROTATION=%f\n" @@ -220,7 +219,7 @@ namespace SerialCommands { logger.info("Note:"); logger.info(" Temperature calibration config saves automatically when calibration percent is at 100%"); } - + void setUp() { cmdCallbacks.addCmd("SET", &cmdSet); cmdCallbacks.addCmd("GET", &cmdGet);