diff --git a/libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp b/libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp index 47b4b514bb..9fd2a7ae4e 100644 --- a/libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp +++ b/libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp @@ -53,17 +53,30 @@ AP_Proximity_Boundary_3D::Face AP_Proximity_Boundary_3D::get_face(float pitch, f } // Set the actual body-frame angle(yaw), pitch, and distance of the detected object. -// This method will also mark the sector and layer to be "valid", so this distance can be used for Obstacle Avoidance -void AP_Proximity_Boundary_3D::set_face_attributes(const Face &face, float pitch, float angle, float distance) +// This method will also mark the sector and layer to be "valid", +// This distance can then be used for Obstacle Avoidance +// Assume detected obstacle is horizontal (zero pitch), if no pitch is passed +// prx_instance should be set to the proximity sensor backend instance number +void AP_Proximity_Boundary_3D::set_face_attributes(const Face &face, float pitch, float angle, float distance, uint8_t prx_instance) { if (!face.valid()) { return; } + // ignore update if another instance has provided a shorter distance within the last 0.2 seconds + if ((prx_instance != _prx_instance[face.layer][face.sector]) && _distance_valid[face.layer][face.sector] && (_filtered_distance[face.layer][face.sector].get() < distance)) { + // check if recent + const uint32_t now_ms = AP_HAL::millis(); + if (now_ms - _last_update_ms[face.layer][face.sector] < PROXIMITY_FACE_RESET_MS) { + return; + } + } + _angle[face.layer][face.sector] = angle; _pitch[face.layer][face.sector] = pitch; _distance[face.layer][face.sector] = distance; _distance_valid[face.layer][face.sector] = true; + _prx_instance[face.layer][face.sector] = prx_instance; // apply filter set_filtered_distance(face, distance); @@ -170,11 +183,26 @@ void AP_Proximity_Boundary_3D::reset() // Reset this location, specified by Face object, back to default // i.e Distance is marked as not-valid, and set to a large number. -void AP_Proximity_Boundary_3D::reset_face(const Face &face) +// prx_instance should be set to the proximity sensor's backend instance number +void AP_Proximity_Boundary_3D::reset_face(const Face &face, uint8_t prx_instance) { if (!face.valid()) { return; } + + // return immediately if face already has no valid distance + if (!_distance_valid[face.layer][face.sector]) { + return; + } + + // ignore reset if another instance provided this face's distance within the last 0.2 seconds + if (prx_instance != _prx_instance[face.layer][face.sector]) { + const uint32_t now_ms = AP_HAL::millis(); + if (now_ms - _last_update_ms[face.layer][face.sector] < 200) { + return; + } + } + _distance_valid[face.layer][face.sector] = false; // update simple avoidance boundary @@ -194,10 +222,10 @@ void AP_Proximity_Boundary_3D::check_face_timeout() for (uint8_t layer=0; layer < PROXIMITY_NUM_LAYERS; layer++) { for (uint8_t sector=0; sector < PROXIMITY_NUM_SECTORS; sector++) { if (_distance_valid[layer][sector]) { - if ((AP_HAL::millis() - _last_update_ms[layer][sector]) > PROXIMITY_FACE_RESET_MS) { + if ((now_ms - _last_update_ms[layer][sector]) > PROXIMITY_FACE_RESET_MS) { // this face has a valid distance but wasn't updated for a long time, reset it - AP_Proximity_Boundary_3D::Face face{layer, sector}; - reset_face(face); + _distance_valid[layer][sector] = false; + update_boundary(AP_Proximity_Boundary_3D::Face{layer, sector}); } } } @@ -417,13 +445,14 @@ void AP_Proximity_Temp_Boundary::add_distance(const AP_Proximity_Boundary_3D::Fa } // fill the original 3D boundary with the contents of this temporary boundary -void AP_Proximity_Temp_Boundary::update_3D_boundary(AP_Proximity_Boundary_3D &boundary) +// prx_instance should be set to the proximity sensor's backend instance number +void AP_Proximity_Temp_Boundary::update_3D_boundary(uint8_t prx_instance, AP_Proximity_Boundary_3D &boundary) { for (uint8_t layer=0; layer < PROXIMITY_NUM_LAYERS; layer++) { for (uint8_t sector=0; sector < PROXIMITY_NUM_SECTORS; sector++) { if (_distances[layer][sector] < FLT_MAX) { AP_Proximity_Boundary_3D::Face face{layer, sector}; - boundary.set_face_attributes(face, _pitch[layer][sector], _angle[layer][sector], _distances[layer][sector]); + boundary.set_face_attributes(face, _pitch[layer][sector], _angle[layer][sector], _distances[layer][sector], prx_instance); } } } diff --git a/libraries/AP_Proximity/AP_Proximity_Boundary_3D.h b/libraries/AP_Proximity/AP_Proximity_Boundary_3D.h index 53a32ef3c1..3604492d7c 100644 --- a/libraries/AP_Proximity/AP_Proximity_Boundary_3D.h +++ b/libraries/AP_Proximity/AP_Proximity_Boundary_3D.h @@ -79,8 +79,9 @@ public: // This method will also mark the sector and layer to be "valid", // This distance can then be used for Obstacle Avoidance // Assume detected obstacle is horizontal (zero pitch), if no pitch is passed - void set_face_attributes(const Face &face, float pitch, float yaw, float distance); - void set_face_attributes(const Face &face, float yaw, float distance) { set_face_attributes(face, 0, yaw, distance); } + // prx_instance should be set to the proximity sensor backend instance number + void set_face_attributes(const Face &face, float pitch, float yaw, float distance, uint8_t prx_instance); + void set_face_attributes(const Face &face, float yaw, float distance, uint8_t prx_instance) { set_face_attributes(face, 0, yaw, distance, prx_instance); } // update boundary points used for simple avoidance based on a single sector and pitch distance changing // the boundary points lie on the line between sectors meaning two boundary points may be updated based on a single sector's distance changing @@ -92,7 +93,8 @@ public: // Reset this location, specified by Face object, back to default // i.e Distance is marked as not-valid - void reset_face(const Face &face); + // prx_instance should be set to the proximity sensor's backend instance number + void reset_face(const Face &face, uint8_t prx_instance); // check if a face has valid distance even if it was updated a long time back void check_face_timeout(); @@ -170,6 +172,7 @@ private: float _distance[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // distance to closest object within each sector and layer bool _distance_valid[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // true if a valid distance received for each sector and layer uint32_t _last_update_ms[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // time when distance was last updated + uint8_t _prx_instance[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // proximity sensor backend instance that provided the distance LowPassFilterFloat _filtered_distance[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // low pass filter float _filter_freq; // cutoff freq of low pass filter uint32_t _last_check_face_timeout_ms; // system time to throttle check_face_timeout method @@ -193,7 +196,8 @@ public: void add_distance(const AP_Proximity_Boundary_3D::Face &face, float yaw, float distance) { add_distance(face, 0.0f, yaw, distance); } // fill the original 3D boundary with the contents of this temporary boundary - void update_3D_boundary(AP_Proximity_Boundary_3D &boundary); + // prx_instance should be set to the proximity sensor's backend instance number + void update_3D_boundary(uint8_t prx_instance, AP_Proximity_Boundary_3D &boundary); private: diff --git a/libraries/AP_Proximity/AP_Proximity_Cygbot_D1.cpp b/libraries/AP_Proximity/AP_Proximity_Cygbot_D1.cpp index e275d89be2..25a0ed8a81 100644 --- a/libraries/AP_Proximity/AP_Proximity_Cygbot_D1.cpp +++ b/libraries/AP_Proximity/AP_Proximity_Cygbot_D1.cpp @@ -118,7 +118,7 @@ bool AP_Proximity_Cygbot_D1::parse_byte(uint8_t data) // checksum is valid, parse payload _last_distance_received_ms = AP_HAL::millis(); parse_payload(); - _temp_boundary.update_3D_boundary(frontend.boundary); + _temp_boundary.update_3D_boundary(state.instance, frontend.boundary); reset(); return true; } diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp index 3946e63891..c182b7c2b7 100644 --- a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp +++ b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp @@ -326,10 +326,10 @@ void AP_Proximity_LightWareSF40C::process_message() if (face != _face) { // update boundary used for avoidance if (_face_distance_valid) { - frontend.boundary.set_face_attributes(_face, _face_yaw_deg, _face_distance); + frontend.boundary.set_face_attributes(_face, _face_yaw_deg, _face_distance, state.instance); } else { // mark previous face invalid - frontend.boundary.reset_face(_face); + frontend.boundary.reset_face(_face, state.instance); } // init for new face _face = face; diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.cpp b/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.cpp index df01865783..79db05c6ae 100644 --- a/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.cpp +++ b/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.cpp @@ -144,10 +144,10 @@ void AP_Proximity_LightWareSF45B::process_message() const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(angle_deg); if (face != _face) { if (_face_distance_valid) { - frontend.boundary.set_face_attributes(_face, _face_yaw_deg, _face_distance); + frontend.boundary.set_face_attributes(_face, _face_yaw_deg, _face_distance, state.instance); } else { // mark previous face invalid - frontend.boundary.reset_face(_face); + frontend.boundary.reset_face(_face, state.instance); } // record updated face _face = face; diff --git a/libraries/AP_Proximity/AP_Proximity_MAV.cpp b/libraries/AP_Proximity/AP_Proximity_MAV.cpp index f54e19cd9a..a9d2814abf 100644 --- a/libraries/AP_Proximity/AP_Proximity_MAV.cpp +++ b/libraries/AP_Proximity/AP_Proximity_MAV.cpp @@ -86,7 +86,7 @@ void AP_Proximity_MAV::handle_distance_sensor_msg(const mavlink_message_t &msg) // provided we got the new obstacle in less than PROXIMITY_TIMESTAMP_MSG_TIMEOUT_MS if ((previous_msg_timestamp != _last_msg_update_timestamp_ms) || (time_diff > PROXIMITY_TIMESTAMP_MSG_TIMEOUT_MS)) { // push data from temp boundary to the main 3-D proximity boundary - temp_boundary.update_3D_boundary(frontend.boundary); + temp_boundary.update_3D_boundary(state.instance, frontend.boundary); // clear temp boundary for new data temp_boundary.reset(); } @@ -178,9 +178,9 @@ void AP_Proximity_MAV::handle_obstacle_distance_msg(const mavlink_message_t &msg if (latest_face != face) { // store previous face if (face_distance_valid) { - frontend.boundary.set_face_attributes(face, face_yaw_deg, face_distance); + frontend.boundary.set_face_attributes(face, face_yaw_deg, face_distance, state.instance); } else { - frontend.boundary.reset_face(face); + frontend.boundary.reset_face(face, state.instance); } // init for latest face face = latest_face; @@ -202,9 +202,9 @@ void AP_Proximity_MAV::handle_obstacle_distance_msg(const mavlink_message_t &msg // process the last face if (face_distance_valid) { - frontend.boundary.set_face_attributes(face, face_yaw_deg, face_distance); + frontend.boundary.set_face_attributes(face, face_yaw_deg, face_distance, state.instance); } else { - frontend.boundary.reset_face(face); + frontend.boundary.reset_face(face, state.instance); } return; } @@ -231,7 +231,7 @@ void AP_Proximity_MAV::handle_obstacle_distance_3d_msg(const mavlink_message_t & if ((previous_msg_timestamp != _last_msg_update_timestamp_ms) || (time_diff > PROXIMITY_TIMESTAMP_MSG_TIMEOUT_MS)) { // push data from temp boundary to the main 3-D proximity boundary because a new timestamp has arrived - temp_boundary.update_3D_boundary(frontend.boundary); + temp_boundary.update_3D_boundary(state.instance, frontend.boundary); // clear temp boundary for new data temp_boundary.reset(); } diff --git a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp b/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp index 4e0bf20bb6..6f30df2fd6 100644 --- a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp +++ b/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp @@ -319,10 +319,10 @@ void AP_Proximity_RPLidarA2::parse_response_data() if (face != _last_face) { // distance is for a new face, the previous one can be updated now if (_last_distance_valid) { - frontend.boundary.set_face_attributes(_last_face, _last_angle_deg, _last_distance_m); + frontend.boundary.set_face_attributes(_last_face, _last_angle_deg, _last_distance_m, state.instance); } else { // reset distance from last face - frontend.boundary.reset_face(face); + frontend.boundary.reset_face(face, state.instance); } // initialize the new face diff --git a/libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp b/libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp index af8b2447c3..ebcca88be7 100644 --- a/libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp +++ b/libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp @@ -51,11 +51,11 @@ void AP_Proximity_RangeFinder::update(void) _distance_min = sensor->min_distance_cm() * 0.01f; _distance_max = sensor->max_distance_cm() * 0.01f; if ((distance <= _distance_max) && (distance >= _distance_min) && !ignore_reading(angle, distance, false)) { - frontend.boundary.set_face_attributes(face, angle, distance); + frontend.boundary.set_face_attributes(face, angle, distance, state.instance); // update OA database database_push(angle, distance); } else { - frontend.boundary.reset_face(face); + frontend.boundary.reset_face(face, state.instance); } _last_update_ms = now; } diff --git a/libraries/AP_Proximity/AP_Proximity_SITL.cpp b/libraries/AP_Proximity/AP_Proximity_SITL.cpp index a813d8865a..f5a71de2f3 100644 --- a/libraries/AP_Proximity/AP_Proximity_SITL.cpp +++ b/libraries/AP_Proximity/AP_Proximity_SITL.cpp @@ -64,11 +64,11 @@ void AP_Proximity_SITL::update(void) AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(yaw_angle_deg); float fence_distance; if (get_distance_to_fence(yaw_angle_deg, fence_distance)) { - frontend.boundary.set_face_attributes(face, yaw_angle_deg, fence_distance); + frontend.boundary.set_face_attributes(face, yaw_angle_deg, fence_distance, state.instance); // update OA database database_push(yaw_angle_deg, fence_distance); } else { - frontend.boundary.reset_face(face); + frontend.boundary.reset_face(face, state.instance); } } } else { diff --git a/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp b/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp index 06bd4a5692..3c7119726a 100644 --- a/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp +++ b/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp @@ -97,11 +97,11 @@ void AP_Proximity_TeraRangerTower::update_sector_data(int16_t angle_deg, uint16_ // Get location on 3-D boundary based on angle to the object const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(angle_deg); if ((distance_mm != 0xffff) && !ignore_reading(angle_deg, distance_mm * 0.001f, false)) { - frontend.boundary.set_face_attributes(face, angle_deg, ((float) distance_mm) / 1000); + frontend.boundary.set_face_attributes(face, angle_deg, ((float) distance_mm) / 1000, state.instance); // update OA database database_push(angle_deg, ((float) distance_mm) / 1000); } else { - frontend.boundary.reset_face(face); + frontend.boundary.reset_face(face, state.instance); } _last_distance_received_ms = AP_HAL::millis(); } diff --git a/libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp b/libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp index cae390f867..e9aceee786 100644 --- a/libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp +++ b/libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp @@ -152,11 +152,11 @@ void AP_Proximity_TeraRangerTowerEvo::update_sector_data(int16_t angle_deg, uint //check for target too far, target too close and sensor not connected const bool valid = (distance_mm != 0xffff) && (distance_mm > 0x0001); if (valid && !ignore_reading(angle_deg, distance_mm * 0.001f, false)) { - frontend.boundary.set_face_attributes(face, angle_deg, ((float) distance_mm) / 1000); + frontend.boundary.set_face_attributes(face, angle_deg, ((float) distance_mm) / 1000, state.instance); // update OA database database_push(angle_deg, ((float) distance_mm) / 1000); } else { - frontend.boundary.reset_face(face); + frontend.boundary.reset_face(face, state.instance); } _last_distance_received_ms = AP_HAL::millis(); }