Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 12:14:10 -04:00
Code Issues Packages Projects Releases Wiki Activity

DataFlash: use WITH_SEMAPHORE()

Browse Source 
and removed usage of hal.util->new_semaphore()
This commit is contained in:
Andrew Tridgell 2018-10-12 10:35:04 +11:00
parent f662cf55e5
commit 1d6b58f9ca
7 changed files with 57 additions and 82 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
libraries/DataFlash/DataFlash_File.cpp
View File

@ -98,17 +98,6 @@ void DataFlash_File::Init()
int ret; int ret;
struct stat st; struct stat st;
semaphore = hal.util->new_semaphore();
if (semaphore == nullptr) {
AP_HAL::panic("Failed to create DataFlash_File semaphore");
return;
}
write_fd_semaphore = hal.util->new_semaphore();
if (write_fd_semaphore == nullptr) {
AP_HAL::panic("Failed to create DataFlash_File write_fd_semaphore");
return;
}
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
// try to cope with an existing lowercase log directory // try to cope with an existing lowercase log directory
// name. NuttX does not handle case insensitive VFAT well // name. NuttX does not handle case insensitive VFAT well
@ -542,7 +531,7 @@ bool DataFlash_File::_WritePrioritisedBlock(const void *pBuffer, uint16_t size,
return false; return false;
} }
if (!semaphore->take(1)) { if (!semaphore.take(1)) {
return false; return false;
} }
@ -559,7 +548,7 @@ bool DataFlash_File::_WritePrioritisedBlock(const void *pBuffer, uint16_t size,
if (!must_dribble && if (!must_dribble &&
space < non_messagewriter_message_reserved_space()) { space < non_messagewriter_message_reserved_space()) {
// this message isn't dropped, it will be sent again... // this message isn't dropped, it will be sent again...
semaphore->give(); semaphore.give();
return false; return false;
} }
last_messagewrite_message_sent = now; last_messagewrite_message_sent = now;
@ -567,7 +556,7 @@ bool DataFlash_File::_WritePrioritisedBlock(const void *pBuffer, uint16_t size,
// we reserve some amount of space for critical messages: // we reserve some amount of space for critical messages:
if (!is_critical && space < critical_message_reserved_space()) { if (!is_critical && space < critical_message_reserved_space()) {
_dropped++; _dropped++;
semaphore->give(); semaphore.give();
return false; return false;
} }
} }
@ -576,13 +565,13 @@ bool DataFlash_File::_WritePrioritisedBlock(const void *pBuffer, uint16_t size,
if (space < size) { if (space < size) {
hal.util->perf_count(_perf_overruns); hal.util->perf_count(_perf_overruns);
_dropped++; _dropped++;
semaphore->give(); semaphore.give();
return false; return false;
} }
_writebuf.write((uint8_t*)pBuffer, size); _writebuf.write((uint8_t*)pBuffer, size);
df_stats_gather(size); df_stats_gather(size);
semaphore->give(); semaphore.give();
return true; return true;
} }
@ -613,20 +602,20 @@ uint16_t DataFlash_File::find_last_log()
return ret; return ret;
} }
uint32_t DataFlash_File::_get_log_size(const uint16_t log_num) const uint32_t DataFlash_File::_get_log_size(const uint16_t log_num)
{ {
char *fname = _log_file_name(log_num); char *fname = _log_file_name(log_num);
if (fname == nullptr) { if (fname == nullptr) {
return 0; return 0;
} }
if (_write_fd != -1 && write_fd_semaphore->take_nonblocking()) { if (_write_fd != -1 && write_fd_semaphore.take_nonblocking()) {
if (_write_filename != nullptr && strcmp(_write_filename, fname) == 0) { if (_write_filename != nullptr && strcmp(_write_filename, fname) == 0) {
// it is the file we are currently writing // it is the file we are currently writing
free(fname); free(fname);
write_fd_semaphore->give(); write_fd_semaphore.give();
return _write_offset; return _write_offset;
} }
write_fd_semaphore->give(); write_fd_semaphore.give();
} }
struct stat st; struct stat st;
if (::stat(fname, &st) != 0) { if (::stat(fname, &st) != 0) {
@ -638,24 +627,24 @@ uint32_t DataFlash_File::_get_log_size(const uint16_t log_num) const
return st.st_size; return st.st_size;
} }
uint32_t DataFlash_File::_get_log_time(const uint16_t log_num) const uint32_t DataFlash_File::_get_log_time(const uint16_t log_num)
{ {
char *fname = _log_file_name(log_num); char *fname = _log_file_name(log_num);
if (fname == nullptr) { if (fname == nullptr) {
return 0; return 0;
} }
if (_write_fd != -1 && write_fd_semaphore->take_nonblocking()) { if (_write_fd != -1 && write_fd_semaphore.take_nonblocking()) {
if (_write_filename != nullptr && strcmp(_write_filename, fname) == 0) { if (_write_filename != nullptr && strcmp(_write_filename, fname) == 0) {
// it is the file we are currently writing // it is the file we are currently writing
free(fname); free(fname);
write_fd_semaphore->give(); write_fd_semaphore.give();
uint64_t utc_usec; uint64_t utc_usec;
if (!AP::rtc().get_utc_usec(utc_usec)) { if (!AP::rtc().get_utc_usec(utc_usec)) {
return 0; return 0;
} }
return utc_usec / 1000000U; return utc_usec / 1000000U;
} }
write_fd_semaphore->give(); write_fd_semaphore.give();
} }
struct stat st; struct stat st;
if (::stat(fname, &st) != 0) { if (::stat(fname, &st) != 0) {
@ -837,14 +826,14 @@ uint16_t DataFlash_File::get_num_logs()
void DataFlash_File::stop_logging(void) void DataFlash_File::stop_logging(void)
{ {
// best-case effort to avoid annoying the IO thread // best-case effort to avoid annoying the IO thread
const bool have_sem = write_fd_semaphore->take(1); const bool have_sem = write_fd_semaphore.take(1);
if (_write_fd != -1) { if (_write_fd != -1) {
int fd = _write_fd; int fd = _write_fd;
_write_fd = -1; _write_fd = -1;
::close(fd); ::close(fd);
} }
if (have_sem) { if (have_sem) {
write_fd_semaphore->give(); write_fd_semaphore.give();
} else { } else {
_internal_errors++; _internal_errors++;
} }
@ -892,7 +881,7 @@ uint16_t DataFlash_File::start_new_log(void)
if (log_num > MAX_LOG_FILES) { if (log_num > MAX_LOG_FILES) {
log_num = 1; log_num = 1;
} }
if (!write_fd_semaphore->take(1)) { if (!write_fd_semaphore.take(1)) {
_open_error = true; _open_error = true;
return 0xFFFF; return 0xFFFF;
} }
@ -903,7 +892,7 @@ uint16_t DataFlash_File::start_new_log(void)
_write_filename = _log_file_name(log_num); _write_filename = _log_file_name(log_num);
if (_write_filename == nullptr) { if (_write_filename == nullptr) {
_open_error = true; _open_error = true;
write_fd_semaphore->give(); write_fd_semaphore.give();
return 0xFFFF; return 0xFFFF;
} }
#if HAL_OS_POSIX_IO #if HAL_OS_POSIX_IO
@ -917,7 +906,7 @@ uint16_t DataFlash_File::start_new_log(void)
if (_write_fd == -1) { if (_write_fd == -1) {
_initialised = false; _initialised = false;
_open_error = true; _open_error = true;
write_fd_semaphore->give(); write_fd_semaphore.give();
int saved_errno = errno; int saved_errno = errno;
::printf("Log open fail for %s - %s\n", ::printf("Log open fail for %s - %s\n",
_write_filename, strerror(saved_errno)); _write_filename, strerror(saved_errno));
@ -928,7 +917,7 @@ uint16_t DataFlash_File::start_new_log(void)
_last_write_ms = AP_HAL::millis(); _last_write_ms = AP_HAL::millis();
_write_offset = 0; _write_offset = 0;
_writebuf.clear(); _writebuf.clear();
write_fd_semaphore->give(); write_fd_semaphore.give();
// now update lastlog.txt with the new log number // now update lastlog.txt with the new log number
char *fname = _lastlog_file_name(); char *fname = _lastlog_file_name();
@ -974,11 +963,11 @@ void DataFlash_File::flush(void)
} }
_io_timer(); _io_timer();
} }
if (write_fd_semaphore->take(1)) { if (write_fd_semaphore.take(1)) {
if (_write_fd != -1) { if (_write_fd != -1) {
::fsync(_write_fd); ::fsync(_write_fd);
} }
write_fd_semaphore->give(); write_fd_semaphore.give();
} else { } else {
_internal_errors++; _internal_errors++;
} }
@ -1042,11 +1031,11 @@ void DataFlash_File::_io_timer(void)
} }
last_io_operation = "write"; last_io_operation = "write";
if (!write_fd_semaphore->take(1)) { if (!write_fd_semaphore.take(1)) {
return; return;
} }
if (_write_fd == -1) { if (_write_fd == -1) {
write_fd_semaphore->give(); write_fd_semaphore.give();
return; return;
} }
ssize_t nwritten = ::write(_write_fd, head, nbytes); ssize_t nwritten = ::write(_write_fd, head, nbytes);
@ -1080,7 +1069,7 @@ void DataFlash_File::_io_timer(void)
last_io_operation = ""; last_io_operation = "";
#endif #endif
} }
write_fd_semaphore->give(); write_fd_semaphore.give();
hal.util->perf_end(_perf_write); hal.util->perf_end(_perf_write);
} }

