RangeFinder: Add up to 10 RangeFinder logs

libraries/AP_Logger/LogStructure.h

@@ -801,12 +801,10 @@ struct PACKED log_Mode {
 struct PACKED log_RFND {
     LOG_PACKET_HEADER;
     uint64_t time_us;
-    uint16_t dist1;
-    uint8_t status1;
-    uint8_t orient1;
-    uint16_t dist2;
-    uint8_t status2;
-    uint8_t orient2;
+    uint8_t instance;
+    uint16_t dist;
+    uint8_t status;
+    uint8_t orient;
 };
 
 /*
@@ -1329,7 +1327,7 @@ struct PACKED log_Arm_Disarm {
     { LOG_MODE_MSG, sizeof(log_Mode), \
       "MODE", "QMBB",         "TimeUS,Mode,ModeNum,Rsn", "s---", "F---" }, \
     { LOG_RFND_MSG, sizeof(log_RFND), \
-      "RFND", "QCBBCBB", "TimeUS,Dist1,Stat1,Orient1,Dist2,Stat2,Orient2", "sm--m--", "FB--B--" }, \
+      "RFND", "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), \

libraries/AP_RangeFinder/RangeFinder.cpp

@@ -675,23 +675,22 @@ void RangeFinder::Log_RFND()
         return;
     }
 
-    const AP_RangeFinder_Backend *s0 = get_backend(0);
-    const AP_RangeFinder_Backend *s1 = get_backend(1);
-    if (s0 == nullptr && s1 == nullptr) {
-        return;
-    }
+    for (uint8_t i=0; i<RANGEFINDER_MAX_INSTANCES; i++) {
+        const AP_RangeFinder_Backend *s = get_backend(i);
+        if (s == nullptr) {
+            continue;
+        }
 
-    struct log_RFND pkt = {
-        LOG_PACKET_HEADER_INIT((uint8_t)(LOG_RFND_MSG)),
-        time_us       : AP_HAL::micros64(),
-        dist1         : s0 ? s0->distance_cm() : (uint16_t)0,
-        status1       : s0 ? (uint8_t)s0->status() : (uint8_t)0,
-        orient1       : s0 ? s0->orientation() : ROTATION_NONE,
-        dist2         : s1 ? s1->distance_cm() : (uint16_t)0,
-        status2       : s1 ? (uint8_t)s1->status() : (uint8_t)0,
-        orient2       : s1 ? s1->orientation() : ROTATION_NONE,
-    };
-    AP::logger().WriteBlock(&pkt, sizeof(pkt));
+        const struct log_RFND pkt = {
+                LOG_PACKET_HEADER_INIT(LOG_RFND_MSG),
+                time_us      : AP_HAL::micros64(),
+                instance     : i,
+                dist         : s->distance_cm(),
+                status       : (uint8_t)s->status(),
+                orient       : s->orientation(),
+        };
+        AP::logger().WriteBlock(&pkt, sizeof(pkt));
+    }
 }
 
 RangeFinder *RangeFinder::_singleton;