Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
35 changes: 14 additions & 21 deletions components/RaftROS/AutoPub/AutoPubDeviceSource.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -344,11 +344,13 @@ bool AutoPubDeviceSource<Backend, CAPACITY>::attachDevice(
pCtx->structSize = devTypeRec.pollStructSize;
pCtx->pollDataSizeBytes = devTypeRec.pollDataSizeBytes;

// Up to 8 records per decode for FIFO-backed devices keeps the heap
// allocation under 1 kB for typical struct sizes (IMU = 24 B).
static constexpr uint16_t MAX_DECODE_RECORDS = 8;
pCtx->maxDecodeRecords = MAX_DECODE_RECORDS;
pCtx->decodeBufSize = (uint32_t)pCtx->structSize * MAX_DECODE_RECORDS;
// Working space for every record of a poll (a FIFO read holds several: an LSM6DS read up to 20, so that the
// newest isn't dropped), within 1 kB
static constexpr uint32_t MAX_DECODE_BUF_BYTES = 1024;
const uint32_t maxDecodeRecords = std::min<uint32_t>(DeviceTypeRecords::NEWEST_DECODE_MAX_RECORDS,
std::max<uint32_t>(1, MAX_DECODE_BUF_BYTES / pCtx->structSize));
pCtx->maxDecodeRecords = (uint16_t)maxDecodeRecords;
pCtx->decodeBufSize = (uint32_t)pCtx->structSize * maxDecodeRecords;
if (pCtx->decodeBufSize > 0)
pCtx->pDecodeBuf = new uint8_t[pCtx->decodeBufSize];

Expand Down Expand Up @@ -510,22 +512,13 @@ void AutoPubDeviceSource<Backend, CAPACITY>::onDeviceData(
pCtx->structSize == recordSize))
return 0;

// Pad up to the fixed record stride the generated decoder expects
const uint32_t expectedRecordSize = pCtx->pollDataSizeBytes
+ DevicePollingInfo::POLL_RESULT_TIMESTAMP_SIZE;
if (data.size() < expectedRecordSize)
data.resize(expectedRecordSize, 0);

const uint32_t numRecords = pCtx->decodeFn(data.data(), data.size(),
pCtx->pDecodeBuf, pCtx->decodeBufSize,
pCtx->maxDecodeRecords, pCtx->decodeState);
// Keep only the latest record (FIFO latest-only); check the
// decoder's count before forming the pointer.
if (numRecords == 0 || numRecords > pCtx->decodeBufSize / pCtx->structSize)
return 0;
std::memcpy(pRecord, pCtx->pDecodeBuf + (numRecords - 1) * pCtx->structSize,
pCtx->structSize);
return pCtx->structSize;
// Decoded as every on-device consumer decodes (DeviceTypeRecords pads a poll cut short, e.g. a FIFO
// read), keeping the newest record; the working space is the context's, so nothing is allocated here
// for a full-length poll
const uint32_t numRecords = DeviceTypeRecords::decodeNewestPollRecord(pCtx->decodeFn,
pCtx->pollDataSizeBytes, pCtx->structSize, data.data(), data.size(), pRecord, recordSize,
pCtx->decodeState, pCtx->pDecodeBuf, pCtx->decodeBufSize);
return (numRecords > 0) ? pCtx->structSize : 0;
},
PRODUCER_LOCK_TIMEOUT_MS);
}
Expand Down
33 changes: 9 additions & 24 deletions examples/ExampleDiscoverable/components/MainSysMod/MainSysMod.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -276,41 +276,26 @@ void MainSysMod::appendReading(AttachedDevice& device, bool fresh, String& out)
out += " no new reading";
return;
}
// A sample may hold several records (a FIFO read); the last is the newest
const uint32_t maxRecords = std::max<uint32_t>(1, std::min<uint32_t>(16, 4096 / devTypeRec.pollStructSize));
if (_decodeBuf.size() < maxRecords * devTypeRec.pollStructSize)
_decodeBuf.resize(maxRecords * devTypeRec.pollStructSize);
const uint32_t numRecords = devTypeRec.pollResultDecodeFn(data.data(), data.size(), _decodeBuf.data(),
_decodeBuf.size(), maxRecords, device.decodeState);
// Decoded as every on-device consumer decodes: a poll cut short (a FIFO read) is padded, and of the records
// a poll holds the newest is kept
if (_decodeBuf.size() < devTypeRec.pollStructSize)
_decodeBuf.resize(devTypeRec.pollStructSize);
const uint32_t numRecords = deviceTypeRecords.decodeNewestPollRecord(device.deviceTypeIndex, data.data(),
data.size(), _decodeBuf.data(), _decodeBuf.size(), device.decodeState);
if (numRecords == 0)
{
out += " undecodable sample";
return;
}
const uint8_t* pRecord = _decodeBuf.data() + (numRecords - 1) * devTypeRec.pollStructSize;
for (uint16_t index = 0; index < devTypeRec.pollFieldCount; ++index)
{
const AttrFieldDesc& field = devTypeRec.pollFieldDescs[index];
const uint8_t* pField = pRecord + field.offset;
double value = 0;
switch (field.type)
{
case AttrType::Float: { float v; memcpy(&v, pField, sizeof(v)); value = v; break; }
case AttrType::Int32: { int32_t v; memcpy(&v, pField, sizeof(v)); value = v; break; }
case AttrType::Uint32: { uint32_t v; memcpy(&v, pField, sizeof(v)); value = v; break; }
case AttrType::Int16: { int16_t v; memcpy(&v, pField, sizeof(v)); value = v; break; }
case AttrType::Uint16: { uint16_t v; memcpy(&v, pField, sizeof(v)); value = v; break; }
case AttrType::Int8: value = (int8_t)*pField; break;
case AttrType::Uint8:
case AttrType::Bool: value = *pField; break;
}
if (field.divisor != 0.0f && field.divisor != 1.0f)
value /= field.divisor;
value += field.addend;
field.getValue(_decodeBuf.data(), devTypeRec.pollStructSize, 0, value);
char text[48];
// An array attribute (a multizone frame, say) shows its first element and its size
if (field.count > 1)
snprintf(text, sizeof(text), " %s[%u]=%.4g..", field.name, (unsigned)field.count, value);
if (field.elemCount() > 1)
snprintf(text, sizeof(text), " %s[%u]=%.4g..", field.name, (unsigned)field.elemCount(), value);
else
snprintf(text, sizeof(text), " %s=%.4g", field.name, value);
out += text;
Expand Down