Movesense设备IMU6偏置校准及SBEM数据解码问题求助

我正尝试在Movesense设备上实现偏置校准流程，通过Logbook记录IMU6的数据，但在使用C++解码Movesense设备的SBEM数据时遇到问题。仅找到一篇含Python代码的旧帖子，不确定其是否适配当前版本的Movesense固件。

我的onGetResult函数 #

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
        }
    }
}

SBEM数据解析代码 #

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]);
        }
    }

}

读取第一个数据块后，我得到了"\0"的ID，因此想确认SBEM的编码方式是否仍未改变。同时有以下疑问：

1. 是否有已确立的Movesense设备校准流程？
2. C++ API中是否有可直接解析SBEM数据的函数？（我尝试从模拟器调用/MDS/Logbook/{serial}/byId/{LogId}/Data，但不确定是否可行）

解答 #

1. Movesense设备校准流程

Movesense官方提供了标准校准流程，核心步骤：

• 将设备放置在水平静止平面，确保无震动
• 调用/Meas/IMU6/Calibrate资源触发校准，设备会自动计算并存储加速度计、陀螺仪的偏置值
• 校准完成后，可通过/Meas/IMU6/Config读取校准参数，或直接使用校准后的IMU数据
• 校准结果会自动持久化到设备Flash，无需手动写入Logbook

2. SBEM数据解析的C++ API支持

当前Movesense SDK的公共C++ API中没有直接解析SBEM的函数，但可以使用SDK内部的SbemParser类（路径：movesense-core-lib/sbem/src/sbemparser.hpp）来处理，示例代码：

#include "sbemparser.hpp"

void parseSbemData(uint8* data, uint32 length) {
    SbemParser parser;
    parser.Parse(data, length);
    
    const auto& entries = parser.GetEntries();
    for (const auto& entry : entries) {
        DEBUGLOG("SBEM Entry ID: %u, Length: %u", entry.id, entry.dataLength);
        // 可在此处处理entry.data中的原始数据
    }
}

另外，模拟器中调用/MDS/Logbook/{serial}/byId/{LogId}/Data是可行的，返回的原始数据格式与设备端一致，可用于调试SBEM解析逻辑。

内容的提问来源于stack exchange，提问作者dasirra