AP_Logger: Log Simple Avoidance

libraries/AP_Logger/AP_Logger.h

@@ -289,6 +289,7 @@ public:
     void Write_SRTL(bool active, uint16_t num_points, uint16_t max_points, uint8_t action, const Vector3f& point);
     void Write_OABendyRuler(bool active, float target_yaw, 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(const char *name, const char *labels, const char *fmt, ...);
     void Write(const char *name, const char *labels, const char *units, const char *mults, const char *fmt, ...);

libraries/AP_Logger/LogFile.cpp

@@ -1078,3 +1078,18 @@ void AP_Logger::Write_OADijkstra(uint8_t state, uint8_t error_id, uint8_t curr_p
     };
     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));
+}

libraries/AP_Logger/LogStructure.h

@@ -1194,6 +1194,17 @@ struct PACKED log_OADijkstra {
     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;
@@ -2107,6 +2118,16 @@ struct PACKED log_Arm_Disarm {
 // @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
@@ -2436,6 +2457,8 @@ struct PACKED log_Arm_Disarm {
       "OABR","QBHHBfLLLL","TimeUS,Active,DesYaw,Yaw,ResChg,Mar,DLat,DLng,OALat,OALng", "sbdd-mDUDU", "F-----GGGG" }, \
     { 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_IMU2_MSG, sizeof(log_IMU), \
       "IMU2",  IMU_FMT,     IMU_LABELS, IMU_UNITS, IMU_MULTS }, \
     { LOG_IMU3_MSG, sizeof(log_IMU), \
@@ -2728,6 +2751,7 @@ enum LogMessages : uint8_t {
     LOG_OA_BENDYRULER_MSG,
     LOG_OA_DIJKSTRA_MSG,
     LOG_VISUALVEL_MSG,
+    LOG_SIMPLE_AVOID_MSG,
 
     _LOG_LAST_MSG_
 };