mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-29 20:18:31 -04:00
AP_Logger: add support for AP_Logger into AP_Periph
This commit is contained in:
parent
3ba57fed89
commit
b681dc940a
@ -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());
|
||||
}
|
||||
|
||||
|
@ -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
|
||||
|
@ -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
|
||||
|
@ -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.
|
||||
|
@ -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
|
||||
|
@ -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)
|
||||
|
@ -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()
|
||||
{
|
||||
|
@ -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
|
||||
};
|
||||
|
Loading…
Reference in New Issue
Block a user