void DataFlash_MAVLink::do_resends(uint32_t now)
{
if (!_initialised || !_logging_started ||!_sending_to_client) {
return;
}
uint8_t count_to_send = 5;
if (_blockcount < count_to_send) {
count_to_send = _blockcount;
}
uint32_t oldest = now - 100; // 100 milliseconds before resend. Hmm.
while (count_to_send-- > 0) {
if (!semaphore->take_nonblocking()) {
return;
}
for (struct dm_block *block=_blocks_sent.oldest; block != nullptr; block=block->next) {
// only want to send blocks every now-and-then:
if (block->last_sent < oldest) {
if (! send_log_block(*block)) {
// failed to send the block; try again later....
semaphore->give();
return;
}
stats.resends++;
}
}
semaphore->give();
}
}
void DataFlash_MAVLink::handle_retry(uint32_t block_num)
{
if (!_initialised) {
return;
}
_last_response_time = hal.scheduler->millis();
for(uint8_t block = 0; block < _total_blocks; block++){
if(_block_num[block] == block_num) {
send_log_block(block);
return;
}
}
}
/* while we "successfully" send log blocks from a queue, move them to
* the sent list. DO NOT call this for blocks already sent!
*/
bool DataFlash_MAVLink::send_log_blocks_from_queue(dm_block_queue_t &queue)
{
uint8_t sent_count = 0;
while (queue.oldest != NULL) {
if (sent_count++ > _max_blocks_per_send_blocks) {
return false;
}
if (! send_log_block(*queue.oldest)) {
return false;
}
queue.sent_count++;
struct DataFlash_MAVLink::dm_block *tmp = dequeue_seqno(queue,queue.oldest->seqno);
if (tmp != NULL) { // should never be NULL
enqueue_block(_blocks_sent, tmp);
} else {
internal_error();
}
}
return true;
}
/* Write a block of data at current offset */
void DataFlash_MAVLink::WriteBlock(const void *pBuffer, uint16_t size)
{
if (!_initialised || !_logging_started) {
return;
}
//transfer nothing else when in the middle of doing critical blocks (Parameters and Formats) transaction
if(is_critical_block) {
_only_critical_blocks = true;
}
if(!is_critical_block && ((hal.scheduler->millis() - _last_response_time) > 2000 || _buffer_empty())) {
for(uint8_t block = 0; block < NUM_BUFFER_BLOCKS; block++) {
_is_critical_block[block] = false;
}
_only_critical_blocks = false;
}
if(!is_critical_block && _only_critical_blocks) {
return;
}
uint16_t copied = 0;
while (copied < size) {
if(is_critical_block) {
_is_critical_block[_cur_block_address] = true;
}
uint16_t remaining_to_copy = size - copied;
uint16_t _curr_remaining = _block_max_size - _latest_block_len;
uint16_t to_copy = (remaining_to_copy > _curr_remaining) ? _curr_remaining : remaining_to_copy;
memcpy(&_buf[_cur_block_address][_latest_block_len], &((const uint8_t *)pBuffer)[copied], to_copy);
copied += to_copy;
_latest_block_len += to_copy;
if (_latest_block_len == _block_max_size) {
send_log_block(_cur_block_address); //block full send it
_cur_block_address = next_block_address();
}
}
}
