udp_marvelmind.c#

static void processStatPosData(MarvelmindUDP udp, byte[] bufferInput, bool highRes)
{
    uni_8x2_16 v16;
    uni_8x4_32 v32;
    //uint8_t addressReceived= bufferInput[0];
    byte n = bufferInput[5];
    byte i;
    byte ofs = 5 + 1;
    int xc, yc, zc;
    byte addr;
    #ifdef WIN32
    EnterCriticalSection(&udp.lock_);
    #else
    Monitor.Enter(udp.lock_);
    #endif
    for (i = 0; i < n; i++)
    {
        addr = bufferInput[ofs];
        v32.b[0] = bufferInput[ofs + 1];
        v32.b[1] = bufferInput[ofs + 2];
        v32.b[2] = bufferInput[ofs + 3];
        v32.b[3] = bufferInput[ofs + 4];
        xc = v32.dwi;
        v32.b[0] = bufferInput[ofs + 5];
        v32.b[1] = bufferInput[ofs + 6];
        v32.b[2] = bufferInput[ofs + 7];
        v32.b[3] = bufferInput[ofs + 8];
        yc = v32.dwi;
        v32.b[0] = bufferInput[ofs + 9];
        v32.b[1] = bufferInput[ofs + 10];
        v32.b[2] = bufferInput[ofs + 11];
        v32.b[3] = bufferInput[ofs + 12];
        zc = v32.dwi;
        ofs += 14;
        udp.stationaryPos.items[i].address = addr;
        udp.stationaryPos.items[i].x = xc;
        udp.stationaryPos.items[i].y = yc;
        udp.stationaryPos.items[i].z = zc;
    }//for i
    udp.stationaryPos.n = n;
    udp.stationaryPos.updated = true;
    #ifdef WIN32
    LeaveCriticalSection(&udp.lock_);
    #else
    Monitor.Exit(udp.lock_);
    #endif
}



void ProcessRawDistancesData(MarvelmindUDP udp, byte[] bufferInput)
{
    byte[] v32 = new byte[4];
    #if WIN32
        EnterCriticalSection(ref udp.lock_);
    #else
        Monitor.Enter(udp.lock_);
    #endif
    udp.rawDistances.hedgeAddress = bufferInput[5];
    byte ofs = 6;
    byte i;
    for (i = 0; i < 4; i++)
    {
        udp.rawDistances.items[i].address = bufferInput[ofs + 0];
        v32[0] = bufferInput[ofs + 1];
        v32[1] = bufferInput[ofs + 2];
        v32[2] = bufferInput[ofs + 3];
        v32[3] = bufferInput[ofs + 4];
        udp.rawDistances.items[i].distance_mm = BitConverter.ToInt32(v32, 0);
        ofs += 6;
    }
    udp.rawDistances.updated = true;
    #if WIN32
        LeaveCriticalSection(ref udp.lock_);
    #else
        Monitor.Exit(udp.lock_);
    #endif
}



using System.Threading;

public static void ProcessTelemetryData(MarvelmindUDP udp, byte[] bufferInput)
{
    ushort[] v16 = new ushort[8];
    #if WIN32
        Monitor.Enter(udp.lock_);
    #else
        Monitor.Enter(udp.lock_);
    #endif
    udp.telemetryPacket.address = bufferInput[0];
    v16[0] = bufferInput[5];
    v16[1] = bufferInput[6];
    udp.telemetryPacket.vbat_mv = v16[0];
    udp.telemetryPacket.rssi_dbm = (sbyte)bufferInput[7];
    udp.telemetryPacket.updated = true;
    #if WIN32
        Monitor.Exit(udp.lock_);
    #else
        Monitor.Exit(udp.lock_);
    #endif
}

public static void ProcessQualityData(MarvelmindUDP udp, byte[] bufferInput)
{
    #if WIN32
        Monitor.Enter(udp.lock_);
    #else
        Monitor.Enter(udp.lock_);
    #endif
    udp.qualityPacket.address = bufferInput[5];
    udp.qualityPacket.quality = bufferInput[6];
    udp.qualityPacket.updated = true;
    #if WIN32
        Monitor.Exit(udp.lock_);
    #else
        Monitor.Exit(udp.lock_);
    #endif
}




static void processUserPayloadData(MarvelmindUDP udp, byte[] bufferInput)
{
    byte dsize;
    lock (udp.lock_)
    {
        dsize = bufferInput[4];
        Buffer.BlockCopy(bufferInput, 5, udp.userPayloadData.timestamp.timestamp64, 0, sizeof(udp.userPayloadData.timestamp.timestamp64));
        udp.userPayloadData.realTime = true;
        udp.userPayloadData.address = bufferInput[0];
        udp.userPayloadData.dataSize = (byte)(dsize - 8);
        for (byte i = 0; i < udp.userPayloadData.dataSize; i++)
        {
            udp.userPayloadData.data[i] = bufferInput[5 + 8 + i];
        }
        udp.userPayloadData.updated = true;
    }
}