AP_VisualOdom: Privatize Logging

libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp

@@ -66,7 +66,7 @@ void AP_VisualOdom_Backend::handle_vision_position_delta_msg(const mavlink_messa
                                          _frontend.get_pos_offset());
 
     // log sensor data
-    AP::logger().Write_VisualOdom(time_delta_sec,
+    Write_VisualOdom(time_delta_sec,
                                   angle_delta,
                                   position_delta,
                                   packet.confidence);

libraries/AP_VisualOdom/AP_VisualOdom_Backend.h

@@ -51,6 +51,11 @@ protected:
     // updates the reset timestamp to the current system time if the reset_counter has changed
     uint32_t get_reset_timestamp_ms(uint8_t reset_counter);
 
+    // Logging Functions
+    void Write_VisualOdom(float time_delta, const Vector3f &angle_delta, const Vector3f &position_delta, float confidence);
+    void Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float pos_err, float ang_err, uint8_t reset_counter, bool ignored);
+    void Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, bool ignored);
+
     AP_VisualOdom &_frontend;   // reference to frontend
     uint32_t _last_update_ms;   // system time of last update from sensor (used by health checks)
 

libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp

@@ -65,7 +65,7 @@ void AP_VisualOdom_IntelT265::handle_vision_position_estimate(uint64_t remote_ti
     att.to_euler(roll, pitch, yaw);
 
     // log sensor data
-    AP::logger().Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), wrap_360(degrees(yaw)), posErr, angErr, reset_counter, !consume);
+    Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), wrap_360(degrees(yaw)), posErr, angErr, reset_counter, !consume);
 
     // store corrected attitude for use in pre-arm checks
     _attitude_last = att;
@@ -91,7 +91,7 @@ void AP_VisualOdom_IntelT265::handle_vision_speed_estimate(uint64_t remote_time_
     // record time for health monitoring
     _last_update_ms = AP_HAL::millis();
 
-    AP::logger().Write_VisualVelocity(remote_time_us, time_ms, vel_corrected, _frontend.get_vel_noise(), reset_counter, !consume);
+    Write_VisualVelocity(remote_time_us, time_ms, vel_corrected, reset_counter, !consume);
 }
 
 // apply rotation and correction to position

libraries/AP_VisualOdom/AP_VisualOdom_Logging.cpp

@@ -0,0 +1,65 @@
+#include "AP_VisualOdom_Backend.h"
+
+#if HAL_VISUALODOM_ENABLED
+
+#include <AP_Logger/AP_Logger.h>
+
+// Write visual odometry sensor data 
+void AP_VisualOdom_Backend::Write_VisualOdom(float time_delta, const Vector3f &angle_delta, const Vector3f &position_delta, float confidence)
+{
+    const struct log_VisualOdom pkt_visualodom{
+        LOG_PACKET_HEADER_INIT(LOG_VISUALODOM_MSG),
+        time_us             : AP_HAL::micros64(),
+        time_delta          : time_delta,
+        angle_delta_x       : angle_delta.x,
+        angle_delta_y       : angle_delta.y,
+        angle_delta_z       : angle_delta.z,
+        position_delta_x    : position_delta.x,
+        position_delta_y    : position_delta.y,
+        position_delta_z    : position_delta.z,
+        confidence          : confidence
+    };
+    AP::logger().WriteBlock(&pkt_visualodom, sizeof(log_VisualOdom));
+}
+
+// Write visual position sensor data.  x,y,z are in meters, angles are in degrees
+void AP_VisualOdom_Backend::Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float pos_err, float ang_err, uint8_t reset_counter, bool ignored)
+{
+    const struct log_VisualPosition pkt_visualpos {
+        LOG_PACKET_HEADER_INIT(LOG_VISUALPOS_MSG),
+        time_us         : AP_HAL::micros64(),
+        remote_time_us  : remote_time_us,
+        time_ms         : time_ms,
+        pos_x           : x,
+        pos_y           : y,
+        pos_z           : z,
+        roll            : roll,
+        pitch           : pitch,
+        yaw             : yaw,
+        pos_err         : pos_err,
+        ang_err         : ang_err,
+        reset_counter   : reset_counter,
+        ignored         : (uint8_t)ignored
+    };
+    AP::logger().WriteBlock(&pkt_visualpos, sizeof(log_VisualPosition));
+}
+
+// Write visual velocity sensor data, velocity in NED meters per second
+void AP_VisualOdom_Backend::Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, bool ignored)
+{
+    const struct log_VisualVelocity pkt_visualvel {
+        LOG_PACKET_HEADER_INIT(LOG_VISUALVEL_MSG),
+        time_us         : AP_HAL::micros64(),
+        remote_time_us  : remote_time_us,
+        time_ms         : time_ms,
+        vel_x           : vel.x,
+        vel_y           : vel.y,
+        vel_z           : vel.z,
+        vel_err         : _frontend.get_vel_noise(),
+        reset_counter   : reset_counter,
+        ignored         : (uint8_t)ignored
+    };
+    AP::logger().WriteBlock(&pkt_visualvel, sizeof(log_VisualVelocity));
+}
+
+#endif // HAL_VISUALODOM_ENABLED

libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp

