// 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; }
uint16_t DataFlash_MAVLink::bufferspace_available() { return (_blockcount_free * 200 + remaining_space_in_current_block()); }