// 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;
}
/* Write a block of data at current offset */
bool DataFlash_MAVLink::WriteBlock(const void *pBuffer, uint16_t size)
{
if (!_initialised || !_sending_to_client || !_writes_enabled) {
return false;
}
if (! WriteBlockCheckPrefaceMessages()) {
return false;
}
if (bufferspace_available() < size) {
if (_startup_messagewriter->finished()) {
// do not count the startup packets as being dropped...
dropped++;
}
return false;
}
uint16_t copied = 0;
while (copied < size) {
if (_current_block == NULL) {
_current_block = next_block();
if (_current_block == NULL) {
// should not happen - there's a sanity check above
internal_error();
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();
}
}
if (!_writing_preface_messages) {
// push_log_blocks();
}
return true;
}
bool DataFlash_Backend::Log_Write(const uint8_t msg_type, va_list arg_list, bool is_critical)
{
// stack-allocate a buffer so we can WriteBlock(); this could be
// 255 bytes! If we were willing to lose the WriteBlock
// abstraction we could do WriteBytes() here instead?
const char *fmt = nullptr;
uint8_t msg_len;
DataFlash_Class::log_write_fmt *f;
for (f = _front.log_write_fmts; f; f=f->next) {
if (f->msg_type == msg_type) {
fmt = f->fmt;
msg_len = f->msg_len;
break;
}
}
if (fmt == nullptr) {
// this is a bug.
internal_error();
return false;
}
if (bufferspace_available() < msg_len) {
return false;
}
uint8_t buffer[msg_len];
uint8_t offset = 0;
buffer[offset++] = HEAD_BYTE1;
buffer[offset++] = HEAD_BYTE2;
buffer[offset++] = msg_type;
for (uint8_t i=0; i<strlen(fmt); i++) {
uint8_t charlen = 0;
switch(fmt[i]) {
case 'b': {
int8_t tmp = va_arg(arg_list, int);
memcpy(&buffer[offset], &tmp, sizeof(int8_t));
offset += sizeof(int8_t);
break;
}
case 'h':
case 'c': {
int16_t tmp = va_arg(arg_list, int);
memcpy(&buffer[offset], &tmp, sizeof(int16_t));
offset += sizeof(int16_t);
break;
}
case 'd': {
double tmp = va_arg(arg_list, double);
memcpy(&buffer[offset], &tmp, sizeof(double));
offset += sizeof(double);
break;
}
case 'i':
case 'L':
case 'e': {
int32_t tmp = va_arg(arg_list, int);
memcpy(&buffer[offset], &tmp, sizeof(int32_t));
offset += sizeof(int32_t);
break;
}
case 'f': {
float tmp = va_arg(arg_list, double);
memcpy(&buffer[offset], &tmp, sizeof(float));
offset += sizeof(float);
break;
}
case 'n':
charlen = 4;
break;
case 'M':
case 'B': {
uint8_t tmp = va_arg(arg_list, int);
memcpy(&buffer[offset], &tmp, sizeof(uint8_t));
offset += sizeof(uint8_t);
break;
}
case 'H':
case 'C': {
uint16_t tmp = va_arg(arg_list, int);
memcpy(&buffer[offset], &tmp, sizeof(uint16_t));
offset += sizeof(uint16_t);
break;
}
case 'I':
case 'E': {
uint32_t tmp = va_arg(arg_list, uint32_t);
memcpy(&buffer[offset], &tmp, sizeof(uint32_t));
offset += sizeof(uint32_t);
break;
}
case 'N':
charlen = 16;
break;
case 'Z':
charlen = 64;
break;
case 'q': {
int64_t tmp = va_arg(arg_list, int64_t);
memcpy(&buffer[offset], &tmp, sizeof(int64_t));
offset += sizeof(int64_t);
break;
}
case 'Q': {
uint64_t tmp = va_arg(arg_list, uint64_t);
memcpy(&buffer[offset], &tmp, sizeof(uint64_t));
offset += sizeof(uint64_t);
break;
}
}
if (charlen != 0) {
char *tmp = va_arg(arg_list, char*);
memcpy(&buffer[offset], tmp, charlen);
offset += charlen;
}
}
