AP_Logger: Privatize AC_Avoidance Logging

libraries/AP_Logger/AP_Logger.h

@@ -318,9 +318,6 @@ public:
     void Write_Proximity(AP_Proximity &proximity);
 #endif
     void Write_SRTL(bool active, uint16_t num_points, uint16_t max_points, uint8_t action, const Vector3f& point);
-    void Write_OABendyRuler(uint8_t type, bool active, float target_yaw, float target_pitch, bool ignore_chg, float margin, const Location &final_dest, const Location &oa_dest);
-    void Write_OADijkstra(uint8_t state, uint8_t error_id, uint8_t curr_point, uint8_t tot_points, const Location &final_dest, const Location &oa_dest);
-    void Write_SimpleAvoidance(uint8_t state, const Vector2f& desired_vel, const Vector2f& modified_vel, bool back_up);
     void Write_Winch(bool healthy, bool thread_end, bool moving, bool clutch, uint8_t mode, float desired_length, float length, float desired_rate, uint16_t tension, float voltage, int8_t temp);
     void Write_PSC(const Vector3f &pos_target, const Vector3f &position, const Vector3f &vel_target, const Vector3f &velocity, const Vector3f &accel_target, const float &accel_x, const float &accel_y);
     void Write_PSCZ(float pos_target_z, float pos_z, float vel_desired_z, float vel_target_z, float vel_z, float accel_desired_z, float accel_target_z, float accel_z, float throttle_out);

libraries/AP_Logger/LogFile.cpp

@@ -566,64 +566,6 @@ void AP_Logger::Write_SRTL(bool active, uint16_t num_points, uint16_t max_points
     WriteBlock(&pkt_srtl, sizeof(pkt_srtl));
 }
 