8
libraries/DataFlash/DataFlash_File.h
View File

@ -109,8 +109,8 @@ private:
char *_log_file_name_long(const uint16_t log_num) const; char *_log_file_name_long(const uint16_t log_num) const;
char *_log_file_name_short(const uint16_t log_num) const; char *_log_file_name_short(const uint16_t log_num) const;
char *_lastlog_file_name() const; char *_lastlog_file_name() const;
uint32_t _get_log_size(const uint16_t log_num) const; uint32_t _get_log_size(const uint16_t log_num);
uint32_t _get_log_time(const uint16_t log_num) const; uint32_t _get_log_time(const uint16_t log_num);
void stop_logging(void) override; void stop_logging(void) override;
@ -146,11 +146,11 @@ private:
const uint32_t _free_space_min_avail = 8388608; // bytes const uint32_t _free_space_min_avail = 8388608; // bytes
// semaphore mediates access to the ringbuffer // semaphore mediates access to the ringbuffer
AP_HAL::Semaphore *semaphore; HAL_Semaphore semaphore;
// write_fd_semaphore mediates access to write_fd so the frontend // write_fd_semaphore mediates access to write_fd so the frontend
// can open/close files without causing the backend to write to a // can open/close files without causing the backend to write to a
// bad fd // bad fd
AP_HAL::Semaphore *write_fd_semaphore; HAL_Semaphore write_fd_semaphore;
// performance counters // performance counters
AP_HAL::Util::perf_counter_t _perf_write; AP_HAL::Util::perf_counter_t _perf_write;

