AP_Proximity: sf40c combines seven readings for oadb

libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp

@@ -338,35 +338,58 @@ void AP_Proximity_LightWareSF40C::process_message()
         const float angle_correction = frontend.get_yaw_correction(state.instance);
         const uint16_t dist_min_cm = distance_min() * 100;
         const uint16_t dist_max_cm = distance_max() * 100;
+
+        // mini sectors are used to combine several readings together
+        uint8_t combined_count = 0;
+        float combined_angle_deg = 0;
+        float combined_dist_m = INT16_MAX;
         for (uint16_t i = 0; i < point_count; i++) {
             const uint16_t idx = 14 + (i * 2);
             const int16_t dist_cm = (int16_t)buff_to_uint16(_msg.payload[idx], _msg.payload[idx+1]);
             const float angle_deg = wrap_360((point_start_index + i) * angle_inc_deg * angle_sign + angle_correction);
-            if (!ignore_reading(angle_deg)) {
+            const uint8_t sector = convert_angle_to_sector(angle_deg);
-                const uint8_t sector = convert_angle_to_sector(angle_deg);
+
-                if (sector != _last_sector) {
+            // if we've entered a new sector then finish off previous sector
-                    // update boundary used for avoidance
+            if (sector != _last_sector) {
-                    if (_last_sector != UINT8_MAX) {
+                // update boundary used for avoidance
-                        update_boundary_for_sector(_last_sector, false);
+                if (_last_sector != UINT8_MAX) {
-                    }
+                    update_boundary_for_sector(_last_sector, false);
-                    _last_sector = sector;
-                    // init for new sector
-                    _distance[sector] = INT16_MAX;
-                    _distance_valid[sector] = false;
                 }
+                // init for new sector
+                _last_sector = sector;
+                _distance[sector] = INT16_MAX;
+                _distance_valid[sector] = false;
+            }
+
+            // check reading is not within an ignore zone
+            if (!ignore_reading(angle_deg)) {
+                // check distance reading is valid
                 if ((dist_cm >= dist_min_cm) && (dist_cm <= dist_max_cm)) {
-                    // use shortest valid distance for this sector's distance
                     const float dist_m = dist_cm * 0.01f;
+
+                    // update shortest distance for this sector
                     if (dist_m < _distance[sector]) {
                         _angle[sector] = angle_deg;
                         _distance[sector] = dist_m;
                         _distance_valid[sector] = true;
                     }
-                    // send point to object avoidance database
+
-                    if (database_ready) {
+                    // calculate shortest of last few readings
-                        database_push(angle_deg, dist_m, _last_distance_received_ms, current_loc, current_heading);
+                    if (dist_m < combined_dist_m) {
+                        combined_dist_m = dist_m;
+                        combined_angle_deg = angle_deg;
                     }
+                    combined_count++;
+                }
+            }
+
+            // send combined distance to object database
+            if ((i+1 >= point_count) || (combined_count >= PROXIMITY_SF40C_COMBINE_READINGS)) {
+                if ((combined_dist_m < INT16_MAX) && database_ready) {
+                    database_push(combined_angle_deg, combined_dist_m, _last_distance_received_ms, current_loc, current_heading);
                 }
+                combined_count = 0;
+                combined_dist_m = INT16_MAX;
             }
         }
         break;

libraries/AP_Proximity/AP_Proximity_LightWareSF40C.h

@@ -5,6 +5,7 @@
 
 #define PROXIMITY_SF40C_TIMEOUT_MS            200   // requests timeout after 0.2 seconds
 #define PROXIMITY_SF40C_PAYLOAD_LEN_MAX       256   // maximum payload size we can accept (in some configurations sensor may send as large as 1023)
+#define PROXIMITY_SF40C_COMBINE_READINGS        7   // combine this many readings together to improve efficiency
 
 class AP_Proximity_LightWareSF40C : public AP_Proximity_Backend
 {