-void AP_Logger::Write_OABendyRuler(uint8_t type, bool active, float target_yaw, float target_pitch, bool resist_chg, float margin, const Location &final_dest, const Location &oa_dest)
-{
-    int32_t oa_dest_alt, final_alt;
-    bool got_oa_dest = oa_dest.get_alt_cm(Location::AltFrame::ABOVE_ORIGIN, oa_dest_alt);
-    bool got_final_dest = final_dest.get_alt_cm(Location::AltFrame::ABOVE_ORIGIN, final_alt);
-    
-    const struct log_OABendyRuler pkt{
-        LOG_PACKET_HEADER_INIT(LOG_OA_BENDYRULER_MSG),
-        time_us     : AP_HAL::micros64(),
-        type        : type,
-        active      : active,
-        target_yaw  : (uint16_t)wrap_360(target_yaw),
-        yaw         : (uint16_t)wrap_360(AP::ahrs().yaw_sensor * 0.01f),
-        target_pitch: (uint16_t)target_pitch,
-        resist_chg  : resist_chg,
-        margin      : margin,
-        final_lat   : final_dest.lat,
-        final_lng   : final_dest.lng,
-        final_alt   : got_final_dest ? final_alt : final_dest.alt,
-        oa_lat      : oa_dest.lat,
-        oa_lng      : oa_dest.lng,
-        oa_alt      : got_oa_dest ? oa_dest_alt : oa_dest.alt
-    };
-    WriteBlock(&pkt, sizeof(pkt));
-}
-
-void AP_Logger::Write_OADijkstra(uint8_t state, uint8_t error_id, uint8_t curr_point, uint8_t tot_points, const Location &final_dest, const Location &oa_dest)
-{
-    struct log_OADijkstra pkt{
-        LOG_PACKET_HEADER_INIT(LOG_OA_DIJKSTRA_MSG),
-        time_us     : AP_HAL::micros64(),
-        state       : state,
-        error_id    : error_id,
-        curr_point  : curr_point,
-        tot_points  : tot_points,
-        final_lat   : final_dest.lat,
-        final_lng   : final_dest.lng,
-        oa_lat      : oa_dest.lat,
-        oa_lng      : oa_dest.lng
-    };
-    WriteBlock(&pkt, sizeof(pkt));
-}
-
-void AP_Logger::Write_SimpleAvoidance(uint8_t state, const Vector2f& desired_vel, const Vector2f& modified_vel, bool back_up)
-{
-    struct log_SimpleAvoid pkt{
-        LOG_PACKET_HEADER_INIT(LOG_SIMPLE_AVOID_MSG),
-        time_us         : AP_HAL::micros64(),
-        state           : state,
-        desired_vel_x   : desired_vel.x * 0.01f,
-        desired_vel_y   : desired_vel.y * 0.01f,
-        modified_vel_x  : modified_vel.x * 0.01f,
-        modified_vel_y  : modified_vel.y * 0.01f,
-        backing_up      : back_up,
-    };
-    WriteBlock(&pkt, sizeof(pkt));
-}
-
 void AP_Logger::Write_Winch(bool healthy, bool thread_end, bool moving, bool clutch, uint8_t mode, float desired_length, float length, float desired_rate, uint16_t tension, float voltage, int8_t temp)
 {
     struct log_Winch pkt{

libraries/AP_Logger/LogStructure.h

@@ -129,6 +129,7 @@ const struct MultiplierStructure log_Multipliers[] = {
 #include <AP_Baro/LogStructure.h>
 #include <AP_VisualOdom/LogStructure.h>
 #include <AC_PrecLand/LogStructure.h>
+#include <AC_Avoidance/LogStructure.h>
 
 // structure used to define logging format
 struct LogStructure {
@@ -657,48 +658,6 @@ struct PACKED log_SRTL {
     float D;
 };
 
-struct PACKED log_OABendyRuler {
-    LOG_PACKET_HEADER;
-    uint64_t time_us;
-    uint8_t type;
-    uint8_t active;
-    uint16_t target_yaw;
-    uint16_t yaw;
-    uint16_t target_pitch;
-    bool resist_chg;
-    float margin;
-    int32_t final_lat;
-    int32_t final_lng;
-    int32_t final_alt;
-    int32_t oa_lat;
-    int32_t oa_lng;
-    int32_t oa_alt;
-};
-
-struct PACKED log_OADijkstra {
-    LOG_PACKET_HEADER;
-    uint64_t time_us;
-    uint8_t state;
-    uint8_t error_id;
-    uint8_t curr_point;
-    uint8_t tot_points;
-    int32_t final_lat;
-    int32_t final_lng;
-    int32_t oa_lat;
-    int32_t oa_lng;
-};
-
-struct PACKED log_SimpleAvoid {
-  LOG_PACKET_HEADER;
-  uint64_t time_us;
-  uint8_t state;
-  float desired_vel_x;
-  float desired_vel_y;
-  float modified_vel_x;
-  float modified_vel_y;
-  uint8_t backing_up;
-};
-
 struct PACKED log_DSTL {
     LOG_PACKET_HEADER;
     uint64_t time_us;
@@ -1032,35 +991,6 @@ struct PACKED log_PSCZ {
 // @Field: Id: character referenced by FMTU
 // @Field: Mult: numeric multiplier
 
-// @LoggerMessage: OABR
-// @Description: Object avoidance (Bendy Ruler) diagnostics
-// @Field: TimeUS: Time since system startup
-// @Field: Type: Type of BendyRuler currently active
-// @Field: Act: True if Bendy Ruler avoidance is being used
-// @Field: DYaw: Best yaw chosen to avoid obstacle
-// @Field: Yaw: Current vehicle yaw
-// @Field: DP: Desired pitch chosen to avoid obstacle
-// @Field: RChg: True if BendyRuler resisted changing bearing and continued in last calculated bearing
-// @Field: Mar: Margin from path to obstacle on best yaw chosen
-// @Field: DLt: Destination latitude
-// @Field: DLg: Destination longitude
-// @Field: DAlt: Desired alt above EKF Origin
-// @Field: OLt: Intermediate location chosen for avoidance
-// @Field: OLg: Intermediate location chosen for avoidance
-// @Field: OAlt: Intermediate alt chosen for avoidance above EKF origin
-
-// @LoggerMessage: OADJ
-// @Description: Object avoidance (Dijkstra) diagnostics
-// @Field: TimeUS: Time since system startup
-// @Field: State: Dijkstra avoidance library state
-// @Field: Err: Dijkstra library error condition
-// @Field: CurrPoint: Destination point in calculated path to destination
-// @Field: TotPoints: Number of points in path to destination
-// @Field: DLat: Destination latitude
-// @Field: DLng: Destination longitude
-// @Field: OALat: Object Avoidance chosen destination point latitude
-// @Field: OALng: Object Avoidance chosen destination point longitude
-
 // @LoggerMessage: OF
 // @Description: Optical flow sensor data
 // @Field: TimeUS: Time since system startup
@@ -1245,16 +1175,6 @@ struct PACKED log_PSCZ {
 // @Field: E: point associated with most recent action (East component)
 // @Field: D: point associated with most recent action (Down component)
 
-// @LoggerMessage: SA
-// @Description: Simple Avoidance messages
-// @Field: TimeUS: Time since system startup
-// @Field: State: True if Simple Avoidance is active
-// @Field: DVelX: Desired velocity, X-Axis (Velocity before Avoidance)
-// @Field: DVelY: Desired velocity, Y-Axis (Velocity before Avoidance)
-// @Field: MVelX: Modified velocity, X-Axis (Velocity after Avoidance)
-// @Field: MVelY: Modified velocity, Y-Axis (Velocity after Avoidance)
-// @Field: Back: True if vehicle is backing away 
-
 // @LoggerMessage: TERR
 // @Description: Terrain database infomration
 // @Field: TimeUS: Time since system startup
@@ -1385,12 +1305,7 @@ LOG_STRUCTURE_FROM_CAMERA \
       "PM",  "QHHIIHHIIIIII", "TimeUS,NLon,NLoop,MaxT,Mem,Load,ErrL,IntE,ErrC,SPIC,I2CC,I2CI,Ex", "s---b%------s", "F---0A------F" }, \
     { LOG_SRTL_MSG, sizeof(log_SRTL), \
       "SRTL", "QBHHBfff", "TimeUS,Active,NumPts,MaxPts,Action,N,E,D", "s----mmm", "F----000" }, \
-    { LOG_OA_BENDYRULER_MSG, sizeof(log_OABendyRuler), \
+LOG_STRUCTURE_FROM_AVOIDANCE \
-      "OABR","QBBHHHBfLLiLLi","TimeUS,Type,Act,DYaw,Yaw,DP,RChg,Mar,DLt,DLg,DAlt,OLt,OLg,OAlt", "s-bddd-mDUmDUm", "F-------GGBGGB" }, \
-    { LOG_OA_DIJKSTRA_MSG, sizeof(log_OADijkstra), \
-      "OADJ","QBBBBLLLL","TimeUS,State,Err,CurrPoint,TotPoints,DLat,DLng,OALat,OALng", "sbbbbDUDU", "F----GGGG" }, \
-    { LOG_SIMPLE_AVOID_MSG, sizeof(log_SimpleAvoid), \
-      "SA",  "QBffffB","TimeUS,State,DVelX,DVelY,MVelX,MVelY,Back", "sbnnnnb", "F------"}, \
     { LOG_SIMSTATE_MSG, sizeof(log_AHRS), \
       "SIM","QccCfLLffff","TimeUS,Roll,Pitch,Yaw,Alt,Lat,Lng,Q1,Q2,Q3,Q4", "sddhmDU????", "FBBB0GG????" }, \
     { LOG_TERRAIN_MSG, sizeof(log_TERRAIN), \
@@ -1558,9 +1473,7 @@ enum LogMessages : uint8_t {
     LOG_ERROR_MSG,
     LOG_ADSB_MSG,
     LOG_ARM_DISARM_MSG,
-    LOG_OA_BENDYRULER_MSG,
+    LOG_IDS_FROM_AVOIDANCE,
-    LOG_OA_DIJKSTRA_MSG,
-    LOG_SIMPLE_AVOID_MSG,
     LOG_WINCH_MSG,
     LOG_PSC_MSG,
     LOG_PSCZ_MSG,