AP_Logger: add support for AP_Logger into AP_Periph

This commit is contained in:
Peter Barker 2021-05-20 12:34:13 +10:00 committed by Andrew Tridgell
parent 3ba57fed89
commit b681dc940a
8 changed files with 58 additions and 10 deletions

View File

@ -92,12 +92,14 @@ const AP_Param::GroupInfo AP_Logger::var_info[] = {
// @User: Standard
AP_GROUPINFO("_FILE_DSRMROT", 4, AP_Logger, _params.file_disarm_rot, 0),
#if HAL_LOGGING_MAVLINK_ENABLED
// @Param: _MAV_BUFSIZE
// @DisplayName: Maximum AP_Logger MAVLink Backend buffer size
// @Description: Maximum amount of memory to allocate to AP_Logger-over-mavlink
// @User: Advanced
// @Units: kB
AP_GROUPINFO("_MAV_BUFSIZE", 5, AP_Logger, _params.mav_bufsize, HAL_LOGGING_MAV_BUFSIZE),
#endif
// @Param: _FILE_TIMEOUT
// @DisplayName: Timeout before giving up on file writes
@ -136,7 +138,7 @@ void AP_Logger::Init(const struct LogStructure *structures, uint8_t num_types)
_params.file_bufsize.convert_parameter_width(AP_PARAM_INT8);
if (hal.util->was_watchdog_armed()) {
gcs().send_text(MAV_SEVERITY_INFO, "Forcing logging for watchdog reset");
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Forcing logging for watchdog reset");
_params.log_disarmed.set(1);
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
@ -833,7 +835,9 @@ void AP_Logger::handle_mavlink_msg(GCS_MAVLINK &link, const mavlink_message_t &m
}
void AP_Logger::periodic_tasks() {
#ifndef HAL_BUILD_AP_PERIPH
handle_log_send();
#endif
FOR_EACH_BACKEND(periodic_tasks());
}

View File

@ -12,19 +12,19 @@
// set default for HAL_LOGGING_DATAFLASH_ENABLED
#ifndef HAL_LOGGING_DATAFLASH_ENABLED
#ifdef HAL_LOGGING_DATAFLASH
#define HAL_LOGGING_DATAFLASH_ENABLED 1
#define HAL_LOGGING_DATAFLASH_ENABLED HAL_LOGGING_ENABLED
#else
#define HAL_LOGGING_DATAFLASH_ENABLED 0
#endif
#endif
#ifndef HAL_LOGGING_MAVLINK_ENABLED
#define HAL_LOGGING_MAVLINK_ENABLED 1
#define HAL_LOGGING_MAVLINK_ENABLED HAL_LOGGING_ENABLED
#endif
#ifndef HAL_LOGGING_FILESYSTEM_ENABLED
#if HAVE_FILESYSTEM_SUPPORT
#define HAL_LOGGING_FILESYSTEM_ENABLED 1
#define HAL_LOGGING_FILESYSTEM_ENABLED HAL_LOGGING_ENABLED
#else
#define HAL_LOGGING_FILESYSTEM_ENABLED 0
#endif
@ -32,7 +32,7 @@
#ifndef HAL_LOGGING_SITL_ENABLED
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#define HAL_LOGGING_SITL_ENABLED 1
#define HAL_LOGGING_SITL_ENABLED HAL_LOGGING_ENABLED
#else
#define HAL_LOGGING_SITL_ENABLED 0
#endif

View File

@ -483,6 +483,7 @@ bool AP_Logger_Backend::Write_MessageF(const char *fmt, ...)
return Write_Message(msg);
}
#if HAL_RALLY_ENABLED
// Write rally points
bool AP_Logger_Backend::Write_RallyPoint(uint8_t total,
uint8_t sequence,
@ -506,6 +507,7 @@ bool AP_Logger_Backend::Write_Rally()
// kick off asynchronous write:
return _startup_messagewriter->writeallrallypoints();
}
#endif
/*
convert a list entry number back into a log number (which can then

View File

@ -148,7 +148,7 @@ void AP_Logger_File::periodic_1Hz()
if (io_thread_warning_decimation_counter == 0 && _initialised) {
// we don't print this error unless we did initialise. When _initialised is set to true
// we register the IO timer callback
gcs().send_text(MAV_SEVERITY_CRITICAL, "AP_Logger: stuck thread (%s)", last_io_operation);
GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "AP_Logger: stuck thread (%s)", last_io_operation);
}
if (io_thread_warning_decimation_counter++ > 57) {
io_thread_warning_decimation_counter = 0;
@ -972,7 +972,7 @@ bool AP_Logger_File::io_thread_alive() const
// if the io thread hasn't had a heartbeat in a while then it is
// considered dead. Three seconds is enough time for a sdcard remount.
uint32_t timeout_ms = 3000;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(HAL_BUILD_AP_PERIPH)
// the IO thread is working with hardware - writing to a physical
// disk. Unfortunately these hardware devices do not obey our
// SITL speedup options, so we allow for it here.

View File

@ -22,6 +22,8 @@
#include <AP_Logger/AP_Logger.h>
#include <GCS_MAVLink/GCS.h> // for LOG_ENTRY
#ifndef HAL_NO_GCS
extern const AP_HAL::HAL& hal;
// We avoid doing log messages when timing is critical:
@ -328,3 +330,5 @@ bool AP_Logger::handle_log_send_data()
}
return true;
}
#endif

View File

@ -264,11 +264,13 @@ bool AP_Logger_Backend::Write_Mission_Cmd(const AP_Mission &mission,
return WriteBlock(&pkt, sizeof(pkt));
}
#if HAL_MISSION_ENABLED
bool AP_Logger_Backend::Write_EntireMission()
{
// kick off asynchronous write:
return _startup_messagewriter->writeentiremission();
}
#endif
// Write a text message to the log
bool AP_Logger_Backend::Write_Message(const char *message)

View File

@ -24,7 +24,11 @@ void LoggerMessageWriter::reset()
bool LoggerMessageWriter::out_of_time_for_writing_messages() const
{
#if HAL_SCHEDULER_ENABLED
return AP::scheduler().time_available_usec() < MIN_LOOP_TIME_REMAINING_FOR_MESSAGE_WRITE_US;
#else
return false;
#endif
}
void LoggerMessageWriter_DFLogStart::reset()
@ -34,8 +38,12 @@ void LoggerMessageWriter_DFLogStart::reset()
_fmt_done = false;
_params_done = false;
_writesysinfo.reset();
#if HAL_MISSION_ENABLED
_writeentiremission.reset();
#endif
#if HAL_RALLY_ENABLED
_writeallrallypoints.reset();
#endif
stage = Stage::FORMATS;
next_format_to_send = 0;
@ -49,7 +57,11 @@ bool LoggerMessageWriter_DFLogStart::out_of_time_for_writing_messages() const
{
if (stage == Stage::FORMATS) {
// write out the FMT messages as fast as we can
#if HAL_SCHEDULER_ENABLED
return AP::scheduler().time_available_usec() == 0;
#else
return false;
#endif
}
return LoggerMessageWriter::out_of_time_for_writing_messages();
}
@ -122,18 +134,22 @@ void LoggerMessageWriter_DFLogStart::process()
return;
}
}
#if HAL_MISSION_ENABLED
if (!_writeentiremission.finished()) {
_writeentiremission.process();
if (!_writeentiremission.finished()) {
return;
}
}
#endif
#if HAL_RALLY_ENABLED
if (!_writeallrallypoints.finished()) {
_writeallrallypoints.process();
if (!_writeallrallypoints.finished()) {
return;
}
}
#endif
stage = Stage::VEHICLE_MESSAGES;
FALLTHROUGH;
@ -157,6 +173,7 @@ void LoggerMessageWriter_DFLogStart::process()
_finished = true;
}
#if HAL_MISSION_ENABLED
bool LoggerMessageWriter_DFLogStart::writeentiremission()
{
if (stage != Stage::DONE) {
@ -167,7 +184,9 @@ bool LoggerMessageWriter_DFLogStart::writeentiremission()
_writeentiremission.reset();
return true;
}
#endif
#if HAL_RALLY_ENABLED
bool LoggerMessageWriter_DFLogStart::writeallrallypoints()
{
if (stage != Stage::DONE) {
@ -178,6 +197,7 @@ bool LoggerMessageWriter_DFLogStart::writeallrallypoints()
_writeallrallypoints.reset();
return true;
}
#endif
void LoggerMessageWriter_WriteSysInfo::reset()
{

View File

@ -75,17 +75,25 @@ private:
class LoggerMessageWriter_DFLogStart : public LoggerMessageWriter {
public:
LoggerMessageWriter_DFLogStart() :
_writesysinfo(),
_writeentiremission(),
_writeallrallypoints()
_writesysinfo()
#if HAL_MISSION_ENABLED
, _writeentiremission()
#endif
#if HAL_RALLY_ENABLED
, _writeallrallypoints()
#endif
{
}
virtual void set_logger_backend(class AP_Logger_Backend *backend) override {
LoggerMessageWriter::set_logger_backend(backend);
_writesysinfo.set_logger_backend(backend);
#if HAL_MISSION_ENABLED
_writeentiremission.set_logger_backend(backend);
#endif
#if HAL_RALLY_ENABLED
_writeallrallypoints.set_logger_backend(backend);
#endif
}
bool out_of_time_for_writing_messages() const;
@ -97,8 +105,12 @@ public:
// reset some writers so we push stuff out to logs again. Will
// only work if we are in state DONE!
#if HAL_MISSION_ENABLED
bool writeentiremission();
#endif
#if HAL_RALLY_ENABLED
bool writeallrallypoints();
#endif
private:
@ -130,6 +142,10 @@ private:
LoggerMessageWriter_WriteSysInfo _writesysinfo;
#if HAL_MISSION_ENABLED
LoggerMessageWriter_WriteEntireMission _writeentiremission;
#endif
#if HAL_RALLY_ENABLED
LoggerMessageWriter_WriteAllRallyPoints _writeallrallypoints;
#endif
};