github/SlimeVR-Tracker-ESP
1
1
Fork 0
mirror of https://github.com/SlimeVR/SlimeVR-Tracker-ESP.git synced 2026-04-06 02:01:57 +02:00
Code Issues 76 Packages Projects Releases Wiki Activity

Fix calibration code

Browse Source
This commit is contained in:
Eiren Rain
2021-01-08 00:17:34 +03:00
parent efa194a7c0
commit d0104fd67f
2 changed files with 67 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files

114
include/udpclient.cpp
View File

@@ -1,7 +1,7 @@
#include "udpclient.h"
#include "configuration.h"
#define TIMEOUT 3000
#define TIMEOUT 3000UL
WiFiUDP Udp;
unsigned char incomingPacket[128]; // buffer for incoming packets
@@ -16,8 +16,8 @@ IPAddress broadcast = IPAddress(255, 255, 255, 255);
int port = 6969;
IPAddress host;
bool connected = false;
long lastConnectionAttemptMs;
long lastPacketMs;
unsigned long lastConnectionAttemptMs;
unsigned long lastPacketMs;
template <typename T>
unsigned char * convert_to_chars(T src, unsigned char * target)
@@ -219,58 +219,64 @@ void clientUpdate()
{
if (WiFi.status() == WL_CONNECTED)
{
int packetSize = Udp.parsePacket();
if (packetSize)
{
lastPacketMs = millis();
// receive incoming UDP packets
Serial.printf("Received %d bytes from %s, port %d\n", packetSize, Udp.remoteIP().toString().c_str(), Udp.remotePort());
int len = Udp.read(incomingPacket, sizeof(incomingPacket));
Serial.print("UDP packet contents: ");
for (int i = 0; i < len; ++i)
Serial.print((byte)incomingPacket[i]);
Serial.println();
switch (convert_chars<int>(incomingPacket))
if(connected) {
int packetSize = Udp.parsePacket();
if (packetSize)
{
case PACKET_RECIEVE_HEARTBEAT:
break;
case PACKET_RECIEVE_VIBRATE:
if(fp_commandCallback) {
fp_commandCallback(COMMAND_BLINK, nullptr, 0);
}
break;
case PACKET_RECIEVE_HANDSHAKE:
// Assume handshake sucessful
Serial.println("Handshale recived again, ignoring");
break;
case PACKET_RECIEVE_COMMAND:
if (len < 6)
lastPacketMs = millis();
// receive incoming UDP packets
Serial.printf("Received %d bytes from %s, port %d\n", packetSize, Udp.remoteIP().toString().c_str(), Udp.remotePort());
int len = Udp.read(incomingPacket, sizeof(incomingPacket));
Serial.print("UDP packet contents: ");
for (int i = 0; i < len; ++i)
Serial.print((byte)incomingPacket[i]);
Serial.println();
switch (convert_chars<int>(incomingPacket))
{
Serial.println("Command packet too short");
case PACKET_RECIEVE_HEARTBEAT:
break;
case PACKET_RECIEVE_VIBRATE:
if(fp_commandCallback) {
fp_commandCallback(COMMAND_BLINK, nullptr, 0);
}
break;
case PACKET_RECIEVE_HANDSHAKE:
// Assume handshake sucessful
Serial.println("Handshale recived again, ignoring");
break;
case PACKET_RECIEVE_COMMAND:
if (len < 6)
{
Serial.println("Command packet too short");
break;
}
Serial.printf("Recieved command %d\n", incomingPacket[4]);
if (fp_commandCallback)
{
fp_commandCallback(incomingPacket[4], &incomingPacket[5], len - 6);
}
break;
case PACKET_CONFIG:
if (len < sizeof(DeviceConfig) + 4)
{
Serial.println("config packet too short");
break;
}
if (fp_configCallback)
{
fp_configCallback(convert_chars<DeviceConfig>(&incomingPacket[4]));
}
break;
}
Serial.printf("Recieved command %d\n", incomingPacket[4]);
if (fp_commandCallback)
{
fp_commandCallback(incomingPacket[4], &incomingPacket[5], len - 6);
}
break;
case PACKET_CONFIG:
if (len < sizeof(DeviceConfig) + 4)
{
Serial.println("config packet too short");
break;
}
if (fp_configCallback)
{
fp_configCallback(convert_chars<DeviceConfig>(&incomingPacket[4]));
}
break;
}
if(lastPacketMs + TIMEOUT < millis())
{
connected = false;
Serial.println("Connection to server timed out");
}
}
if(connected && (lastPacketMs + TIMEOUT < millis()))
connected = false;
if(!connected)
connectClient();
}
@@ -330,15 +336,15 @@ void setUpWiFi(DeviceConfig * const config) {
void connectClient()
{
long now = millis();
unsigned long now = millis();
while(true) {
int packetSize = Udp.parsePacket();
if (packetSize)
{
// receive incoming UDP packets
Serial.printf("Received %d bytes from %s, port %d\n", packetSize, Udp.remoteIP().toString().c_str(), Udp.remotePort());
Serial.printf("[Handshake] Received %d bytes from %s, port %d\n", packetSize, Udp.remoteIP().toString().c_str(), Udp.remotePort());
int len = Udp.read(incomingPacket, sizeof(incomingPacket));
Serial.print("UDP packet contents: ");
Serial.print("[Handshake] UDP packet contents: ");
for (int i = 0; i < len; ++i)
Serial.print((byte)incomingPacket[i]);
Serial.println();
@@ -352,8 +358,10 @@ void connectClient()
lastPacketMs = now;
connected = true;
digitalWrite(LOADING_LED, HIGH);
Serial.printf("Handshale sucessful, server is %s:%d", Udp.remoteIP().toString().c_str(), + Udp.remotePort());
Serial.printf("[Handshake] Handshale sucessful, server is %s:%d\n", Udp.remoteIP().toString().c_str(), + Udp.remotePort());
return;
default:
continue;
}
}
else

10
src/main.cpp
View File

@@ -163,6 +163,7 @@ void loop()
if (isCalibrating)
{
performCalibration();
isCalibrating = false;
}
get_MPU_scaled();
now = micros();
@@ -348,11 +349,12 @@ void performCalibration()
// Blink calibrating led before user should rotate the sensor
Serial.println("Gently rotate the device while it's gathering accelerometer and magnetometer data");
for (int i = 0; i < 2000 / 20; ++i)
for (int i = 0; i < 3000 / 310; ++i)
{
digitalWrite(CALIBRATING_LED, HIGH);
delay(2000 / 10);
digitalWrite(CALIBRATING_LED, LOW);
delay(15);
digitalWrite(CALIBRATING_LED, HIGH);
delay(300);
}
int calibrationData[6];
for (int i = 0; i < calibrationSamples; i++)
@@ -367,7 +369,7 @@ void performCalibration()
calibrationData[5] = mz;
sendRawCalibrationData(calibrationData, PACKET_RAW_CALIBRATION_DATA);
digitalWrite(CALIBRATING_LED, HIGH);
delay(300);
delay(250);
}
Serial.println("Calibration data gathered and sent");
digitalWrite(CALIBRATING_LED, HIGH);