@@ -47,7 +47,7 @@ void AP_VisualOdom_MAV::handle_vision_position_estimate(uint64_t remote_time_us,
     attitude.to_euler(roll, pitch, yaw);
 
     // log sensor data
-    AP::logger().Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), degrees(yaw), posErr, angErr, reset_counter, false);
+    Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), degrees(yaw), posErr, angErr, reset_counter, false);
 
     // record time for health monitoring
     _last_update_ms = AP_HAL::millis();
@@ -61,7 +61,7 @@ void AP_VisualOdom_MAV::handle_vision_speed_estimate(uint64_t remote_time_us, ui
     // record time for health monitoring
     _last_update_ms = AP_HAL::millis();
 
-    AP::logger().Write_VisualVelocity(remote_time_us, time_ms, vel, _frontend.get_vel_noise(), reset_counter, false);
+    Write_VisualVelocity(remote_time_us, time_ms, vel, reset_counter, false);
 }
 
 #endif

libraries/AP_VisualOdom/LogStructure.h

@@ -0,0 +1,97 @@
+#pragma once
+
+#include <AP_Logger/LogStructure.h>
+
+#define LOG_IDS_FROM_VISUALODOM \
+    LOG_VISUALODOM_MSG, \
+    LOG_VISUALPOS_MSG, \
+    LOG_VISUALVEL_MSG
+
+// @LoggerMessage: VISO
+// @Description: Visual Odometry
+// @Field: TimeUS: System time
+// @Field: dt: Time period this data covers
+// @Field: AngDX: Angular change for body-frame roll axis
+// @Field: AngDY: Angular change for body-frame pitch axis
+// @Field: AngDZ: Angular change for body-frame z axis
+// @Field: PosDX: Position change for body-frame X axis (Forward-Back)
+// @Field: PosDY: Position change for body-frame Y axis (Right-Left)
+// @Field: PosDZ: Position change for body-frame Z axis (Down-Up)
+// @Field: conf: Confidence
+struct PACKED log_VisualOdom {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    float time_delta;
+    float angle_delta_x;
+    float angle_delta_y;
+    float angle_delta_z;
+    float position_delta_x;
+    float position_delta_y;
+    float position_delta_z;
+    float confidence;
+};
+
+// @LoggerMessage: VISP
+// @Description: Vision Position
+// @Field: TimeUS: System time
+// @Field: RTimeUS: Remote system time
+// @Field: CTimeMS: Corrected system time
+// @Field: PX: Position X-axis (North-South)
+// @Field: PY: Position Y-axis (East-West)
+// @Field: PZ: Position Z-axis (Down-Up)
+// @Field: Roll: Roll lean angle
+// @Field: Pitch: Pitch lean angle
+// @Field: Yaw: Yaw angle
+// @Field: PErr: Position estimate error
+// @Field: AErr: Attitude estimate error
+// @Field: Rst: Position reset counter
+// @Field: Ign: Ignored
+struct PACKED log_VisualPosition {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint64_t remote_time_us;
+    uint32_t time_ms;
+    float pos_x;
+    float pos_y;
+    float pos_z;
+    float roll;     // degrees
+    float pitch;    // degrees
+    float yaw;      // degrees
+    float pos_err;  // meters
+    float ang_err;  // radians
+    uint8_t reset_counter;
+    uint8_t ignored;
+};
+
+// @LoggerMessage: VISV
+// @Description: Vision Velocity
+// @Field: TimeUS: System time
+// @Field: RTimeUS: Remote system time
+// @Field: CTimeMS: Corrected system time
+// @Field: VX: Velocity X-axis (North-South)
+// @Field: VY: Velocity Y-axis (East-West)
+// @Field: VZ: Velocity Z-axis (Down-Up)
+// @Field: VErr: Velocity estimate error
+// @Field: Rst: Velocity reset counter
+// @Field: Ign: Ignored
+struct PACKED log_VisualVelocity {
+    LOG_PACKET_HEADER;
+    uint64_t time_us;
+    uint64_t remote_time_us;
+    uint32_t time_ms;
+    float vel_x;
+    float vel_y;
+    float vel_z;
+    float vel_err;
+    uint8_t reset_counter;
+    uint8_t ignored;
+};
+
+#define LOG_STRUCTURE_FROM_VISUALODOM \
+    { LOG_VISUALODOM_MSG, sizeof(log_VisualOdom), \
+      "VISO", "Qffffffff", "TimeUS,dt,AngDX,AngDY,AngDZ,PosDX,PosDY,PosDZ,conf", "ssrrrmmm-", "FF000000-" }, \
+    { LOG_VISUALPOS_MSG, sizeof(log_VisualPosition), \
+      "VISP", "QQIffffffffBB", "TimeUS,RTimeUS,CTimeMS,PX,PY,PZ,Roll,Pitch,Yaw,PErr,AErr,Rst,Ign", "sssmmmddhmd--", "FFC00000000--" }, \
+    { LOG_VISUALVEL_MSG, sizeof(log_VisualVelocity), \
+      "VISV", "QQIffffBB", "TimeUS,RTimeUS,CTimeMS,VX,VY,VZ,VErr,Rst,Ign", "sssnnnn--", "FFC0000--" },
+