Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Proximity: remove mavlink boundary clearing conditions

This commit is contained in:
Rishabh 2021-04-19 01:03:21 +05:30 committed by Randy Mackay
parent fd2909d53f
commit acfdf6a38f
3 changed files with 4 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_Proximity/AP_Proximity_Backend.h
View File

@ -24,7 +24,7 @@
#define PROXIMITY_GND_DETECT_THRESHOLD 1.0f // set ground detection threshold to be 1 meters #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_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 class AP_Proximity_Backend
{ {

2
libraries/AP_Proximity/AP_Proximity_Boundary_3D.h
View File

@ -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_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_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_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 class AP_Proximity_Boundary_3D
{ {

10
libraries/AP_Proximity/AP_Proximity_MAV.cpp
View File

@ -23,7 +23,7 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
#define PROXIMITY_MAV_TIMEOUT_MS 500 // distance messages must arrive within this many milliseconds #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 // update the state of the sensor
void AP_Proximity_MAV::update(void) 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 // 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 // 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)) { 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
temp_boundary.update_3D_boundary(boundary); temp_boundary.update_3D_boundary(boundary);
// clear temp boundary for new data // clear temp boundary for new data
@ -231,12 +229,8 @@ void AP_Proximity_MAV::handle_obstacle_distance_3d_msg(const mavlink_message_t &
return; 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)) { 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 // push data from temp boundary to the main 3-D proximity boundary because a new timestamp has arrived
boundary.reset();
// push data from temp boundary to the main 3-D proximity boundary
temp_boundary.update_3D_boundary(boundary); temp_boundary.update_3D_boundary(boundary);
// clear temp boundary for new data // clear temp boundary for new data
temp_boundary.reset(); temp_boundary.reset();