17
libraries/DataFlash/DataFlash_File_sd.cpp
View File

@ -58,13 +58,6 @@ void DataFlash_File::Init()
if(HAL_F4Light::state.sd_busy) return; // SD mounted via USB if(HAL_F4Light::state.sd_busy) return; // SD mounted via USB
semaphore = hal.util->new_semaphore();
if (semaphore == nullptr) {
AP_HAL::panic("Failed to create DataFlash_File semaphore");
return;
}
// create the log directory if need be // create the log directory if need be
const char* custom_dir = hal.util->get_custom_log_directory(); const char* custom_dir = hal.util->get_custom_log_directory();
if (custom_dir != nullptr){ if (custom_dir != nullptr){
@ -457,7 +450,7 @@ bool DataFlash_File::_WritePrioritisedBlock(const void *pBuffer, uint16_t size,
return false; return false;
} }
if (!semaphore->take(1)) { if (!semaphore.take(1)) {
return false; return false;
} }
@ -471,7 +464,7 @@ bool DataFlash_File::_WritePrioritisedBlock(const void *pBuffer, uint16_t size,
// things: // things:
if (space < non_messagewriter_message_reserved_space()) { if (space < non_messagewriter_message_reserved_space()) {
// this message isn't dropped, it will be sent again... // this message isn't dropped, it will be sent again...
semaphore->give(); semaphore.give();
return false; return false;
} }
} else { } else {
@ -479,7 +472,7 @@ bool DataFlash_File::_WritePrioritisedBlock(const void *pBuffer, uint16_t size,
if (!is_critical && space < critical_message_reserved_space()) { if (!is_critical && space < critical_message_reserved_space()) {
// printf("dropping NC block! size=%d\n", size); // printf("dropping NC block! size=%d\n", size);
_dropped++; _dropped++;
semaphore->give(); semaphore.give();
return false; return false;
} }
} }
@ -490,14 +483,14 @@ bool DataFlash_File::_WritePrioritisedBlock(const void *pBuffer, uint16_t size,
printf("dropping block! size=%d\n", size); printf("dropping block! size=%d\n", size);
_dropped++; _dropped++;
semaphore->give(); semaphore.give();
return false; return false;
} }
_writebuf.write((uint8_t*)pBuffer, size); _writebuf.write((uint8_t*)pBuffer, size);
has_data=true; has_data=true;
semaphore->give(); semaphore.give();
return true; return true;
} }

