From 10108819b10c0108f97874b015739246e392fe44 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 4 Dec 2019 14:27:30 +0900 Subject: [PATCH] AP_Proximity: sf40c combines seven readings for oadb --- .../AP_Proximity_LightWareSF40C.cpp | 53 +++++++++++++------ .../AP_Proximity_LightWareSF40C.h | 1 + 2 files changed, 39 insertions(+), 15 deletions(-) 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 {