CODEWITHSUNDEEP
movesense
dasirra
dasirra

Reputation: 11

Extract Logbook data in Movesense device

I'm trying to implement a bias calibration procedure in the Movesense device using the Logbook to record data from IMU6, but I'm having trouble in decoding SBEM data from the Movesense device in C++. I looked for specific documentation about it, but I just found this old post with some Python code that I don't know is working in current versions of Movesense firmware.

This is my onGetResult function:

void IMU12Service::onGetResult(wb::RequestId requestId, wb::ResourceId resourceId, wb::Result resultCode,
                                   const wb::Value &result)
{
    uint64 size = 0;
    static uint32 calibrationDataLength = 0;
    const uint32 index = 1; // I already know that entryId is 1 in the logbook
    if (wb::IsErrorResult(resultCode)) {
        DEBUGLOG("onGetResult failed! resource: %u, result: %u", resourceId.localResourceId, resultCode);
        return;
    }
    switch (resourceId.localResourceId) {
        case WB_RES::LOCAL::MEM_LOGBOOK_ENTRIES::LID: {
                     resourceId.localResourceId, resultCode, &result);
            const WB_RES::LogEntries &logEntries = result.convertTo<const WB_RES::LogEntries &>();
            if (logEntries.elements.size() == 0) {
                DEBUGLOG("Sensor is Not Calibrated");
                
            } else {
DEBUGLOG("Sensor is Calibrated");
                size = logEntries.elements[0].size.getValue();
                
// Init the Descriptors extraction process, and then the Data extraction process
                asyncGet(WB_RES::LOCAL::MEM_LOGBOOK_BYID_LOGID_DESCRIPTORS(),
                 AsyncRequestOptions::Empty, index);
            }
            break;
        }
        case WB_RES::LOCAL::MEM_LOGBOOK_BYID_LOGID_DATA::LID: {
            DEBUGLOG("Fetching data");
            if (resultCode == wb::HTTP_CODE_CONTINUE) {
                const wb::ByteStream &chunk = result.convertTo<wb::ByteStream &>();
                const uint32 chunkLength = chunk.length();

                if (calibrationDataLength == 0) {
                    calibrationData = new uint8[chunkLength];
                    //memcpy(calibrationData, chunk.data, chunkSize);
                    chunk.serialize(calibrationData, chunkLength);
                    calibrationDataLength += chunkLength;
                } else {
                    uint32 calibrationDataSize = calibrationDataLength * sizeof(calibrationData[0]);
                    uint8* newData = new uint8[calibrationDataLength + chunkLength];
                    memcpy(newData, calibrationData, calibrationDataSize);
                    chunk.serialize(newData+calibrationDataLength, chunk.length());
                    calibrationDataLength += chunkLength;
                    delete[] calibrationData;
                    calibrationData = newData;
                }
                asyncGet(WB_RES::LOCAL::MEM_LOGBOOK_BYID_LOGID_DATA(),
                    AsyncRequestOptions::Empty, index);
            }
            else {
                const wb::ByteStream &chunk = result.convertTo<wb::ByteStream &>();
                const uint32 chunkLength = chunk.length();

                if (calibrationDataLength == 0) {
                    calibrationData = new uint8[chunkLength];
                    //memcpy(calibrationData, chunk.data, chunkSize);
                    chunk.serialize(calibrationData, chunkLength);
                    calibrationDataLength += chunkLength;
                }
                else {
                    uint32 calibrationDataSize = calibrationDataLength * sizeof(calibrationData[0]);
                    uint8* newData = new uint8[calibrationDataLength + chunkLength];
                    memcpy(newData, calibrationData, calibrationDataSize);
                    chunk.serialize(newData + calibrationDataLength, chunk.length());
                    calibrationDataLength += chunkLength;
                    delete[] calibrationData;
                    calibrationData = newData;
                }
                DEBUGLOG("Number of bytes: %d", calibrationDataLength);

                // parse SBEM data
                parseSbemData(calibrationData, calibrationDataLength);
            }
            break;
        }
        case WB_RES::LOCAL::MEM_LOGBOOK_BYID_LOGID_DESCRIPTORS::LID: {
            DEBUGLOG("Fetching descriptors");
            if (resultCode == wb::HTTP_CODE_CONTINUE) {
                asyncGet(WB_RES::LOCAL::MEM_LOGBOOK_BYID_LOGID_DESCRIPTORS(),
                 AsyncRequestOptions::Empty, index);
            } else {
                asyncGet(WB_RES::LOCAL::MEM_LOGBOOK_BYID_LOGID_DATA(),
                 AsyncRequestOptions::Empty, index);
            }
            break;
        }
        default: {
            DEBUGLOG("resource id: %u", resourceId.localResourceId);
            //do nothing
        }
    }

