【发布时间】：2023-02-01 22:10:06
【问题描述】：
我正在尝试使用日志在 Movesense 设备中实施偏差校准程序以记录来自 IMU6 的数据，但我在使用 C++ 解码来自 Movesense 设备的 SBEM 数据时遇到了问题。我查找了有关它的特定文档，但我刚刚发现 this 旧帖子，其中包含一些我不知道在当前版本的 Movesense 固件中工作的 Python 代码。
这是我的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，所以我想知道编码是否仍然相同。
我不知道我是否正确地解决了这个问题，因为我没有找到任何其他更简单的方法来解决偏差校准问题。
- 是否有任何既定程序来校准设备？
- 在C++ API中是否有直接解析SBEM数据的函数？ （我尝试从模拟器调用 /MDS/Logbook/{serial}/byId/{LogId}/Data，但我什至不知道这是否可能）
多谢您的支持！
【问题讨论】：
标签： movesense