diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp index 2877d57f19..7853af3245 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.cpp +++ b/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); diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h index fd1a0abf88..f8de864f2b 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h +++ b/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) diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp index 4baf8273c6..d75926441c 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp +++ b/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 diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_Logging.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_Logging.cpp new file mode 100644 index 0000000000..19351d3591 --- /dev/null +++ b/libraries/AP_VisualOdom/AP_VisualOdom_Logging.cpp @@ -0,0 +1,65 @@ +#include "AP_VisualOdom_Backend.h" + +#if HAL_VISUALODOM_ENABLED + +#include + +// 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 diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp index 23f813bd59..fe529dfc0e 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp +++ b/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 diff --git a/libraries/AP_VisualOdom/LogStructure.h b/libraries/AP_VisualOdom/LogStructure.h new file mode 100644 index 0000000000..51b92ccf4f --- /dev/null +++ b/libraries/AP_VisualOdom/LogStructure.h @@ -0,0 +1,97 @@ +#pragma once + +#include + +#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--" }, +