And this is the code to parse SBEM data

uint16 readId(uint8* data, uint32* pos, uint32 length) {
    uint16 id;
    byte ReservedSbemId_e_Escape = 0xFF;
    //byte ReservedSbemId_e_Descriptor = 0;
    if (*pos >= length) {
        DEBUGLOG("End of SBEM sequence");
        id = NULL;
    }

    byte byte1 = data[*pos]; (*pos)++;
    if (byte1 < ReservedSbemId_e_Escape) {
        id = (uint16)byte1;
    }
    else {
        // read 2 bytes more
        id = ((uint16*)(&data[*pos]))[0]; (* pos) += 2;
    }

    return id;
}

uint32 readLen(uint8* data, uint32* pos, uint32 length) {
    uint32 datasize;
    byte byte1 = data[*pos]; (*pos)++;
    byte ReservedSbemId_e_Escape = 0xFF;
    if (byte1 < ReservedSbemId_e_Escape) {
        datasize = (uint32)byte1;
    } else {
        datasize = ((uint32*)(&data[*pos]))[0]; (* pos) += 4;
    }

    return datasize;
}

struct ChunkHeader{
    uint16 id;
    uint32 datasize;
};

ChunkHeader* readChunkHeader(uint8* data, uint32* pos, uint32 length) {
    ChunkHeader* chunkHeader = new ChunkHeader{};
    uint16 id = readId(data, pos, length);
    if (id == NULL) {
        chunkHeader = nullptr;
    } else {
        uint32 datasize = readLen(data, pos, length);
        chunkHeader->id = id;
        chunkHeader->datasize = datasize;
    }

    return chunkHeader;
}

void parseSbemData(uint8* data, uint32 calibrationDataLength) {

    uint32 pos = 0;

    while (true) {
        ChunkHeader* header = readChunkHeader(data, &pos, calibrationDataLength);
        if (header == NULL) {
            DEBUGLOG("None id");
            break;
        }

        char* array = new char[header->datasize];
        memcpy(array, &data[pos], header->datasize);
        pos += header->datasize;
        //if (sizeof(array) != header->datasize) {
        //    DEBUGLOG("ERROR: too few bytes returned.");
        //    break;
        //}

        for (uint32 i=0; i<(header->datasize); i++){
            DEBUGLOG("Data Value: %d", array[i]);
        }
    }

}

After reading the first chunk I'm getting a "\0" ID, so I'm wondering if the codification is still the same.

I don't know if I'm approaching the problem correctly because I didn't find any other easier way to solve the bias calibration problem.

Thanks a lot for your support!

Upvotes: 0

Views: 145

Answers (1)

PetriL
PetriL

Reputation: 1299

The Movesense firmware does not include a SBEM-parsers. However, sbem is a relatively simple format (see the answer here). The Movesense flash variant mixes the descriptors in the "/Data" resource as a single file, so it's possible to encounter descriptors there as well.

The descriptors can be recognized by chunkId == 0. Since you are makin a custom firmware you can confirm the correct ID that you are interrested in by checking the generated sbem code in build folder.

Full disclosure: I work for the Movesense team

Upvotes: 0

Related Questions