4
libraries/DataFlash/DataFlash_File_sd.h
View File

@ -97,7 +97,7 @@ private:
/* construct a file name given a log number. Caller must free. */ /* construct a file name given a log number. Caller must free. */
char *_log_file_name(const uint16_t log_num) const; char *_log_file_name(const uint16_t log_num) const;
char *_lastlog_file_name() const; char *_lastlog_file_name() const;
uint32_t _get_log_size(const uint16_t log_num) const; uint32_t _get_log_size(const uint16_t log_num);
uint32_t _get_log_time(const uint16_t log_num) const; uint32_t _get_log_time(const uint16_t log_num) const;
void stop_logging(void); void stop_logging(void);
@ -127,7 +127,7 @@ private:
float avail_space_percent(uint32_t *free = NULL); float avail_space_percent(uint32_t *free = NULL);
AP_HAL::Semaphore *semaphore; HAL_Semaphore semaphore;
bool has_data; bool has_data;

38
libraries/DataFlash/DataFlash_MAVLink.cpp
View File

@ -25,12 +25,6 @@ extern const AP_HAL::HAL& hal;
// initialisation // initialisation
void DataFlash_MAVLink::Init() void DataFlash_MAVLink::Init()
{ {
semaphore = hal.util->new_semaphore();
if (semaphore == nullptr) {
AP_HAL::panic("Failed to create DataFlash_MAVLink semaphore");
return;
}
DataFlash_Backend::Init(); DataFlash_Backend::Init();
_blocks = nullptr; _blocks = nullptr;
@ -128,13 +122,13 @@ bool DataFlash_MAVLink::WritesOK() const
// DM_write: 70734 events, 0 overruns, 167806us elapsed, 2us avg, min 1us max 34us 0.620us rms // 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) bool DataFlash_MAVLink::_WritePrioritisedBlock(const void *pBuffer, uint16_t size, bool is_critical)
{ {
if (!semaphore->take_nonblocking()) { if (!semaphore.take_nonblocking()) {
_dropped++; _dropped++;
return false; return false;
} }
if (! WriteBlockCheckStartupMessages()) { if (! WriteBlockCheckStartupMessages()) {
semaphore->give(); semaphore.give();
return false; return false;
} }
@ -143,7 +137,7 @@ bool DataFlash_MAVLink::_WritePrioritisedBlock(const void *pBuffer, uint16_t siz
// do not count the startup packets as being dropped... // do not count the startup packets as being dropped...
_dropped++; _dropped++;
} }
semaphore->give(); semaphore.give();
return false; return false;
} }
@ -155,7 +149,7 @@ bool DataFlash_MAVLink::_WritePrioritisedBlock(const void *pBuffer, uint16_t siz
if (_current_block == nullptr) { if (_current_block == nullptr) {
// should not happen - there's a sanity check above // should not happen - there's a sanity check above
internal_error(); internal_error();
semaphore->give(); semaphore.give();
return false; return false;
} }
} }
@ -172,7 +166,7 @@ bool DataFlash_MAVLink::_WritePrioritisedBlock(const void *pBuffer, uint16_t siz
} }
} }
semaphore->give(); semaphore.give();
return true; return true;
} }
@ -278,7 +272,7 @@ void DataFlash_MAVLink::remote_log_block_status_msg(mavlink_channel_t chan,
{ {
mavlink_remote_log_block_status_t packet; mavlink_remote_log_block_status_t packet;
mavlink_msg_remote_log_block_status_decode(msg, &packet); mavlink_msg_remote_log_block_status_decode(msg, &packet);
if (!semaphore->take_nonblocking()) { if (!semaphore.take_nonblocking()) {
return; return;
} }
if(packet.status == 0){ if(packet.status == 0){
@ -286,7 +280,7 @@ void DataFlash_MAVLink::remote_log_block_status_msg(mavlink_channel_t chan,
} else{ } else{
handle_ack(chan, msg, packet.seqno); handle_ack(chan, msg, packet.seqno);
} }
semaphore->give(); semaphore.give();
} }
void DataFlash_MAVLink::handle_retry(uint32_t seqno) void DataFlash_MAVLink::handle_retry(uint32_t seqno)
@ -400,7 +394,7 @@ void DataFlash_MAVLink::stats_collect()
if (!_initialised) { if (!_initialised) {
return; return;
} }
if (!semaphore->take_nonblocking()) { if (!semaphore.take_nonblocking()) {
return; return;
} }
uint8_t pending = queue_size(_blocks_pending); uint8_t pending = queue_size(_blocks_pending);
@ -411,7 +405,7 @@ void DataFlash_MAVLink::stats_collect()
if (sfree != _blockcount_free) { if (sfree != _blockcount_free) {
internal_error(); internal_error();
} }
semaphore->give(); semaphore.give();
stats.state_pending += pending; stats.state_pending += pending;
stats.state_sent += sent; stats.state_sent += sent;
@ -478,20 +472,20 @@ void DataFlash_MAVLink::push_log_blocks()
DataFlash_Backend::WriteMoreStartupMessages(); DataFlash_Backend::WriteMoreStartupMessages();
if (!semaphore->take_nonblocking()) { if (!semaphore.take_nonblocking()) {
return; return;
} }
if (! send_log_blocks_from_queue(_blocks_retry)) { if (! send_log_blocks_from_queue(_blocks_retry)) {
semaphore->give(); semaphore.give();
return; return;
} }
if (! send_log_blocks_from_queue(_blocks_pending)) { if (! send_log_blocks_from_queue(_blocks_pending)) {
semaphore->give(); semaphore.give();
return; return;
} }
semaphore->give(); semaphore.give();
} }
void DataFlash_MAVLink::do_resends(uint32_t now) void DataFlash_MAVLink::do_resends(uint32_t now)
@ -506,7 +500,7 @@ void DataFlash_MAVLink::do_resends(uint32_t now)
} }
uint32_t oldest = now - 100; // 100 milliseconds before resend. Hmm. uint32_t oldest = now - 100; // 100 milliseconds before resend. Hmm.
while (count_to_send-- > 0) { while (count_to_send-- > 0) {
if (!semaphore->take_nonblocking()) { if (!semaphore.take_nonblocking()) {
return; return;
} }
for (struct dm_block *block=_blocks_sent.oldest; block != nullptr; block=block->next) { for (struct dm_block *block=_blocks_sent.oldest; block != nullptr; block=block->next) {
@ -514,13 +508,13 @@ void DataFlash_MAVLink::do_resends(uint32_t now)
if (block->last_sent < oldest) { if (block->last_sent < oldest) {
if (! send_log_block(*block)) { if (! send_log_block(*block)) {
// failed to send the block; try again later.... // failed to send the block; try again later....
semaphore->give(); semaphore.give();
return; return;
} }
stats.resends++; stats.resends++;
} }
} }
semaphore->give(); semaphore.give();
} }
} }

2
libraries/DataFlash/DataFlash_MAVLink.h
View File

@ -178,7 +178,7 @@ private:
AP_HAL::Util::perf_counter_t _perf_packing; AP_HAL::Util::perf_counter_t _perf_packing;
AP_HAL::Util::perf_counter_t _perf_overruns; AP_HAL::Util::perf_counter_t _perf_overruns;
AP_HAL::Semaphore *semaphore; HAL_Semaphore semaphore;
}; };
#endif // DATAFLASH_MAVLINK_SUPPORT #endif // DATAFLASH_MAVLINK_SUPPORT

7
libraries/DataFlash/DataFlash_Revo.cpp
View File

@ -387,13 +387,12 @@ void DataFlash_Revo::Init()
} }
{
_spi_sem->take(10); WITH_SEMAPHORE(_spi_sem);
_spi->set_speed(AP_HAL::Device::SPEED_LOW); _spi->set_speed(AP_HAL::Device::SPEED_LOW);
DataFlash_Backend::Init(); DataFlash_Backend::Init();
}
_spi_sem->give();
df_NumPages = BOARD_DATAFLASH_PAGES - 1; // reserve last page for config information df_NumPages = BOARD_DATAFLASH_PAGES - 1; // reserve last page for config information