AP_Logger: support new replay system

added allow_start_ekf and block write method for replay

Co-authored-by: Peter Barker <pbarker@barker.dropbear.id.au>

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,

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);
 

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] = {};

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);

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;
     }

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();

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

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;