rangefind2 log added

libraries/AP_Logger/LogStructure.h

@@ -1384,6 +1384,8 @@ struct PACKED log_Arm_Disarm {
       "MODE", "QMBB",         "TimeUS,Mode,ModeNum,Rsn", "s---", "F---" }, \
     { LOG_RFND_MSG, sizeof(log_RFND), \
       "RFND", "QBCBB", "TimeUS,Instance,Dist,Stat,Orient", "s#m--", "F-B--" }, \
+     { LOG_RFND2_MSG, sizeof(log_RFND), \
+      "RFN2", "QBCBB", "TimeUS,Instance,Dist,Stat,Orient", "s#m--", "F-B--" }, \
     { LOG_MAV_STATS, sizeof(log_MAV_Stats), \
       "DMS", "IIIIIBBBBBBBBB",         "TimeMS,N,Dp,RT,RS,Fa,Fmn,Fmx,Pa,Pmn,Pmx,Sa,Smn,Smx", "s-------------", "C-------------" }, \
     { LOG_BEACON_MSG, sizeof(log_Beacon), \
@@ -1772,7 +1774,7 @@ enum LogMessages : uint8_t {
     LOG_ARM_DISARM_MSG,
     LOG_OA_BENDYRULER_MSG,
     LOG_OA_DIJKSTRA_MSG,
-
+    LOG_RFND2_MSG,
     _LOG_LAST_MSG_
 };
 

libraries/AP_RangeFinder/RangeFinder.cpp

@@ -676,7 +676,8 @@ void RangeFinder::Log_RFND()
         if (s == nullptr) {
             continue;
         }
-
+        if (i == 0)
+        {
             const struct log_RFND pkt = {
                 LOG_PACKET_HEADER_INIT(LOG_RFND_MSG),
                 time_us : AP_HAL::micros64(),
@@ -687,6 +688,19 @@ void RangeFinder::Log_RFND()
             };
             AP::logger().WriteBlock(&pkt, sizeof(pkt));
         }
+        else
+        {
+            const struct log_RFND pkt2 = {
+                LOG_PACKET_HEADER_INIT(LOG_RFND2_MSG),
+                time_us : AP_HAL::micros64(),
+                instance : i,
+                dist : s->distance_cm(),
+                status : (uint8_t)s->status(),
+                orient : s->orientation(),
+            };
+            AP::logger().WriteBlock(&pkt2, sizeof(pkt2));
+        }
+    }
 }
 
 bool RangeFinder::prearm_healthy(char *failure_msg, const uint8_t failure_msg_len) const

modules/mavlink

@@ -1 +1 @@
-Subproject commit ca507c39ff56b88ce78a072a691e4b0c2a95f527
+Subproject commit 18cab6950986cd7b9b7a507ec35f3501d4a7e019