mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AP_logger: rename dataflash to logger and refactor the filename logger.bin
This commit is contained in:
parent
91b0650765
commit
027f9afcf8
@ -96,7 +96,7 @@ void AP_Logger::Init(const struct LogStructure *structures, uint8_t num_types)
|
||||
validate_structures(structures, num_types);
|
||||
dump_structures(structures, num_types);
|
||||
#endif
|
||||
if (_next_backend == DATAFLASH_MAX_BACKENDS) {
|
||||
if (_next_backend == LOGGER_MAX_BACKENDS) {
|
||||
AP_HAL::panic("Too many backends");
|
||||
return;
|
||||
}
|
||||
@ -122,9 +122,9 @@ void AP_Logger::Init(const struct LogStructure *structures, uint8_t num_types)
|
||||
#endif
|
||||
#endif // HAL_BOARD_LOG_DIRECTORY
|
||||
|
||||
#if DATAFLASH_MAVLINK_SUPPORT
|
||||
#if LOGGER_MAVLINK_SUPPORT
|
||||
if (_params.backend_types & uint8_t(Backend_Type::MAVLINK)) {
|
||||
if (_next_backend == DATAFLASH_MAX_BACKENDS) {
|
||||
if (_next_backend == LOGGER_MAX_BACKENDS) {
|
||||
AP_HAL::panic("Too many backends");
|
||||
return;
|
||||
}
|
||||
@ -144,7 +144,7 @@ void AP_Logger::Init(const struct LogStructure *structures, uint8_t num_types)
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
if (_params.backend_types & uint8_t(Backend_Type::BLOCK)) {
|
||||
if (_next_backend == DATAFLASH_MAX_BACKENDS) {
|
||||
if (_next_backend == LOGGER_MAX_BACKENDS) {
|
||||
AP_HAL::panic("Too many backends");
|
||||
return;
|
||||
}
|
||||
@ -163,7 +163,7 @@ void AP_Logger::Init(const struct LogStructure *structures, uint8_t num_types)
|
||||
|
||||
#ifdef HAL_LOGGING_DATAFLASH
|
||||
if (_params.backend_types & uint8_t(Backend_Type::BLOCK)) {
|
||||
if (_next_backend == DATAFLASH_MAX_BACKENDS) {
|
||||
if (_next_backend == LOGGER_MAX_BACKENDS) {
|
||||
AP_HAL::panic("Too many backends");
|
||||
return;
|
||||
}
|
||||
|
@ -377,9 +377,9 @@ protected:
|
||||
bool is_critical);
|
||||
|
||||
private:
|
||||
#define DATAFLASH_MAX_BACKENDS 2
|
||||
#define LOGGER_MAX_BACKENDS 2
|
||||
uint8_t _next_backend;
|
||||
AP_Logger_Backend *backends[DATAFLASH_MAX_BACKENDS];
|
||||
AP_Logger_Backend *backends[LOGGER_MAX_BACKENDS];
|
||||
const AP_Int32 &_log_bitmask;
|
||||
|
||||
enum class Backend_Type : uint8_t {
|
||||
|
@ -11,7 +11,7 @@ AP_Logger_Backend::AP_Logger_Backend(AP_Logger &front,
|
||||
_front(front),
|
||||
_startup_messagewriter(writer)
|
||||
{
|
||||
writer->set_dataflash_backend(this);
|
||||
writer->set_logger_backend(this);
|
||||
}
|
||||
|
||||
uint8_t AP_Logger_Backend::num_types() const
|
||||
@ -432,6 +432,6 @@ bool AP_Logger_Backend::Write_RallyPoint(uint8_t total,
|
||||
void AP_Logger_Backend::Write_Rally()
|
||||
{
|
||||
LoggerMessageWriter_WriteAllRallyPoints writer;
|
||||
writer.set_dataflash_backend(this);
|
||||
writer.set_logger_backend(this);
|
||||
writer.process();
|
||||
}
|
||||
|
@ -67,10 +67,10 @@ public:
|
||||
virtual void flush(void) { }
|
||||
#endif
|
||||
|
||||
// for Dataflash_MAVlink
|
||||
// for Logger_MAVlink
|
||||
virtual void remote_log_block_status_msg(mavlink_channel_t chan,
|
||||
mavlink_message_t* msg) { }
|
||||
// end for Dataflash_MAVlink
|
||||
// end for Logger_MAVlink
|
||||
|
||||
virtual void periodic_tasks();
|
||||
|
||||
|
@ -66,7 +66,7 @@ uint32_t AP_Logger_Block::bufferspace_available()
|
||||
return df_NumPages * df_PageSize;
|
||||
}
|
||||
|
||||
// *** DATAFLASH PUBLIC FUNCTIONS ***
|
||||
// *** LOGGER PUBLIC FUNCTIONS ***
|
||||
void AP_Logger_Block::StartWrite(uint32_t PageAdr)
|
||||
{
|
||||
df_PageAdr = PageAdr;
|
||||
|
@ -48,7 +48,7 @@
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
#define MAX_LOG_FILES 500U
|
||||
#define DATAFLASH_PAGE_SIZE 1024UL
|
||||
#define LOGGER_PAGE_SIZE 1024UL
|
||||
|
||||
#ifndef HAL_LOGGER_WRITE_CHUNK_SIZE
|
||||
#define HAL_LOGGER_WRITE_CHUNK_SIZE 4096
|
||||
@ -665,7 +665,7 @@ void AP_Logger_File::get_log_boundaries(const uint16_t list_entry, uint32_t & st
|
||||
}
|
||||
|
||||
start_page = 0;
|
||||
end_page = _get_log_size(log_num) / DATAFLASH_PAGE_SIZE;
|
||||
end_page = _get_log_size(log_num) / LOGGER_PAGE_SIZE;
|
||||
}
|
||||
|
||||
/*
|
||||
@ -708,7 +708,7 @@ int16_t AP_Logger_File::get_log_data(const uint16_t list_entry, const uint16_t p
|
||||
_read_offset = 0;
|
||||
_read_fd_log_num = log_num;
|
||||
}
|
||||
uint32_t ofs = page * (uint32_t)DATAFLASH_PAGE_SIZE + offset;
|
||||
uint32_t ofs = page * (uint32_t)LOGGER_PAGE_SIZE + offset;
|
||||
|
||||
/*
|
||||
this rather strange bit of code is here to work around a bug
|
||||
|
@ -4,7 +4,7 @@
|
||||
|
||||
#include "AP_Logger_MAVLink.h"
|
||||
|
||||
#if DATAFLASH_MAVLINK_SUPPORT
|
||||
#if LOGGER_MAVLINK_SUPPORT
|
||||
|
||||
#include "LogStructure.h"
|
||||
|
||||
@ -318,27 +318,27 @@ void AP_Logger_MAVLink::stats_reset() {
|
||||
stats.collection_count = 0;
|
||||
}
|
||||
|
||||
void AP_Logger_MAVLink::Write_DF_MAV(AP_Logger_MAVLink &df)
|
||||
void AP_Logger_MAVLink::Write_logger_MAV(AP_Logger_MAVLink &logger_mav)
|
||||
{
|
||||
if (df.stats.collection_count == 0) {
|
||||
if (logger_mav.stats.collection_count == 0) {
|
||||
return;
|
||||
}
|
||||
struct log_DF_MAV_Stats pkt = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_DF_MAV_STATS),
|
||||
struct log_MAV_Stats pkt = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_MAV_STATS),
|
||||
timestamp : AP_HAL::millis(),
|
||||
seqno : df._next_seq_num-1,
|
||||
dropped : df._dropped,
|
||||
retries : df._blocks_retry.sent_count,
|
||||
resends : df.stats.resends,
|
||||
state_free_avg : (uint8_t)(df.stats.state_free/df.stats.collection_count),
|
||||
state_free_min : df.stats.state_free_min,
|
||||
state_free_max : df.stats.state_free_max,
|
||||
state_pending_avg : (uint8_t)(df.stats.state_pending/df.stats.collection_count),
|
||||
state_pending_min : df.stats.state_pending_min,
|
||||
state_pending_max : df.stats.state_pending_max,
|
||||
state_sent_avg : (uint8_t)(df.stats.state_sent/df.stats.collection_count),
|
||||
state_sent_min : df.stats.state_sent_min,
|
||||
state_sent_max : df.stats.state_sent_max,
|
||||
seqno : logger_mav._next_seq_num-1,
|
||||
dropped : logger_mav._dropped,
|
||||
retries : logger_mav._blocks_retry.sent_count,
|
||||
resends : logger_mav.stats.resends,
|
||||
state_free_avg : (uint8_t)(logger_mav.stats.state_free/logger_mav.stats.collection_count),
|
||||
state_free_min : logger_mav.stats.state_free_min,
|
||||
state_free_max : logger_mav.stats.state_free_max,
|
||||
state_pending_avg : (uint8_t)(logger_mav.stats.state_pending/logger_mav.stats.collection_count),
|
||||
state_pending_min : logger_mav.stats.state_pending_min,
|
||||
state_pending_max : logger_mav.stats.state_pending_max,
|
||||
state_sent_avg : (uint8_t)(logger_mav.stats.state_sent/logger_mav.stats.collection_count),
|
||||
state_sent_min : logger_mav.stats.state_sent_min,
|
||||
state_sent_max : logger_mav.stats.state_sent_max,
|
||||
};
|
||||
WriteBlock(&pkt,sizeof(pkt));
|
||||
}
|
||||
@ -351,7 +351,7 @@ void AP_Logger_MAVLink::stats_log()
|
||||
if (stats.collection_count == 0) {
|
||||
return;
|
||||
}
|
||||
Write_DF_MAV(*this);
|
||||
Write_logger_MAV(*this);
|
||||
#if REMOTE_LOG_DEBUGGING
|
||||
printf("D:%d Retry:%d Resent:%d SF:%d/%d/%d SP:%d/%d/%d SS:%d/%d/%d SR:%d/%d/%d\n",
|
||||
dropped,
|
||||
|
@ -5,9 +5,9 @@
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#define DATAFLASH_MAVLINK_SUPPORT 1
|
||||
#define LOGGER_MAVLINK_SUPPORT 1
|
||||
|
||||
#if DATAFLASH_MAVLINK_SUPPORT
|
||||
#if LOGGER_MAVLINK_SUPPORT
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
@ -144,7 +144,7 @@ private:
|
||||
uint8_t _next_block_number_to_resend;
|
||||
bool _sending_to_client;
|
||||
|
||||
void Write_DF_MAV(AP_Logger_MAVLink &df);
|
||||
void Write_logger_MAV(AP_Logger_MAVLink &logger);
|
||||
|
||||
uint32_t bufferspace_available() override; // in bytes
|
||||
uint8_t remaining_space_in_current_block();
|
||||
@ -181,4 +181,4 @@ private:
|
||||
HAL_Semaphore semaphore;
|
||||
};
|
||||
|
||||
#endif // DATAFLASH_MAVLINK_SUPPORT
|
||||
#endif // LOGGER_MAVLINK_SUPPORT
|
||||
|
@ -27,14 +27,14 @@ extern const AP_HAL::HAL& hal;
|
||||
void AP_Logger_SITL::Init()
|
||||
{
|
||||
if (flash_fd == 0) {
|
||||
flash_fd = open("dataflash.bin", O_RDWR|O_CLOEXEC, 0777);
|
||||
flash_fd = open(filename, O_RDWR|O_CLOEXEC, 0777);
|
||||
if (flash_fd == -1) {
|
||||
flash_fd = open("dataflash.bin", O_RDWR | O_CREAT | O_CLOEXEC, 0777);
|
||||
flash_fd = open(filename, O_RDWR | O_CREAT | O_CLOEXEC, 0777);
|
||||
StartErase();
|
||||
erase_started_ms = 0;
|
||||
}
|
||||
if (ftruncate(flash_fd, DF_PAGE_SIZE*uint32_t(DF_NUM_PAGES)) != 0) {
|
||||
AP_HAL::panic("Failed to create dataflash.bin");
|
||||
AP_HAL::panic("Failed to create %s", filename);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -16,6 +16,7 @@ public:
|
||||
AP_Logger_Block(front, writer) {}
|
||||
void Init() override;
|
||||
bool CardInserted() const override;
|
||||
static constexpr const char *filename = "dataflash.bin";
|
||||
|
||||
private:
|
||||
void BufferToPage(uint32_t PageAdr) override;
|
||||
|
@ -431,7 +431,7 @@ bool AP_Logger_Backend::Write_Mission_Cmd(const AP_Mission &mission,
|
||||
void AP_Logger_Backend::Write_EntireMission()
|
||||
{
|
||||
LoggerMessageWriter_WriteEntireMission writer;
|
||||
writer.set_dataflash_backend(this);
|
||||
writer.set_logger_backend(this);
|
||||
writer.process();
|
||||
}
|
||||
|
||||
|
@ -907,7 +907,7 @@ struct PACKED log_GYRO {
|
||||
float GyrX, GyrY, GyrZ;
|
||||
};
|
||||
|
||||
struct PACKED log_DF_MAV_Stats {
|
||||
struct PACKED log_MAV_Stats {
|
||||
LOG_PACKET_HEADER;
|
||||
uint32_t timestamp;
|
||||
uint32_t seqno;
|
||||
@ -1310,7 +1310,7 @@ Format characters in the format string for binary log messages
|
||||
"MODE", "QMBB", "TimeUS,Mode,ModeNum,Rsn", "s---", "F---" }, \
|
||||
{ LOG_RFND_MSG, sizeof(log_RFND), \
|
||||
"RFND", "QCBBCBB", "TimeUS,Dist1,Stat1,Orient1,Dist2,Stat2,Orient2", "sm--m--", "FB--B--" }, \
|
||||
{ LOG_DF_MAV_STATS, sizeof(log_DF_MAV_Stats), \
|
||||
{ LOG_MAV_STATS, sizeof(log_MAV_Stats), \
|
||||
"DMS", "IIIIIBBBBBBBBB", "TimeMS,N,Dp,RT,RS,Fa,Fmn,Fmx,Pa,Pmn,Pmx,Sa,Smn,Smx", "s-------------", "C-------------" }, \
|
||||
{ LOG_BEACON_MSG, sizeof(log_Beacon), \
|
||||
"BCN", "QBBfffffff", "TimeUS,Health,Cnt,D0,D1,D2,D3,PosX,PosY,PosZ", "s--mmmmmmm", "F--BBBBBBB" }, \
|
||||
@ -1625,7 +1625,7 @@ enum LogMessages : uint8_t {
|
||||
LOG_GPAB_MSG,
|
||||
LOG_RFND_MSG,
|
||||
LOG_BAR3_MSG,
|
||||
LOG_DF_MAV_STATS,
|
||||
LOG_MAV_STATS,
|
||||
LOG_FORMAT_UNITS_MSG,
|
||||
LOG_UNIT_MSG,
|
||||
LOG_MULT_MSG,
|
||||
|
@ -38,8 +38,8 @@ void LoggerMessageWriter_DFLogStart::process()
|
||||
switch(stage) {
|
||||
case ls_blockwriter_stage_formats:
|
||||
// write log formats so the log is self-describing
|
||||
while (next_format_to_send < _dataflash_backend->num_types()) {
|
||||
if (!_dataflash_backend->Write_Format(_dataflash_backend->structure(next_format_to_send))) {
|
||||
while (next_format_to_send < _logger_backend->num_types()) {
|
||||
if (!_logger_backend->Write_Format(_logger_backend->structure(next_format_to_send))) {
|
||||
return; // call me again!
|
||||
}
|
||||
next_format_to_send++;
|
||||
@ -49,8 +49,8 @@ void LoggerMessageWriter_DFLogStart::process()
|
||||
FALLTHROUGH;
|
||||
|
||||
case ls_blockwriter_stage_units:
|
||||
while (_next_unit_to_send < _dataflash_backend->num_units()) {
|
||||
if (!_dataflash_backend->Write_Unit(_dataflash_backend->unit(_next_unit_to_send))) {
|
||||
while (_next_unit_to_send < _logger_backend->num_units()) {
|
||||
if (!_logger_backend->Write_Unit(_logger_backend->unit(_next_unit_to_send))) {
|
||||
return; // call me again!
|
||||
}
|
||||
_next_unit_to_send++;
|
||||
@ -59,8 +59,8 @@ void LoggerMessageWriter_DFLogStart::process()
|
||||
FALLTHROUGH;
|
||||
|
||||
case ls_blockwriter_stage_multipliers:
|
||||
while (_next_multiplier_to_send < _dataflash_backend->num_multipliers()) {
|
||||
if (!_dataflash_backend->Write_Multiplier(_dataflash_backend->multiplier(_next_multiplier_to_send))) {
|
||||
while (_next_multiplier_to_send < _logger_backend->num_multipliers()) {
|
||||
if (!_logger_backend->Write_Multiplier(_logger_backend->multiplier(_next_multiplier_to_send))) {
|
||||
return; // call me again!
|
||||
}
|
||||
_next_multiplier_to_send++;
|
||||
@ -69,8 +69,8 @@ void LoggerMessageWriter_DFLogStart::process()
|
||||
FALLTHROUGH;
|
||||
|
||||
case ls_blockwriter_stage_format_units:
|
||||
while (_next_format_unit_to_send < _dataflash_backend->num_types()) {
|
||||
if (!_dataflash_backend->Write_Format_Units(_dataflash_backend->structure(_next_format_unit_to_send))) {
|
||||
while (_next_format_unit_to_send < _logger_backend->num_types()) {
|
||||
if (!_logger_backend->Write_Format_Units(_logger_backend->structure(_next_format_unit_to_send))) {
|
||||
return; // call me again!
|
||||
}
|
||||
_next_format_unit_to_send++;
|
||||
@ -80,7 +80,7 @@ void LoggerMessageWriter_DFLogStart::process()
|
||||
|
||||
case ls_blockwriter_stage_parms:
|
||||
while (ap) {
|
||||
if (!_dataflash_backend->Write_Parameter(ap, token, type)) {
|
||||
if (!_logger_backend->Write_Parameter(ap, token, type)) {
|
||||
return;
|
||||
}
|
||||
ap = AP_Param::next_scalar(&token, &type);
|
||||
@ -117,11 +117,11 @@ void LoggerMessageWriter_DFLogStart::process()
|
||||
// we guarantee 200 bytes of space for the vehicle startup
|
||||
// messages. This allows them to be simple functions rather
|
||||
// than e.g. LoggerMessageWriter-based state machines
|
||||
if (_dataflash_backend->vehicle_message_writer()) {
|
||||
if (_dataflash_backend->bufferspace_available() < 200) {
|
||||
if (_logger_backend->vehicle_message_writer()) {
|
||||
if (_logger_backend->bufferspace_available() < 200) {
|
||||
return;
|
||||
}
|
||||
(_dataflash_backend->vehicle_message_writer())();
|
||||
(_logger_backend->vehicle_message_writer())();
|
||||
}
|
||||
stage = ls_blockwriter_stage_done;
|
||||
FALLTHROUGH;
|
||||
@ -149,7 +149,7 @@ void LoggerMessageWriter_WriteSysInfo::process() {
|
||||
FALLTHROUGH;
|
||||
|
||||
case ws_blockwriter_stage_firmware_string:
|
||||
if (! _dataflash_backend->Write_Message(fwver.fw_string)) {
|
||||
if (! _logger_backend->Write_Message(fwver.fw_string)) {
|
||||
return; // call me again
|
||||
}
|
||||
stage = ws_blockwriter_stage_git_versions;
|
||||
@ -157,7 +157,7 @@ void LoggerMessageWriter_WriteSysInfo::process() {
|
||||
|
||||
case ws_blockwriter_stage_git_versions:
|
||||
if (fwver.middleware_name && fwver.os_name) {
|
||||
if (! _dataflash_backend->Write_MessageF("%s: %s %s: %s",
|
||||
if (! _logger_backend->Write_MessageF("%s: %s %s: %s",
|
||||
fwver.middleware_name,
|
||||
fwver.middleware_hash_str,
|
||||
fwver.os_name,
|
||||
@ -165,7 +165,7 @@ void LoggerMessageWriter_WriteSysInfo::process() {
|
||||
return; // call me again
|
||||
}
|
||||
} else if (fwver.os_name) {
|
||||
if (! _dataflash_backend->Write_MessageF("%s: %s",
|
||||
if (! _logger_backend->Write_MessageF("%s: %s",
|
||||
fwver.os_name,
|
||||
fwver.os_hash_str)) {
|
||||
return; // call me again
|
||||
@ -177,7 +177,7 @@ void LoggerMessageWriter_WriteSysInfo::process() {
|
||||
case ws_blockwriter_stage_system_id:
|
||||
char sysid[40];
|
||||
if (hal.util->get_system_id(sysid)) {
|
||||
if (! _dataflash_backend->Write_Message(sysid)) {
|
||||
if (! _logger_backend->Write_Message(sysid)) {
|
||||
return; // call me again
|
||||
}
|
||||
}
|
||||
@ -197,7 +197,7 @@ void LoggerMessageWriter_WriteAllRallyPoints::process()
|
||||
switch(stage) {
|
||||
|
||||
case ar_blockwriter_stage_write_new_rally_message:
|
||||
if (! _dataflash_backend->Write_Message("New rally")) {
|
||||
if (! _logger_backend->Write_Message("New rally")) {
|
||||
return; // call me again
|
||||
}
|
||||
stage = ar_blockwriter_stage_write_all_rally_points;
|
||||
@ -207,7 +207,7 @@ void LoggerMessageWriter_WriteAllRallyPoints::process()
|
||||
while (_rally_number_to_send < _rally->get_rally_total()) {
|
||||
RallyLocation rallypoint;
|
||||
if (_rally->get_rally_point_with_index(_rally_number_to_send, rallypoint)) {
|
||||
if (!_dataflash_backend->Write_RallyPoint(
|
||||
if (!_logger_backend->Write_RallyPoint(
|
||||
_rally->get_rally_total(),
|
||||
_rally_number_to_send,
|
||||
rallypoint)) {
|
||||
@ -243,7 +243,7 @@ void LoggerMessageWriter_WriteEntireMission::process() {
|
||||
switch(stage) {
|
||||
|
||||
case em_blockwriter_stage_write_new_mission_message:
|
||||
if (! _dataflash_backend->Write_Message("New mission")) {
|
||||
if (! _logger_backend->Write_Message("New mission")) {
|
||||
return; // call me again
|
||||
}
|
||||
stage = em_blockwriter_stage_write_mission_items;
|
||||
@ -255,7 +255,7 @@ void LoggerMessageWriter_WriteEntireMission::process() {
|
||||
// upon failure to write the mission we will re-read from
|
||||
// storage; this could be improved.
|
||||
if (_mission->read_cmd_from_storage(_mission_number_to_send,cmd)) {
|
||||
if (!_dataflash_backend->Write_Mission_Cmd(*_mission, cmd)) {
|
||||
if (!_logger_backend->Write_Mission_Cmd(*_mission, cmd)) {
|
||||
return; // call me again
|
||||
}
|
||||
}
|
||||
|
@ -12,13 +12,13 @@ public:
|
||||
virtual void process() = 0;
|
||||
virtual bool finished() { return _finished; }
|
||||
|
||||
virtual void set_dataflash_backend(class AP_Logger_Backend *backend) {
|
||||
_dataflash_backend = backend;
|
||||
virtual void set_logger_backend(class AP_Logger_Backend *backend) {
|
||||
_logger_backend = backend;
|
||||
}
|
||||
|
||||
protected:
|
||||
bool _finished = false;
|
||||
AP_Logger_Backend *_dataflash_backend = nullptr;
|
||||
AP_Logger_Backend *_logger_backend = nullptr;
|
||||
};
|
||||
|
||||
|
||||
@ -81,11 +81,11 @@ public:
|
||||
{
|
||||
}
|
||||
|
||||
virtual void set_dataflash_backend(class AP_Logger_Backend *backend) override {
|
||||
LoggerMessageWriter::set_dataflash_backend(backend);
|
||||
_writesysinfo.set_dataflash_backend(backend);
|
||||
_writeentiremission.set_dataflash_backend(backend);
|
||||
_writeallrallypoints.set_dataflash_backend(backend);
|
||||
virtual void set_logger_backend(class AP_Logger_Backend *backend) override {
|
||||
LoggerMessageWriter::set_logger_backend(backend);
|
||||
_writesysinfo.set_logger_backend(backend);
|
||||
_writeentiremission.set_logger_backend(backend);
|
||||
_writeallrallypoints.set_logger_backend(backend);
|
||||
}
|
||||
|
||||
void reset() override;
|
||||
|
@ -91,21 +91,21 @@ public:
|
||||
private:
|
||||
|
||||
AP_Int32 log_bitmask;
|
||||
AP_Logger dataflash{log_bitmask};
|
||||
AP_Logger logger{log_bitmask};
|
||||
void print_mode(AP_HAL::BetterStream *port, uint8_t mode);
|
||||
|
||||
void Log_Write_TypeMessages();
|
||||
void Log_Write_TypeMessages_Log_Write();
|
||||
|
||||
void flush_dataflash(AP_Logger &dataflash);
|
||||
void flush_logger(AP_Logger &logger);
|
||||
};
|
||||
|
||||
void AP_LoggerTest_AllTypes::flush_dataflash(AP_Logger &_dataflash)
|
||||
void AP_LoggerTest_AllTypes::flush_logger(AP_Logger &_logger)
|
||||
{
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX
|
||||
_dataflash.flush();
|
||||
_logger.flush();
|
||||
#else
|
||||
// flush is not available on e.g. px4 as it would be a somewhat
|
||||
// flush is not available on e.g. stm32 as it would be a somewhat
|
||||
// dangerous operation, but if we wait long enough (at time of
|
||||
// writing, 2 seconds, see AP_Logger_File::_io_timer) the data
|
||||
// will go out.
|
||||
@ -116,7 +116,7 @@ void AP_LoggerTest_AllTypes::flush_dataflash(AP_Logger &_dataflash)
|
||||
|
||||
void AP_LoggerTest_AllTypes::Log_Write_TypeMessages()
|
||||
{
|
||||
log_num = dataflash.find_last_log();
|
||||
log_num = logger.find_last_log();
|
||||
hal.console->printf("Using log number %u\n", log_num);
|
||||
|
||||
struct log_TYP1 typ1 = {
|
||||
@ -140,7 +140,7 @@ void AP_LoggerTest_AllTypes::Log_Write_TypeMessages()
|
||||
'A','B','C','D','E','F','G','H','I','J','K','L','M','N','O','P',
|
||||
'A','B','C','D','E','F','G','H','I','J','K','L','M','N','O','P' }
|
||||
};
|
||||
dataflash.WriteBlock(&typ1, sizeof(typ1));
|
||||
logger.WriteBlock(&typ1, sizeof(typ1));
|
||||
|
||||
struct log_TYP2 typ2 = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_TYP2_MSG),
|
||||
@ -154,19 +154,19 @@ void AP_LoggerTest_AllTypes::Log_Write_TypeMessages()
|
||||
q : -98239832498328, // int64_t
|
||||
Q : 3432345232233432 // uint64_t
|
||||
};
|
||||
dataflash.WriteBlock(&typ2, sizeof(typ2));
|
||||
logger.WriteBlock(&typ2, sizeof(typ2));
|
||||
|
||||
flush_dataflash(dataflash);
|
||||
flush_logger(logger);
|
||||
|
||||
dataflash.StopLogging();
|
||||
logger.StopLogging();
|
||||
}
|
||||
|
||||
void AP_LoggerTest_AllTypes::Log_Write_TypeMessages_Log_Write()
|
||||
{
|
||||
log_num = dataflash.find_last_log();
|
||||
log_num = logger.find_last_log();
|
||||
hal.console->printf("Using log number for Log_Write %u\n", log_num);
|
||||
|
||||
dataflash.Write("TYP3", TYP1_LBL, TYP1_FMT,
|
||||
logger.Write("TYP3", TYP1_LBL, TYP1_FMT,
|
||||
AP_HAL::micros64(),
|
||||
-17, // int8_t
|
||||
42, // uint8_t
|
||||
@ -183,7 +183,7 @@ void AP_LoggerTest_AllTypes::Log_Write_TypeMessages_Log_Write()
|
||||
"ABCDEFGHIJKLMNOPABCDEFGHIJKLMNOPABCDEFGHIJKLMNOPABCDEFGHIJKLMNOP"
|
||||
);
|
||||
|
||||
dataflash.Write("TYP4", TYP2_LBL, TYP2_FMT,
|
||||
logger.Write("TYP4", TYP2_LBL, TYP2_FMT,
|
||||
AP_HAL::micros64(),
|
||||
-9823, // int16_t * 100
|
||||
5436, // uint16_t * 100
|
||||
@ -196,21 +196,21 @@ void AP_LoggerTest_AllTypes::Log_Write_TypeMessages_Log_Write()
|
||||
);
|
||||
|
||||
// emit a message which contains NaNs:
|
||||
dataflash.Write("NANS", "f,d,bf,bd", "fdfd", dataflash.quiet_nanf(), dataflash.quiet_nan(), NAN, NAN);
|
||||
logger.Write("NANS", "f,d,bf,bd", "fdfd", logger.quiet_nanf(), logger.quiet_nan(), NAN, NAN);
|
||||
|
||||
flush_dataflash(dataflash);
|
||||
flush_logger(logger);
|
||||
|
||||
dataflash.StopLogging();
|
||||
logger.StopLogging();
|
||||
}
|
||||
|
||||
void AP_LoggerTest_AllTypes::setup(void)
|
||||
{
|
||||
hal.console->printf("Dataflash All Types 1.0\n");
|
||||
hal.console->printf("Logger All Types 1.0\n");
|
||||
|
||||
log_bitmask = (uint32_t)-1;
|
||||
dataflash.Init(log_structure, ARRAY_SIZE(log_structure));
|
||||
dataflash.set_vehicle_armed(true);
|
||||
dataflash.Write_Message("AP_Logger Test");
|
||||
logger.Init(log_structure, ARRAY_SIZE(log_structure));
|
||||
logger.set_vehicle_armed(true);
|
||||
logger.Write_Message("AP_Logger Test");
|
||||
|
||||
// Test
|
||||
hal.scheduler->delay(20);
|
||||
@ -233,6 +233,6 @@ const struct AP_Param::GroupInfo GCS_MAVLINK::var_info[] = {
|
||||
GCS_Dummy _gcs;
|
||||
|
||||
|
||||
static AP_LoggerTest_AllTypes dataflashtest;
|
||||
static AP_LoggerTest_AllTypes loggertest;
|
||||
|
||||
AP_HAL_MAIN_CALLBACKS(&dataflashtest);
|
||||
AP_HAL_MAIN_CALLBACKS(&loggertest);
|
||||
|
@ -40,27 +40,27 @@ public:
|
||||
private:
|
||||
|
||||
AP_Int32 log_bitmask;
|
||||
AP_Logger dataflash{log_bitmask};
|
||||
AP_Logger logger{log_bitmask};
|
||||
|
||||
};
|
||||
|
||||
static AP_LoggerTest dataflashtest;
|
||||
static AP_LoggerTest loggertest;
|
||||
|
||||
void AP_LoggerTest::setup(void)
|
||||
{
|
||||
hal.console->printf("Dataflash Log Test 1.0\n");
|
||||
hal.console->printf("Logger Log Test 1.0\n");
|
||||
|
||||
log_bitmask = (uint32_t)-1;
|
||||
dataflash.Init(log_structure, ARRAY_SIZE(log_structure));
|
||||
dataflash.set_vehicle_armed(true);
|
||||
dataflash.Write_Message("AP_Logger Test");
|
||||
logger.Init(log_structure, ARRAY_SIZE(log_structure));
|
||||
logger.set_vehicle_armed(true);
|
||||
logger.Write_Message("AP_Logger Test");
|
||||
|
||||
// Test
|
||||
hal.scheduler->delay(20);
|
||||
|
||||
// We start to write some info (sequentialy) starting from page 1
|
||||
// This is similar to what we will do...
|
||||
log_num = dataflash.find_last_log();
|
||||
log_num = logger.find_last_log();
|
||||
hal.console->printf("Using log number %u\n", log_num);
|
||||
hal.console->printf("Writing to flash... wait...\n");
|
||||
|
||||
@ -80,7 +80,7 @@ void AP_LoggerTest::setup(void)
|
||||
l1 : (int32_t)(i * 5000),
|
||||
l2 : (int32_t)(i * 16268)
|
||||
};
|
||||
dataflash.WriteBlock(&pkt, sizeof(pkt));
|
||||
logger.WriteBlock(&pkt, sizeof(pkt));
|
||||
total_micros += AP_HAL::micros() - start;
|
||||
hal.scheduler->delay(20);
|
||||
}
|
||||
@ -89,7 +89,7 @@ void AP_LoggerTest::setup(void)
|
||||
(double)total_micros/((double)i*sizeof(struct log_Test)));
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX
|
||||
dataflash.flush();
|
||||
logger.flush();
|
||||
#endif
|
||||
|
||||
hal.scheduler->delay(100);
|
||||
@ -109,12 +109,12 @@ void loop(void);
|
||||
|
||||
void setup()
|
||||
{
|
||||
dataflashtest.setup();
|
||||
loggertest.setup();
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
dataflashtest.loop();
|
||||
loggertest.loop();
|
||||
}
|
||||
|
||||
const struct AP_Param::GroupInfo GCS_MAVLINK::var_info[] = {
|
||||
|
Loading…
Reference in New Issue
Block a user