diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp index 85b6ffb9a9..ec8b3379f1 100644 --- a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp +++ b/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); - if (sector != _last_sector) { - // update boundary used for avoidance - 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; + const uint8_t sector = convert_angle_to_sector(angle_deg); + + // if we've entered a new sector then finish off previous sector + if (sector != _last_sector) { + // update boundary used for avoidance + if (_last_sector != UINT8_MAX) { + update_boundary_for_sector(_last_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) { - database_push(angle_deg, dist_m, _last_distance_received_ms, current_loc, current_heading); + + // calculate shortest of last few readings + 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; diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.h b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.h index 9a68b6cec6..157ee79fac 100644 --- a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.h +++ b/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 {