// DM_write: 70734 events, 0 overruns, 167806us elapsed, 2us avg, min 1us max 34us 0.620us rms
bool DataFlash_MAVLink::WritePrioritisedBlock(const void *pBuffer, uint16_t size, bool is_critical)
{
if (!WritesOK()) {
return false;
}
if (!semaphore->take_nonblocking()) {
dropped++;
return false;
}
if (! WriteBlockCheckStartupMessages()) {
semaphore->give();
return false;
}
if (bufferspace_available() < size) {
if (_startup_messagewriter->finished()) {
// do not count the startup packets as being dropped...
dropped++;
}
semaphore->give();
return false;
}
uint16_t copied = 0;
while (copied < size) {
if (_current_block == nullptr) {
_current_block = next_block();
if (_current_block == nullptr) {
// should not happen - there's a sanity check above
internal_error();
semaphore->give();
return false;
}
}
uint16_t remaining_to_copy = size - copied;
uint16_t _curr_remaining = remaining_space_in_current_block();
uint16_t to_copy = (remaining_to_copy > _curr_remaining) ? _curr_remaining : remaining_to_copy;
memcpy(&(_current_block->buf[_latest_block_len]), &((const uint8_t *)pBuffer)[copied], to_copy);
copied += to_copy;
_latest_block_len += to_copy;
if (_latest_block_len == MAVLINK_MSG_REMOTE_LOG_DATA_BLOCK_FIELD_DATA_LEN) {
//block full, mark it to be sent:
enqueue_block(_blocks_pending, _current_block);
_current_block = next_block();
}
}
semaphore->give();
return true;
}
bool DataFlash_Block::WritePrioritisedBlock(const void *pBuffer, uint16_t size,
bool is_critical)
{
// is_critical is ignored - we're a ring buffer and never run out
// of space. possibly if we do more complicated bandwidth
// limiting we can reservice bandwidth based on is_critical
if (!CardInserted() || !log_write_started || !_writes_enabled) {
return false;
}
if (! WriteBlockCheckStartupMessages()) {
return false;
}
while (size > 0) {
uint16_t n = df_PageSize - df_BufferIdx;
if (n > size) {
n = size;
}
if (df_BufferIdx == 0) {
// if we are at the start of a page we need to insert a
// page header
if (n > df_PageSize - sizeof(struct PageHeader)) {
n = df_PageSize - sizeof(struct PageHeader);
}
struct PageHeader ph = { df_FileNumber, df_FilePage };
BlockWrite(df_BufferNum, df_BufferIdx, &ph, sizeof(ph), pBuffer, n);
df_BufferIdx += n + sizeof(ph);
} else {
BlockWrite(df_BufferNum, df_BufferIdx, NULL, 0, pBuffer, n);
df_BufferIdx += n;
}
size -= n;
pBuffer = (const void *)(n + (uintptr_t)pBuffer);
if (df_BufferIdx == df_PageSize) {
FinishWrite();
df_FilePage++;
}
}
return true;
}
