diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.h b/libraries/AP_Proximity/AP_Proximity_Backend.h index efd71b1598..48422a918d 100644 --- a/libraries/AP_Proximity/AP_Proximity_Backend.h +++ b/libraries/AP_Proximity/AP_Proximity_Backend.h @@ -24,7 +24,7 @@ #define PROXIMITY_GND_DETECT_THRESHOLD 1.0f // set ground detection threshold to be 1 meters #define PROXIMITY_ALT_DETECT_TIMEOUT_MS 500 // alt readings should arrive within this much time -#define PROXIMITY_BOUNDARY_3D_TIMEOUT_MS 1000 // we should check the 3D boundary faces after every this many ms +#define PROXIMITY_BOUNDARY_3D_TIMEOUT_MS 750 // we should check the 3D boundary faces after every this many ms class AP_Proximity_Backend { diff --git a/libraries/AP_Proximity/AP_Proximity_Boundary_3D.h b/libraries/AP_Proximity/AP_Proximity_Boundary_3D.h index 1d8b8e1a30..b9f2c01702 100644 --- a/libraries/AP_Proximity/AP_Proximity_Boundary_3D.h +++ b/libraries/AP_Proximity/AP_Proximity_Boundary_3D.h @@ -29,7 +29,7 @@ #define PROXIMITY_BOUNDARY_DIST_MIN 0.6f // minimum distance for a boundary point. This ensures the object avoidance code doesn't think we are outside the boundary. #define PROXIMITY_BOUNDARY_DIST_DEFAULT 100 // if we have no data for a sector, boundary is placed 100m out #define PROXIMITY_FILT_RESET_TIME 1000 // reset filter if last distance was pushed more than this many ms away -#define PROXIMITY_FACE_RESET_MS 1250 // face will be reset if not updated within this many ms +#define PROXIMITY_FACE_RESET_MS 1000 // face will be reset if not updated within this many ms class AP_Proximity_Boundary_3D { diff --git a/libraries/AP_Proximity/AP_Proximity_MAV.cpp b/libraries/AP_Proximity/AP_Proximity_MAV.cpp index 18f765a363..2936a315ae 100644 --- a/libraries/AP_Proximity/AP_Proximity_MAV.cpp +++ b/libraries/AP_Proximity/AP_Proximity_MAV.cpp @@ -23,7 +23,7 @@ extern const AP_HAL::HAL& hal; #define PROXIMITY_MAV_TIMEOUT_MS 500 // distance messages must arrive within this many milliseconds -#define PROXIMITY_TIMESTAMP_MSG_TIMEOUT_MS 50 // boundary will be reset if mavlink message does not arrive within this many milliseconds +#define PROXIMITY_TIMESTAMP_MSG_TIMEOUT_MS 50 // obstacles will be transferred from temp boundary to actual boundary if mavlink message does not arrive within this many milliseconds // update the state of the sensor void AP_Proximity_MAV::update(void) @@ -85,8 +85,6 @@ void AP_Proximity_MAV::handle_distance_sensor_msg(const mavlink_message_t &msg) // we will add on to the last fence if the time stamp is the same // 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)) { - // cleared fence back to defaults since we have a new timestamp - boundary.reset(); // push data from temp boundary to the main 3-D proximity boundary temp_boundary.update_3D_boundary(boundary); // clear temp boundary for new data @@ -231,12 +229,8 @@ void AP_Proximity_MAV::handle_obstacle_distance_3d_msg(const mavlink_message_t & return; } - // we will add on to the last fence if the time stamp is the same - // 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)) { - // cleared fence back to defaults since we have a new timestamp - boundary.reset(); - // push data from temp boundary to the main 3-D proximity boundary + // push data from temp boundary to the main 3-D proximity boundary because a new timestamp has arrived temp_boundary.update_3D_boundary(boundary); // clear temp boundary for new data temp_boundary.reset();