aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

DataFlash: DataFlash_MAVLink: avoid races by taking semaphore

Browse Source
This commit is contained in:
Peter Barker 2017-04-11 13:57:23 +10:00 committed by Randy Mackay
parent 9e436ec35c
commit ac34405272
1 changed files with 14 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/DataFlash/DataFlash_MAVLink.cpp
View File

@ -277,11 +277,15 @@ void DataFlash_MAVLink::remote_log_block_status_msg(mavlink_channel_t chan,
{
mavlink_remote_log_block_status_t packet;
mavlink_msg_remote_log_block_status_decode(msg, &packet);
if (!semaphore->take_nonblocking()) {
return;
}
if(packet.status == 0){
handle_retry(packet.seqno);
} else{
handle_ack(chan, msg, packet.seqno);
}
semaphore->give();
}
void DataFlash_MAVLink::handle_retry(uint32_t seqno)
@ -399,6 +403,9 @@ void DataFlash_MAVLink::stats_collect()
if (!_initialised || !_logging_started) {
return;
}
if (!semaphore->take_nonblocking()) {
return;
}
uint8_t pending = queue_size(_blocks_pending);
uint8_t sent = queue_size(_blocks_sent);
uint8_t retry = queue_size(_blocks_retry);
@ -407,6 +414,8 @@ void DataFlash_MAVLink::stats_collect()
if (sfree != _blockcount_free) {
internal_error();
}
semaphore->give();
stats.state_pending += pending;
stats.state_sent += sent;
stats.state_free += sfree;
@ -500,16 +509,21 @@ void DataFlash_MAVLink::do_resends(uint32_t now)
}
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();
}
}