diff --git a/libraries/DataFlash/DataFlash_File.cpp b/libraries/DataFlash/DataFlash_File.cpp index abb895f280..07d01da636 100644 --- a/libraries/DataFlash/DataFlash_File.cpp +++ b/libraries/DataFlash/DataFlash_File.cpp @@ -98,17 +98,6 @@ void DataFlash_File::Init() int ret; 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 // try to cope with an existing lowercase log directory // 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; } - if (!semaphore->take(1)) { + if (!semaphore.take(1)) { return false; } @@ -559,7 +548,7 @@ bool DataFlash_File::_WritePrioritisedBlock(const void *pBuffer, uint16_t size, if (!must_dribble && space < non_messagewriter_message_reserved_space()) { // this message isn't dropped, it will be sent again... - semaphore->give(); + semaphore.give(); return false; } 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: if (!is_critical && space < critical_message_reserved_space()) { _dropped++; - semaphore->give(); + semaphore.give(); return false; } } @@ -576,13 +565,13 @@ bool DataFlash_File::_WritePrioritisedBlock(const void *pBuffer, uint16_t size, if (space < size) { hal.util->perf_count(_perf_overruns); _dropped++; - semaphore->give(); + semaphore.give(); return false; } _writebuf.write((uint8_t*)pBuffer, size); df_stats_gather(size); - semaphore->give(); + semaphore.give(); return true; } @@ -613,20 +602,20 @@ uint16_t DataFlash_File::find_last_log() 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); if (fname == nullptr) { 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) { // it is the file we are currently writing free(fname); - write_fd_semaphore->give(); + write_fd_semaphore.give(); return _write_offset; } - write_fd_semaphore->give(); + write_fd_semaphore.give(); } struct stat st; 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; } -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); if (fname == nullptr) { 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) { // it is the file we are currently writing free(fname); - write_fd_semaphore->give(); + write_fd_semaphore.give(); uint64_t utc_usec; if (!AP::rtc().get_utc_usec(utc_usec)) { return 0; } return utc_usec / 1000000U; } - write_fd_semaphore->give(); + write_fd_semaphore.give(); } struct stat st; if (::stat(fname, &st) != 0) { @@ -837,14 +826,14 @@ uint16_t DataFlash_File::get_num_logs() void DataFlash_File::stop_logging(void) { // 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) { int fd = _write_fd; _write_fd = -1; ::close(fd); } if (have_sem) { - write_fd_semaphore->give(); + write_fd_semaphore.give(); } else { _internal_errors++; } @@ -892,7 +881,7 @@ uint16_t DataFlash_File::start_new_log(void) if (log_num > MAX_LOG_FILES) { log_num = 1; } - if (!write_fd_semaphore->take(1)) { + if (!write_fd_semaphore.take(1)) { _open_error = true; return 0xFFFF; } @@ -903,7 +892,7 @@ uint16_t DataFlash_File::start_new_log(void) _write_filename = _log_file_name(log_num); if (_write_filename == nullptr) { _open_error = true; - write_fd_semaphore->give(); + write_fd_semaphore.give(); return 0xFFFF; } #if HAL_OS_POSIX_IO @@ -917,7 +906,7 @@ uint16_t DataFlash_File::start_new_log(void) if (_write_fd == -1) { _initialised = false; _open_error = true; - write_fd_semaphore->give(); + write_fd_semaphore.give(); int saved_errno = errno; ::printf("Log open fail for %s - %s\n", _write_filename, strerror(saved_errno)); @@ -928,7 +917,7 @@ uint16_t DataFlash_File::start_new_log(void) _last_write_ms = AP_HAL::millis(); _write_offset = 0; _writebuf.clear(); - write_fd_semaphore->give(); + write_fd_semaphore.give(); // now update lastlog.txt with the new log number char *fname = _lastlog_file_name(); @@ -974,11 +963,11 @@ void DataFlash_File::flush(void) } _io_timer(); } - if (write_fd_semaphore->take(1)) { + if (write_fd_semaphore.take(1)) { if (_write_fd != -1) { ::fsync(_write_fd); } - write_fd_semaphore->give(); + write_fd_semaphore.give(); } else { _internal_errors++; } @@ -1042,11 +1031,11 @@ void DataFlash_File::_io_timer(void) } last_io_operation = "write"; - if (!write_fd_semaphore->take(1)) { + if (!write_fd_semaphore.take(1)) { return; } if (_write_fd == -1) { - write_fd_semaphore->give(); + write_fd_semaphore.give(); return; } ssize_t nwritten = ::write(_write_fd, head, nbytes); @@ -1080,7 +1069,7 @@ void DataFlash_File::_io_timer(void) last_io_operation = ""; #endif } - write_fd_semaphore->give(); + write_fd_semaphore.give(); hal.util->perf_end(_perf_write); } diff --git a/libraries/DataFlash/DataFlash_File.h b/libraries/DataFlash/DataFlash_File.h index 7a0e2a6ec0..87d1fe3309 100644 --- a/libraries/DataFlash/DataFlash_File.h +++ b/libraries/DataFlash/DataFlash_File.h @@ -109,8 +109,8 @@ private: char *_log_file_name_long(const uint16_t log_num) const; char *_log_file_name_short(const uint16_t log_num) const; char *_lastlog_file_name() const; - uint32_t _get_log_size(const uint16_t log_num) const; - uint32_t _get_log_time(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); void stop_logging(void) override; @@ -146,11 +146,11 @@ private: const uint32_t _free_space_min_avail = 8388608; // bytes // semaphore mediates access to the ringbuffer - AP_HAL::Semaphore *semaphore; + HAL_Semaphore semaphore; // write_fd_semaphore mediates access to write_fd so the frontend // can open/close files without causing the backend to write to a // bad fd - AP_HAL::Semaphore *write_fd_semaphore; + HAL_Semaphore write_fd_semaphore; // performance counters AP_HAL::Util::perf_counter_t _perf_write; diff --git a/libraries/DataFlash/DataFlash_File_sd.cpp b/libraries/DataFlash/DataFlash_File_sd.cpp index fc2c8bd195..b11af43e45 100644 --- a/libraries/DataFlash/DataFlash_File_sd.cpp +++ b/libraries/DataFlash/DataFlash_File_sd.cpp @@ -58,13 +58,6 @@ void DataFlash_File::Init() 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 const char* custom_dir = hal.util->get_custom_log_directory(); if (custom_dir != nullptr){ @@ -457,7 +450,7 @@ bool DataFlash_File::_WritePrioritisedBlock(const void *pBuffer, uint16_t size, return false; } - if (!semaphore->take(1)) { + if (!semaphore.take(1)) { return false; } @@ -471,7 +464,7 @@ bool DataFlash_File::_WritePrioritisedBlock(const void *pBuffer, uint16_t size, // things: if (space < non_messagewriter_message_reserved_space()) { // this message isn't dropped, it will be sent again... - semaphore->give(); + semaphore.give(); return false; } } else { @@ -479,7 +472,7 @@ bool DataFlash_File::_WritePrioritisedBlock(const void *pBuffer, uint16_t size, if (!is_critical && space < critical_message_reserved_space()) { // printf("dropping NC block! size=%d\n", size); _dropped++; - semaphore->give(); + semaphore.give(); return false; } } @@ -490,14 +483,14 @@ bool DataFlash_File::_WritePrioritisedBlock(const void *pBuffer, uint16_t size, printf("dropping block! size=%d\n", size); _dropped++; - semaphore->give(); + semaphore.give(); return false; } _writebuf.write((uint8_t*)pBuffer, size); has_data=true; - semaphore->give(); + semaphore.give(); return true; } diff --git a/libraries/DataFlash/DataFlash_File_sd.h b/libraries/DataFlash/DataFlash_File_sd.h index c96e9bbe4a..b020f70a93 100644 --- a/libraries/DataFlash/DataFlash_File_sd.h +++ b/libraries/DataFlash/DataFlash_File_sd.h @@ -97,7 +97,7 @@ private: /* construct a file name given a log number. Caller must free. */ char *_log_file_name(const uint16_t log_num) 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; void stop_logging(void); @@ -127,7 +127,7 @@ private: float avail_space_percent(uint32_t *free = NULL); - AP_HAL::Semaphore *semaphore; + HAL_Semaphore semaphore; bool has_data; diff --git a/libraries/DataFlash/DataFlash_MAVLink.cpp b/libraries/DataFlash/DataFlash_MAVLink.cpp index cf1cfb84f3..810b793f2e 100644 --- a/libraries/DataFlash/DataFlash_MAVLink.cpp +++ b/libraries/DataFlash/DataFlash_MAVLink.cpp @@ -25,12 +25,6 @@ extern const AP_HAL::HAL& hal; // initialisation 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(); _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 bool DataFlash_MAVLink::_WritePrioritisedBlock(const void *pBuffer, uint16_t size, bool is_critical) { - if (!semaphore->take_nonblocking()) { + if (!semaphore.take_nonblocking()) { _dropped++; return false; } if (! WriteBlockCheckStartupMessages()) { - semaphore->give(); + semaphore.give(); 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... _dropped++; } - semaphore->give(); + semaphore.give(); return false; } @@ -155,7 +149,7 @@ bool DataFlash_MAVLink::_WritePrioritisedBlock(const void *pBuffer, uint16_t siz if (_current_block == nullptr) { // should not happen - there's a sanity check above internal_error(); - semaphore->give(); + semaphore.give(); return false; } } @@ -172,7 +166,7 @@ bool DataFlash_MAVLink::_WritePrioritisedBlock(const void *pBuffer, uint16_t siz } } - semaphore->give(); + semaphore.give(); 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_msg_remote_log_block_status_decode(msg, &packet); - if (!semaphore->take_nonblocking()) { + if (!semaphore.take_nonblocking()) { return; } if(packet.status == 0){ @@ -286,7 +280,7 @@ void DataFlash_MAVLink::remote_log_block_status_msg(mavlink_channel_t chan, } else{ handle_ack(chan, msg, packet.seqno); } - semaphore->give(); + semaphore.give(); } void DataFlash_MAVLink::handle_retry(uint32_t seqno) @@ -400,7 +394,7 @@ void DataFlash_MAVLink::stats_collect() if (!_initialised) { return; } - if (!semaphore->take_nonblocking()) { + if (!semaphore.take_nonblocking()) { return; } uint8_t pending = queue_size(_blocks_pending); @@ -411,7 +405,7 @@ void DataFlash_MAVLink::stats_collect() if (sfree != _blockcount_free) { internal_error(); } - semaphore->give(); + semaphore.give(); stats.state_pending += pending; stats.state_sent += sent; @@ -478,20 +472,20 @@ void DataFlash_MAVLink::push_log_blocks() DataFlash_Backend::WriteMoreStartupMessages(); - if (!semaphore->take_nonblocking()) { + if (!semaphore.take_nonblocking()) { return; } if (! send_log_blocks_from_queue(_blocks_retry)) { - semaphore->give(); + semaphore.give(); return; } if (! send_log_blocks_from_queue(_blocks_pending)) { - semaphore->give(); + semaphore.give(); return; } - semaphore->give(); + semaphore.give(); } 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. while (count_to_send-- > 0) { - if (!semaphore->take_nonblocking()) { + if (!semaphore.take_nonblocking()) { return; } 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 (! send_log_block(*block)) { // failed to send the block; try again later.... - semaphore->give(); + semaphore.give(); return; } stats.resends++; } } - semaphore->give(); + semaphore.give(); } } diff --git a/libraries/DataFlash/DataFlash_MAVLink.h b/libraries/DataFlash/DataFlash_MAVLink.h index 589f3acfc7..829729cdb2 100644 --- a/libraries/DataFlash/DataFlash_MAVLink.h +++ b/libraries/DataFlash/DataFlash_MAVLink.h @@ -178,7 +178,7 @@ private: AP_HAL::Util::perf_counter_t _perf_packing; AP_HAL::Util::perf_counter_t _perf_overruns; - AP_HAL::Semaphore *semaphore; + HAL_Semaphore semaphore; }; #endif // DATAFLASH_MAVLINK_SUPPORT diff --git a/libraries/DataFlash/DataFlash_Revo.cpp b/libraries/DataFlash/DataFlash_Revo.cpp index 7080d3bf00..e080c1ad25 100644 --- a/libraries/DataFlash/DataFlash_Revo.cpp +++ b/libraries/DataFlash/DataFlash_Revo.cpp @@ -387,14 +387,13 @@ void DataFlash_Revo::Init() } + { + WITH_SEMAPHORE(_spi_sem); + _spi->set_speed(AP_HAL::Device::SPEED_LOW); - _spi_sem->take(10); - _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 ReadManufacturerID();