diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index 7f06a9b147..414d770cbe 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/libraries/AP_Logger/AP_Logger.h @@ -327,9 +327,6 @@ public: void Write_RallyPoint(uint8_t total, uint8_t sequence, const RallyLocation &rally_point); - 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, float vel_err, uint8_t reset_counter, bool ignored); void Write_Beacon(AP_Beacon &beacon); void Write_Proximity(AP_Proximity &proximity); void Write_SRTL(bool active, uint16_t num_points, uint16_t max_points, uint8_t action, const Vector3f& point); diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index c62101c1c9..7b3fad5388 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -572,64 +572,6 @@ void AP_Logger::Write_RPM(const AP_RPM &rpm_sensor) WriteBlock(&pkt, sizeof(pkt)); } -// Write visual odometry sensor data -void AP_Logger::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 - }; - WriteBlock(&pkt_visualodom, sizeof(log_VisualOdom)); -} - -// Write visual position sensor data. x,y,z are in meters, angles are in degrees -void AP_Logger::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 - }; - WriteBlock(&pkt_visualpos, sizeof(log_VisualPosition)); -} - -// Write visual velocity sensor data, velocity in NED meters per second -void AP_Logger::Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, float vel_err, 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 : vel_err, - reset_counter : reset_counter, - ignored : (uint8_t)ignored - }; - WriteBlock(&pkt_visualvel, sizeof(log_VisualVelocity)); -} - // Write beacon sensor (position) data void AP_Logger::Write_Beacon(AP_Beacon &beacon) { diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 7b0f67d090..bdb9462b64 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -124,6 +124,7 @@ const struct MultiplierStructure log_Multipliers[] = { #include #include #include +#include // structure used to define logging format struct LogStructure { @@ -374,51 +375,6 @@ struct PACKED log_POWR { uint8_t safety_and_arm; }; -// visual odometry sensor data -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; -}; - -// visual position data -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; -}; - -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; -}; - struct PACKED log_Cmd { LOG_PACKET_HEADER; uint64_t time_us; @@ -1576,46 +1532,6 @@ struct PACKED log_PSC { // @Field: VibeZ: Primary accelerometer filtered vibration, z-axis // @Field: Clip: Number of clipping events on 1st accelerometer -// @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 - -// @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 - -// @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 - // @LoggerMessage: WENC // @Description: Wheel encoder measurements // @Field: TimeUS: Time since system startup @@ -1771,12 +1687,7 @@ LOG_STRUCTURE_FROM_AHRS \ "RALY", "QBBLLh", "TimeUS,Tot,Seq,Lat,Lng,Alt", "s--DUm", "F--GGB" }, \ { LOG_MAV_MSG, sizeof(log_MAV), \ "MAV", "QBHHHBHH", "TimeUS,chan,txp,rxp,rxdp,flags,ss,tf", "s#----s-", "F-000-C-" }, \ - { 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--" }, \ +LOG_STRUCTURE_FROM_VISUALODOM \ { LOG_OPTFLOW_MSG, sizeof(log_Optflow), \ "OF", "QBffff", "TimeUS,Qual,flowX,flowY,bodyX,bodyY", "s-EEnn", "F-0000" }, \ { LOG_WHEELENCODER_MSG, sizeof(log_WheelEncoder), \ @@ -1894,8 +1805,7 @@ enum LogMessages : uint8_t { LOG_MSG_SBPEVENT, LOG_RALLY_MSG, - LOG_VISUALODOM_MSG, - LOG_VISUALPOS_MSG, + LOG_IDS_FROM_VISUALODOM, LOG_BEACON_MSG, LOG_PROXIMITY_MSG, LOG_DF_FILE_STATS, @@ -1912,7 +1822,6 @@ enum LogMessages : uint8_t { LOG_ARM_DISARM_MSG, LOG_OA_BENDYRULER_MSG, LOG_OA_DIJKSTRA_MSG, - LOG_VISUALVEL_MSG, LOG_SIMPLE_AVOID_MSG, LOG_WINCH_MSG, LOG_PSC_MSG,