AC_Avoid: reenable avoidance logging

libraries/AC_Avoidance/AC_Avoid.cpp

@@ -241,6 +241,23 @@ void AC_Avoid::adjust_velocity(Vector3f &desired_vel_cms, bool &backing_up, floa
     if (desired_vel_cms_original != desired_vel_cms) {
         _last_limit_time = AP_HAL::millis();
     }
+
+    if (limits_active()) {
+        // log at not more than 10hz (adjust_velocity method can be potentially called at 400hz!)
+        uint32_t now = AP_HAL::millis();
+        if ((now - _last_log_ms) > 100) {
+            _last_log_ms = now;
+            Write_SimpleAvoidance(true, desired_vel_cms_original, desired_vel_cms, backing_up);
+        }
+    } else {
+        // avoidance isn't active anymore
+        // log once so that it registers in logs
+        if (_last_log_ms) {
+            Write_SimpleAvoidance(false, desired_vel_cms_original, desired_vel_cms, backing_up);
+            // this makes sure logging won't run again till it is active
+            _last_log_ms = 0;
+        }
+    }
 }
 
 /*

libraries/AC_Avoidance/AC_Avoid.h

@@ -205,7 +205,7 @@ private:
     void get_proximity_roll_pitch_pct(float &roll_positive, float &roll_negative, float &pitch_positive, float &pitch_negative);
 
     // Logging function
-    void Write_SimpleAvoidance(const uint8_t state, const Vector2f& desired_vel, const Vector2f& modified_vel, const bool back_up) const;
+    void Write_SimpleAvoidance(const uint8_t state, const Vector3f& desired_vel, const Vector3f& modified_vel, const bool back_up) const;
 
     // parameters
     AP_Int8 _enabled;

libraries/AC_Avoidance/AC_Avoidance_Logging.cpp

@@ -46,7 +46,7 @@ void AP_OADijkstra::Write_OADijkstra(const uint8_t state, const uint8_t error_id
     AP::logger().WriteBlock(&pkt, sizeof(pkt));
 }
 
-void AC_Avoid::Write_SimpleAvoidance(const uint8_t state, const Vector2f& desired_vel, const Vector2f& modified_vel, const bool back_up) const
+void AC_Avoid::Write_SimpleAvoidance(const uint8_t state, const Vector3f& desired_vel, const Vector3f& modified_vel, const bool back_up) const
 {
     const struct log_SimpleAvoid pkt{
         LOG_PACKET_HEADER_INIT(LOG_SIMPLE_AVOID_MSG),
@@ -54,8 +54,10 @@ void AC_Avoid::Write_SimpleAvoidance(const uint8_t state, const Vector2f& desire
         state           : state,
         desired_vel_x   : desired_vel.x * 0.01f,
         desired_vel_y   : desired_vel.y * 0.01f,
+        desired_vel_z   : desired_vel.z * 0.01f,
         modified_vel_x  : modified_vel.x * 0.01f,
         modified_vel_y  : modified_vel.y * 0.01f,
+        modified_vel_z  : modified_vel.z * 0.01f,
         backing_up      : back_up,
     };
     AP::logger().WriteBlock(&pkt, sizeof(pkt));

libraries/AC_Avoidance/LogStructure.h

@@ -71,8 +71,10 @@ struct PACKED log_OADijkstra {
 // @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: DVelZ: Desired velocity, Z-Axis (Velocity before Avoidance)
 // @Field: MVelX: Modified velocity, X-Axis (Velocity after Avoidance)
 // @Field: MVelY: Modified velocity, Y-Axis (Velocity after Avoidance)
+// @Field: MVelZ: Modified velocity, Z-Axis (Velocity after Avoidance)
 // @Field: Back: True if vehicle is backing away
 struct PACKED log_SimpleAvoid {
   LOG_PACKET_HEADER;
@@ -80,8 +82,10 @@ struct PACKED log_SimpleAvoid {
   uint8_t state;
   float desired_vel_x;
   float desired_vel_y;
+  float desired_vel_z;
   float modified_vel_x;
   float modified_vel_y;
+  float modified_vel_z;
   uint8_t backing_up;
 };
 
@@ -91,4 +95,4 @@ struct PACKED log_SimpleAvoid {
     { 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------"},
+      "SA",  "QBffffffB","TimeUS,State,DVelX,DVelY,DVelZ,MVelX,MVelY,MVelZ,Back", "sbnnnnnnb", "F--------"},