Skip to content

Commit

Permalink
Merge remote-tracking branch 'upstream/cleanup-feature-flags' into de…
Browse files Browse the repository at this point in the history 
…v_testing
  • Loading branch information
@unlogisch04
unlogisch04 committed Jun 21, 2024
2 parents e4742ff + 78d4a1d commit 1a0e05a
Show file tree
Hide file tree
Showing 3 changed files with 221 additions and 181 deletions.
180 changes: 100 additions & 80 deletions src/network/connection.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,7 +67,11 @@ namespace Network {

bool Connection::beginPacket() {
if (m_IsBundle) {
m_BundlePacketPosition = 0;
m_BundleInnerStart = m_BundleInnerPosition;
// We have to advance two bytes, since the first two bytes are reserved for the
// packet length
m_BundleInnerPosition += 2;

return true;
}

Expand All @@ -84,21 +88,14 @@ bool Connection::beginPacket() {

bool Connection::endPacket() {
if (m_IsBundle) {
uint32_t innerPacketSize = m_BundlePacketPosition;

MUST_TRANSFER_BOOL((innerPacketSize > 0));
auto innerPacketSize = getBundleInnerSize();

m_IsBundle = false;
// We have to go back to the start of the packet and write the size
convert_to_chars((uint16_t)innerPacketSize, m_Buf);
memcpy(m_BundleInnerStart, m_Buf, 2);

if (m_BundlePacketInnerCount == 0) {
sendPacketType(PACKET_BUNDLE);
sendPacketNumber();
}
sendShort(innerPacketSize);
sendBytes(m_Packet, innerPacketSize);
m_BundlePacketCount++;

m_BundlePacketInnerCount++;
m_IsBundle = true;
return true;
}

Expand All @@ -114,40 +111,68 @@ bool Connection::endPacket() {
}

bool Connection::beginBundle() {
MUST_TRANSFER_BOOL(m_ServerFeatures.has(ServerFeatures::PROTOCOL_BUNDLE_SUPPORT));
MUST_TRANSFER_BOOL(m_ServerFeatures.has(EServerFeatureFlags::PROTOCOL_BUNDLE_SUPPORT
));

memset(m_Bundle, 0, sizeof(m_Bundle));
m_BundleInnerPosition = m_Bundle;
m_BundlePacketCount = 0;

MUST_TRANSFER_BOOL(m_Connected);
MUST_TRANSFER_BOOL(!m_IsBundle);
MUST_TRANSFER_BOOL(beginPacket());

m_IsBundle = true;
m_BundlePacketInnerCount = 0;

return true;
}

bool Connection::endBundle() {
MUST_TRANSFER_BOOL(m_IsBundle);
bool Connection::sendBundle() {
MUST_TRANSFER_BOOL(m_ServerFeatures.has(EServerFeatureFlags::PROTOCOL_BUNDLE_SUPPORT
));
MUST_TRANSFER_BOOL(!m_IsBundle);
MUST_TRANSFER_BOOL((m_BundlePacketCount > 0));

MUST_TRANSFER_BOOL(beginPacket());

MUST_TRANSFER_BOOL(sendPacketType(PACKET_BUNDLE));
MUST_TRANSFER_BOOL(sendPacketNumber());
MUST_TRANSFER_BOOL(write(m_Bundle, getBundleSize()));

MUST_TRANSFER_BOOL(endPacket());

return true;
}

bool Connection::endBundle() {
m_IsBundle = false;

MUST_TRANSFER_BOOL((m_BundlePacketInnerCount > 0));
auto ret = sendBundle();

return endPacket();
memset(m_Buf, 0, sizeof(m_Buf));
m_BundleInnerStart = m_Buf;
m_BundleInnerPosition = m_Buf;
m_BundlePacketCount = 0;

return ret;
}

size_t Connection::write(const uint8_t *buffer, size_t size) {
size_t Connection::write(const uint8_t* buffer, size_t size) {
if (m_IsBundle) {
if (m_BundlePacketPosition + size > sizeof(m_Packet)) {
if (getBundleSize() + size > MAX_BUNDLE_SIZE) {
m_Logger.error("Bundled packet too large");

// TODO: Drop the currently forming packet

return 0;
}
memcpy(m_Packet + m_BundlePacketPosition, buffer, size);
m_BundlePacketPosition += size;

memcpy(m_BundleInnerPosition, buffer, size);
m_BundleInnerPosition += size;

return size;
}
return m_UDP.write(buffer, size);
}

size_t Connection::write(uint8_t byte) {
return write(&byte, 1);
return m_UDP.write(buffer, size);
}

bool Connection::sendFloat(float f) {
Expand All @@ -158,19 +183,19 @@ bool Connection::sendFloat(float f) {

bool Connection::sendByte(uint8_t c) { return write(&c, 1) != 0; }

bool Connection::sendShort(uint16_t i) {
bool Connection::sendU16(uint16_t i) {
convert_to_chars(i, m_Buf);

return write(m_Buf, sizeof(i)) != 0;
}

bool Connection::sendInt(uint32_t i) {
bool Connection::sendI32(int32_t i) {
convert_to_chars(i, m_Buf);

return write(m_Buf, sizeof(i)) != 0;
}

bool Connection::sendLong(uint64_t l) {
bool Connection::sendU64(uint64_t l) {
convert_to_chars(l, m_Buf);

return write(m_Buf, sizeof(l)) != 0;
Expand All @@ -185,9 +210,9 @@ bool Connection::sendPacketNumber() {
return true;
}

uint64_t pn = m_PacketNumber++;
auto pn = m_PacketNumber++;

return sendLong(pn);
return sendU64(pn);
}

bool Connection::sendShortString(const char* str) {
Expand All @@ -199,18 +224,12 @@ bool Connection::sendShortString(const char* str) {
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::sendPacketType(int32_t type) { return sendI32(type); }

bool Connection::sendLongString(const char* str) {
int size = strlen(str);

MUST_TRANSFER_BOOL(sendInt(size));
MUST_TRANSFER_BOOL(sendI32(size));

return sendBytes((const uint8_t*)str, size);
}
Expand Down Expand Up @@ -376,7 +395,8 @@ void Connection::sendFeatureFlags() {

MUST(sendPacketType(PACKET_FEATURE_FLAGS));
MUST(sendPacketNumber());
MUST(write(FirmwareFeatures::flags.data(), FirmwareFeatures::flags.size()));
auto packedFeatures = m_FirmwareFeatures.pack();
MUST(write(packedFeatures.data(), packedFeatures.size()));

MUST(endPacket());
}
Expand All @@ -391,17 +411,17 @@ void Connection::sendTrackerDiscovery() {

MUST(sendPacketType(PACKET_HANDSHAKE));
// Packet number is always 0 for handshake
MUST(sendLong(0));
MUST(sendInt(BOARD));
MUST(sendU64(0));
MUST(sendI32(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(static_cast<int>(sensorManager.getSensorType(0))));
MUST(sendInt(HARDWARE_MCU));
MUST(sendInt(0));
MUST(sendInt(0));
MUST(sendInt(0));
MUST(sendInt(FIRMWARE_BUILD_NUMBER));
MUST(sendI32(static_cast<int>(sensorManager.getSensorType(0))));
MUST(sendI32(HARDWARE_MCU));
MUST(sendI32(0));
MUST(sendI32(0));
MUST(sendI32(0));
MUST(sendI32(FIRMWARE_BUILD_NUMBER));
MUST(sendShortString(FIRMWARE_VERSION));
// MAC address string
MUST(sendBytes(mac, 6));
Expand Down Expand Up @@ -437,19 +457,19 @@ void Connection::sendInspectionRawIMUData(
MUST(sendByte(sensorId));
MUST(sendByte(PACKET_INSPECTION_DATATYPE_INT));

MUST(sendInt(rX));
MUST(sendInt(rY));
MUST(sendInt(rZ));
MUST(sendI32(rX));
MUST(sendI32(rY));
MUST(sendI32(rZ));
MUST(sendByte(rA));

MUST(sendInt(aX));
MUST(sendInt(aY));
MUST(sendInt(aZ));
MUST(sendI32(aX));
MUST(sendI32(aY));
MUST(sendI32(aZ));
MUST(sendByte(aA));

MUST(sendInt(mX));
MUST(sendInt(mY));
MUST(sendInt(mZ));
MUST(sendI32(mX));
MUST(sendI32(mY));
MUST(sendI32(mZ));
MUST(sendByte(mA));

MUST(endPacket());
Expand Down Expand Up @@ -573,7 +593,7 @@ void Connection::searchForServer() {
m_Connected = true;

m_FeatureFlagsRequestAttempts = 0;
m_ServerFeatures = ServerFeatures { };
m_ServerFeatures = FeatureFlags<EServerFeatureFlags>();

statusManager.setStatus(SlimeVR::Status::SERVER_CONNECTING, false);
ledManager.off();
Expand Down Expand Up @@ -603,7 +623,11 @@ void Connection::searchForServer() {

void Connection::reset() {
m_Connected = false;
std::fill(m_AckedSensorState, m_AckedSensorState+MAX_IMU_COUNT, SensorStatus::SENSOR_OFFLINE);
std::fill(
m_AckedSensorState,
m_AckedSensorState + MAX_IMU_COUNT,
SensorStatus::SENSOR_OFFLINE
);

m_UDP.begin(m_ServerPort);

Expand All @@ -614,18 +638,23 @@ void Connection::update() {
auto & sensors = sensorManager.getSensors();

updateSensorState(sensors);
maybeRequestFeatureFlags();

if (!m_Connected) {
searchForServer();
return;
}

maybeRequestFeatureFlags();

if (m_LastPacketTimestamp + TIMEOUT < millis()) {
statusManager.setStatus(SlimeVR::Status::SERVER_CONNECTING, true);

m_Connected = false;
std::fill(m_AckedSensorState, m_AckedSensorState+MAX_IMU_COUNT, SensorStatus::SENSOR_OFFLINE);
std::fill(
m_AckedSensorState,
m_AckedSensorState + MAX_IMU_COUNT,
SensorStatus::SENSOR_OFFLINE
);
m_Logger.warn("Connection to server timed out");

return;
Expand All @@ -639,17 +668,7 @@ void Connection::update() {
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<int>(m_Packet)) {
case PACKET_RECEIVE_HEARTBEAT:
Expand Down Expand Up @@ -697,16 +716,17 @@ void Connection::update() {
}

bool hadFlags = m_ServerFeatures.isAvailable();

uint32_t flagsLength = len - 12;
m_ServerFeatures = ServerFeatures::from(&m_Packet[12], flagsLength);
m_ServerFeatures
= FeatureFlags<EServerFeatureFlags>(&m_Packet[12], flagsLength);

if (!hadFlags) {
#if PACKET_BUNDLING != PACKET_BUNDLING_DISABLED
if (m_ServerFeatures.has(ServerFeatures::PROTOCOL_BUNDLE_SUPPORT)) {
m_Logger.debug("Server supports packet bundling");
}
#endif
#if PACKET_BUNDLING != PACKET_BUNDLING_DISABLED
if (m_ServerFeatures.has(EServerFeatureFlags::PROTOCOL_BUNDLE_SUPPORT
)) {
m_Logger.debug("Server supports packet bundling");
}
#endif
}

break;
Expand Down
Loading

0 comments on commit 1a0e05a

Please sign in to comment.