rangefinder 一些修改

libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp

@@ -120,11 +120,6 @@ bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm)
                 }
                 // if checksum matches extract contents
                 if (checksum == linebuf[BENEWAKE_FRAME_LENGTH-1]) {
-
-                    uint16_t strength = ((uint16_t)linebuf[5] << 8) | linebuf[4];
-                    if(strength < 100 || strength == 0xFFFF){
-                        return false;
-                    }
                     // calculate distance
                     uint16_t dist = ((uint16_t)linebuf[3] << 8) | linebuf[2];
                     if (dist >= BENEWAKE_DIST_MAX_CM) {

libraries/AP_RangeFinder/RangeFinder.cpp

@@ -88,7 +88,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
 
     // @Group: 4_
     // @Path: AP_RangeFinder_Wasp.cpp
-    AP_SUBGROUPVARPTR(drivers[0], "4_",  60, RangeFinder, backend_var_info[3]),
+    AP_SUBGROUPVARPTR(drivers[3], "4_",  60, RangeFinder, backend_var_info[3]),
 #endif
 
 #if RANGEFINDER_MAX_INSTANCES > 4
@@ -556,15 +556,22 @@ bool RangeFinder::has_orientation(enum Rotation orientation) const
 // find first range finder instance with the specified orientation
 AP_RangeFinder_Backend *RangeFinder::find_instance(enum Rotation orientation) const
 {
+    // first try for a rangefinder that is in range
     for (uint8_t i=0; i<num_instances; i++) {
         AP_RangeFinder_Backend *backend = get_backend(i);
-        if (backend == nullptr) {
-            continue;
+        if (backend != nullptr &&
+            backend->orientation() == orientation &&
+            backend->status() == RangeFinder_Good) {
+            return backend;
         }
-        if (backend->orientation() != orientation) {
-            continue;
+    }
+    // if none in range then return first with correct orientation
+    for (uint8_t i=0; i<num_instances; i++) {
+        AP_RangeFinder_Backend *backend = get_backend(i);
+        if (backend != nullptr &&
+            backend->orientation() == orientation) {
+            return backend;
         }
-        return backend;
     }
     return nullptr;
 }
@@ -676,30 +683,16 @@ void RangeFinder::Log_RFND()
         if (s == nullptr) {
             continue;
         }
-        if (i == 0)
-        {
-            const struct log_RFND 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));
-        }
-        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));
-        }
+                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));
     }
 }