From 09aff03edc83c2585a68e5b538b5b6a2fd79c81f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 6 Nov 2020 10:28:26 +1100 Subject: [PATCH] AP_Logger: support new replay system added allow_start_ekf and block write method for replay Co-authored-by: Peter Barker --- libraries/AP_Logger/AP_Logger.cpp | 36 ++++++++++++ libraries/AP_Logger/AP_Logger.h | 13 +++-- libraries/AP_Logger/AP_Logger_Backend.cpp | 24 ++++++++ libraries/AP_Logger/AP_Logger_Backend.h | 4 ++ libraries/AP_Logger/AP_Logger_File.cpp | 7 +++ libraries/AP_Logger/LogFile.cpp | 50 ----------------- libraries/AP_Logger/LoggerMessageWriter.cpp | 61 +++++++++++---------- libraries/AP_Logger/LoggerMessageWriter.h | 6 ++ 8 files changed, 118 insertions(+), 83 deletions(-) diff --git a/libraries/AP_Logger/AP_Logger.cpp b/libraries/AP_Logger/AP_Logger.cpp index 9fa2eb90aa..e6c1a30a6f 100644 --- a/libraries/AP_Logger/AP_Logger.cpp +++ b/libraries/AP_Logger/AP_Logger.cpp @@ -651,6 +651,25 @@ void AP_Logger::WriteBlock(const void *pBuffer, uint16_t size) { FOR_EACH_BACKEND(WriteBlock(pBuffer, size)); } +// write a replay block. This differs from other as it returns false if a backend doesn't +// have space for the msg +bool AP_Logger::WriteReplayBlock(uint8_t msg_id, const void *pBuffer, uint16_t size) { + bool ret = true; + if (log_replay()) { + uint8_t buf[3+size]; + buf[0] = HEAD_BYTE1; + buf[1] = HEAD_BYTE2; + buf[2] = msg_id; + memcpy(&buf[3], pBuffer, size); + for (uint8_t i=0; i<_next_backend; i++) { + if (!backends[i]->WritePrioritisedBlock(buf, sizeof(buf), true)) { + ret = false; + } + } + } + return ret; +} + void AP_Logger::WriteCriticalBlock(const void *pBuffer, uint16_t size) { FOR_EACH_BACKEND(WriteCriticalBlock(pBuffer, size)); } @@ -855,6 +874,8 @@ void AP_Logger::WriteCritical(const char *name, const char *labels, const char * void AP_Logger::WriteV(const char *name, const char *labels, const char *units, const char *mults, const char *fmt, va_list arg_list, bool is_critical) { + // WriteV is not safe in replay as we can re-use IDs +#if !APM_BUILD_TYPE(APM_BUILD_Replay) struct log_write_fmt *f = msg_fmt_for_name(name, labels, units, mults, fmt); if (f == nullptr) { // unable to map name to a messagetype; could be out of @@ -875,8 +896,23 @@ void AP_Logger::WriteV(const char *name, const char *labels, const char *units, backends[i]->Write(f->msg_type, arg_copy, is_critical); va_end(arg_copy); } +#endif } +bool AP_Logger::allow_start_ekf() const +{ + if (!AP::logger().log_replay()) { + return true; + } + + for (uint8_t i=0; i<_next_backend; i++) { + if (!backends[i]->allow_start_ekf()) { + return false; + } + } + + return true; +} #if CONFIG_HAL_BOARD == HAL_BOARD_SITL void AP_Logger::assert_same_fmt_for_name(const AP_Logger::log_write_fmt *f, diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index adf3f3276d..9fcd276e04 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -208,6 +208,9 @@ public: /* Write an *important* block of data at current offset */ void WriteCriticalBlock(const void *pBuffer, uint16_t size); + /* Write a block of replay data at current offset */ + bool WriteReplayBlock(uint8_t msg_id, const void *pBuffer, uint16_t size); + // high level interface uint16_t find_last_log() const; void get_log_boundaries(uint16_t log_num, uint32_t & start_page, uint32_t & end_page); @@ -228,7 +231,6 @@ public: LogErrorCode error_code); void Write_GPS(uint8_t instance, uint64_t time_us=0); void Write_IMU(); - void Write_IMUDT(uint64_t time_us, uint8_t imu_mask); bool Write_ISBH(uint16_t seqno, AP_InertialSensor::IMU_SENSOR_TYPE sensor_type, uint8_t instance, @@ -325,6 +327,10 @@ public: void periodic_tasks(); // may want to split this into GCS/non-GCS duties + // We may need to make sure data is loggable before starting the + // EKF; when allow_start_ekf we should be able to log that data + bool allow_start_ekf() const; + // number of blocks that have been dropped uint32_t num_dropped(void) const; @@ -332,7 +338,7 @@ public: void set_force_log_disarmed(bool force_logging) { _force_log_disarmed = force_logging; } bool log_while_disarmed(void) const; uint8_t log_replay(void) const { return _params.log_replay; } - + vehicle_startup_message_Writer _vehicle_messages; // parameter support @@ -460,9 +466,6 @@ private: uint8_t mag_instance, enum LogMessages type); void Write_Current_instance(uint64_t time_us, uint8_t battery_instance); - void Write_IMUDT_instance(uint64_t time_us, - uint8_t imu_instance, - enum LogMessages type); void backend_starting_new_log(const AP_Logger_Backend *backend); diff --git a/libraries/AP_Logger/AP_Logger_Backend.cpp b/libraries/AP_Logger/AP_Logger_Backend.cpp index 0adbb1851b..6b123bf017 100644 --- a/libraries/AP_Logger/AP_Logger_Backend.cpp +++ b/libraries/AP_Logger/AP_Logger_Backend.cpp @@ -89,6 +89,21 @@ void AP_Logger_Backend::start_new_log_reset_variables() _log_file_size_bytes = 0; } +// We may need to make sure data is loggable before starting the +// EKF; when allow_start_ekf we should be able to log that data +bool AP_Logger_Backend::allow_start_ekf() const +{ + if (!_startup_messagewriter->fmt_done()) { + return false; + } + // we need to push all startup messages out, or the code in + // WriteBlockCheckStartupMessages bites us. + if (!_startup_messagewriter->finished()) { + return false; + } + return true; +} + // this method can be overridden to do extra things with your buffer. // for example, in AP_Logger_MAVLink we may push messages into the UART. void AP_Logger_Backend::push_log_blocks() { @@ -102,6 +117,7 @@ bool AP_Logger_Backend::WriteBlockCheckStartupMessages() #if APM_BUILD_TYPE(APM_BUILD_Replay) return true; #endif + if (_startup_messagewriter->fmt_done()) { return true; } @@ -133,6 +149,9 @@ bool AP_Logger_Backend::WriteBlockCheckStartupMessages() // source more messages from the startup message writer: void AP_Logger_Backend::WriteMoreStartupMessages() { +#if APM_BUILD_TYPE(APM_BUILD_Replay) + return; +#endif if (_startup_messagewriter->finished()) { return; @@ -150,6 +169,11 @@ void AP_Logger_Backend::WriteMoreStartupMessages() bool AP_Logger_Backend::Write_Emit_FMT(uint8_t msg_type) { +#if APM_BUILD_TYPE(APM_BUILD_Replay) + // sure, sure we did.... + return true; +#endif + // get log structure from front end: char ls_name[LS_NAME_SIZE] = {}; char ls_format[LS_FORMAT_SIZE] = {}; diff --git a/libraries/AP_Logger/AP_Logger_Backend.h b/libraries/AP_Logger/AP_Logger_Backend.h index e9ae074ea8..33b937511f 100644 --- a/libraries/AP_Logger/AP_Logger_Backend.h +++ b/libraries/AP_Logger/AP_Logger_Backend.h @@ -122,6 +122,10 @@ public: virtual bool logging_enabled() const; virtual bool logging_failed() const = 0; + // We may need to make sure data is loggable before starting the + // EKF; when allow_start_ekf we should be able to log that data + bool allow_start_ekf() const; + virtual void vehicle_was_disarmed(); bool Write_Unit(const struct UnitStructure *s); diff --git a/libraries/AP_Logger/AP_Logger_File.cpp b/libraries/AP_Logger/AP_Logger_File.cpp index 92a3594d32..7c6cc790e8 100644 --- a/libraries/AP_Logger/AP_Logger_File.cpp +++ b/libraries/AP_Logger/AP_Logger_File.cpp @@ -495,6 +495,13 @@ bool AP_Logger_File::_WritePrioritisedBlock(const void *pBuffer, uint16_t size, return false; } +#if APM_BUILD_TYPE(APM_BUILD_Replay) + if (AP::FS().write(_write_fd, pBuffer, size) != size) { + AP_HAL::panic("Short write"); + } + return true; +#endif + if (!semaphore.take(1)) { return false; } diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index f9bd609b56..61a2ed1ac0 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -348,56 +348,6 @@ void AP_Logger::Write_IMU() Write_IMU_instance(time_us, 2, LOG_IMU3_MSG); } -// Write an accel/gyro delta time data packet -void AP_Logger::Write_IMUDT_instance(const uint64_t time_us, const uint8_t imu_instance, const enum LogMessages type) -{ - const AP_InertialSensor &ins = AP::ins(); - float delta_t = ins.get_delta_time(); - float delta_vel_t = ins.get_delta_velocity_dt(imu_instance); - float delta_ang_t = ins.get_delta_angle_dt(imu_instance); - Vector3f delta_angle, delta_velocity; - ins.get_delta_angle(imu_instance, delta_angle); - ins.get_delta_velocity(imu_instance, delta_velocity); - - const struct log_IMUDT pkt{ - LOG_PACKET_HEADER_INIT(type), - time_us : time_us, - delta_time : delta_t, - delta_vel_dt : delta_vel_t, - delta_ang_dt : delta_ang_t, - delta_ang_x : delta_angle.x, - delta_ang_y : delta_angle.y, - delta_ang_z : delta_angle.z, - delta_vel_x : delta_velocity.x, - delta_vel_y : delta_velocity.y, - delta_vel_z : delta_velocity.z - }; - WriteBlock(&pkt, sizeof(pkt)); -} - -void AP_Logger::Write_IMUDT(uint64_t time_us, uint8_t imu_mask) -{ - const AP_InertialSensor &ins = AP::ins(); - if (imu_mask & 1) { - Write_IMUDT_instance(time_us, 0, LOG_IMUDT_MSG); - } - if ((ins.get_gyro_count() < 2 && ins.get_accel_count() < 2) || !ins.use_gyro(1)) { - return; - } - - if (imu_mask & 2) { - Write_IMUDT_instance(time_us, 1, LOG_IMUDT2_MSG); - } - - if ((ins.get_gyro_count() < 3 && ins.get_accel_count() < 3) || !ins.use_gyro(2)) { - return; - } - - if (imu_mask & 4) { - Write_IMUDT_instance(time_us, 2, LOG_IMUDT3_MSG); - } -} - void AP_Logger::Write_Vibration() { const AP_InertialSensor &ins = AP::ins(); diff --git a/libraries/AP_Logger/LoggerMessageWriter.cpp b/libraries/AP_Logger/LoggerMessageWriter.cpp index 929a652705..d15527b674 100644 --- a/libraries/AP_Logger/LoggerMessageWriter.cpp +++ b/libraries/AP_Logger/LoggerMessageWriter.cpp @@ -22,11 +22,17 @@ void LoggerMessageWriter::reset() _finished = false; } +bool LoggerMessageWriter::out_of_time_for_writing_messages() const +{ + return AP::scheduler().time_available_usec() < MIN_LOOP_TIME_REMAINING_FOR_MESSAGE_WRITE_US; +} + void LoggerMessageWriter_DFLogStart::reset() { LoggerMessageWriter::reset(); _fmt_done = false; + _params_done = false; _writesysinfo.reset(); _writeentiremission.reset(); _writeallrallypoints.reset(); @@ -39,29 +45,48 @@ void LoggerMessageWriter_DFLogStart::reset() ap = AP_Param::first(&token, &type); } +bool LoggerMessageWriter_DFLogStart::out_of_time_for_writing_messages() const +{ + if (stage == Stage::FORMATS) { + // write out the FMT messages as fast as we can + return AP::scheduler().time_available_usec() == 0; + } + return LoggerMessageWriter::out_of_time_for_writing_messages(); +} + void LoggerMessageWriter_DFLogStart::process() { + if (out_of_time_for_writing_messages()) { + return; + } + switch(stage) { case Stage::FORMATS: // write log formats so the log is self-describing while (next_format_to_send < _logger_backend->num_types()) { - if (AP::scheduler().time_available_usec() == 0) { // write out the FMT messages as fast as we can - return; - } if (!_logger_backend->Write_Format(_logger_backend->structure(next_format_to_send))) { return; // call me again! } next_format_to_send++; } _fmt_done = true; + stage = Stage::PARMS; + FALLTHROUGH; + + case Stage::PARMS: + while (ap) { + if (!_logger_backend->Write_Parameter(ap, token, type)) { + return; + } + ap = AP_Param::next_scalar(&token, &type); + } + + _params_done = true; stage = Stage::UNITS; FALLTHROUGH; case Stage::UNITS: while (_next_unit_to_send < _logger_backend->num_units()) { - if (AP::scheduler().time_available_usec() < MIN_LOOP_TIME_REMAINING_FOR_MESSAGE_WRITE_US) { - return; - } if (!_logger_backend->Write_Unit(_logger_backend->unit(_next_unit_to_send))) { return; // call me again! } @@ -72,9 +97,6 @@ void LoggerMessageWriter_DFLogStart::process() case Stage::MULTIPLIERS: while (_next_multiplier_to_send < _logger_backend->num_multipliers()) { - if (AP::scheduler().time_available_usec() < MIN_LOOP_TIME_REMAINING_FOR_MESSAGE_WRITE_US) { - return; - } if (!_logger_backend->Write_Multiplier(_logger_backend->multiplier(_next_multiplier_to_send))) { return; // call me again! } @@ -85,28 +107,11 @@ void LoggerMessageWriter_DFLogStart::process() case Stage::FORMAT_UNITS: while (_next_format_unit_to_send < _logger_backend->num_types()) { - if (AP::scheduler().time_available_usec() < MIN_LOOP_TIME_REMAINING_FOR_MESSAGE_WRITE_US) { - return; - } if (!_logger_backend->Write_Format_Units(_logger_backend->structure(_next_format_unit_to_send))) { return; // call me again! } _next_format_unit_to_send++; } - stage = Stage::PARMS; - FALLTHROUGH; - - case Stage::PARMS: - while (ap) { - if (AP::scheduler().time_available_usec() < MIN_LOOP_TIME_REMAINING_FOR_MESSAGE_WRITE_US) { - return; - } - if (!_logger_backend->Write_Parameter(ap, token, type)) { - return; - } - ap = AP_Param::next_scalar(&token, &type); - } - stage = Stage::RUNNING_SUBWRITERS; FALLTHROUGH; @@ -260,7 +265,7 @@ void LoggerMessageWriter_WriteAllRallyPoints::process() case Stage::WRITE_ALL_RALLY_POINTS: while (_rally_number_to_send < _rally->get_rally_total()) { - if (AP::scheduler().time_available_usec() < MIN_LOOP_TIME_REMAINING_FOR_MESSAGE_WRITE_US) { + if (out_of_time_for_writing_messages()) { return; } RallyLocation rallypoint; @@ -310,7 +315,7 @@ void LoggerMessageWriter_WriteEntireMission::process() { case Stage::WRITE_MISSION_ITEMS: { AP_Mission::Mission_Command cmd; while (_mission_number_to_send < _mission->num_commands()) { - if (AP::scheduler().time_available_usec() < MIN_LOOP_TIME_REMAINING_FOR_MESSAGE_WRITE_US) { + if (out_of_time_for_writing_messages()) { return; } // upon failure to write the mission we will re-read from diff --git a/libraries/AP_Logger/LoggerMessageWriter.h b/libraries/AP_Logger/LoggerMessageWriter.h index d56735e583..6bf967d4fa 100644 --- a/libraries/AP_Logger/LoggerMessageWriter.h +++ b/libraries/AP_Logger/LoggerMessageWriter.h @@ -13,6 +13,8 @@ public: _logger_backend = backend; } + bool out_of_time_for_writing_messages() const; + protected: bool _finished = false; AP_Logger_Backend *_logger_backend = nullptr; @@ -86,9 +88,12 @@ public: _writeallrallypoints.set_logger_backend(backend); } + bool out_of_time_for_writing_messages() const; + void reset() override; void process() override; bool fmt_done() { return _fmt_done; } + bool params_done() { return _params_done; } // reset some writers so we push stuff out to logs again. Will // only work if we are in state DONE! @@ -109,6 +114,7 @@ private: }; bool _fmt_done; + bool _params_done; Stage stage;