diff --git a/libraries/AP_Proximity/AP_Proximity_AirSimSITL.cpp b/libraries/AP_Proximity/AP_Proximity_AirSimSITL.cpp index d7b42649ab..182194cdb8 100644 --- a/libraries/AP_Proximity/AP_Proximity_AirSimSITL.cpp +++ b/libraries/AP_Proximity/AP_Proximity_AirSimSITL.cpp @@ -46,6 +46,8 @@ void AP_Proximity_AirSimSITL::update(void) const float accuracy_sq = sq(PROXIMITY_ACCURACY); bool prev_pos_valid = false; Vector2f prev_pos; + // clear temp boundary since we have a new message + temp_boundary.reset(); for (uint16_t i=0; i= accuracy_sq)) { @@ -72,9 +76,8 @@ void AP_Proximity_AirSimSITL::update(void) } } } - - // update middle boundary - boundary.update_middle_boundary(); + // copy temp boundary to real boundary + temp_boundary.update_3D_boundary(boundary); } // get maximum and minimum distances (in meters) of primary sensor diff --git a/libraries/AP_Proximity/AP_Proximity_AirSimSITL.h b/libraries/AP_Proximity/AP_Proximity_AirSimSITL.h index 495bb785a1..081628e928 100644 --- a/libraries/AP_Proximity/AP_Proximity_AirSimSITL.h +++ b/libraries/AP_Proximity/AP_Proximity_AirSimSITL.h @@ -41,6 +41,7 @@ public: private: SITL::SITL *sitl = AP::sitl(); + AP_Proximity_Temp_Boundary temp_boundary; }; #endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp b/libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp index 61bdf01d75..aa7054a8cc 100644 --- a/libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp +++ b/libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp @@ -73,24 +73,6 @@ void AP_Proximity_Boundary_3D::set_face_attributes(const Face &face, float pitch update_boundary(face); } -// add a distance to the boundary if it is shorter than any other provided distance since the last time the boundary was reset -// pitch and yaw are in degrees, distance is in meters -void AP_Proximity_Boundary_3D::add_distance(float pitch, float yaw, float distance) -{ - Face face = get_face(pitch, yaw); - if (!face.valid()) { - return; - } - - if (!_distance_valid[face.layer][face.sector] || (distance < _distance[face.layer][face.sector])) { - _distance[face.layer][face.sector] = distance; - _distance_valid[face.layer][face.sector] = true; - _angle[face.layer][face.sector] = yaw; - _pitch[face.layer][face.sector] = pitch; - set_filtered_distance(face, distance); - } -} - // apply a new cutoff_freq to low-pass filter void AP_Proximity_Boundary_3D::apply_filter_freq(float cutoff_freq) { @@ -177,14 +159,6 @@ void AP_Proximity_Boundary_3D::update_boundary(const Face &face) } } -// update middle layer boundary points -void AP_Proximity_Boundary_3D::update_middle_boundary() -{ - for (uint8_t sector=0; sector < PROXIMITY_NUM_SECTORS; sector++) { - update_boundary(Face{PROXIMITY_MIDDLE_LAYER, sector}); - } -} - // reset boundary. marks all distances as invalid void AP_Proximity_Boundary_3D::reset() { @@ -404,4 +378,38 @@ bool AP_Proximity_Boundary_3D::get_layer_distances(uint8_t layer_number, float d return valid_distances; } -#endif // HAL_PROXIMITY_ENABLED +// reset the temporary boundary. This fills in distances with FLT_MAX +void AP_Proximity_Temp_Boundary::reset() +{ + for (uint8_t layer=0; layer < PROXIMITY_NUM_LAYERS; layer++) { + for (uint8_t sector=0; sector < PROXIMITY_NUM_SECTORS; sector++) { + _distances[layer][sector] = FLT_MAX; + } + } +} + +// add a distance to the temp boundary if it is shorter than any other provided distance since the last time the boundary was reset +// pitch and yaw are in degrees, distance is in meters +void AP_Proximity_Temp_Boundary::add_distance(const AP_Proximity_Boundary_3D::Face &face, float pitch, float yaw, float distance) +{ + if (face.valid() && distance < _distances[face.layer][face.sector]) { + _distances[face.layer][face.sector] = distance; + _angle[face.layer][face.sector] = yaw; + _pitch[face.layer][face.sector] = pitch; + } +} + +// 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) +{ + 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]); + } + } + } +} + +#endif // HAL_PROXIMITY_ENABLED \ No newline at end of file diff --git a/libraries/AP_Proximity/AP_Proximity_Boundary_3D.h b/libraries/AP_Proximity/AP_Proximity_Boundary_3D.h index d2f30fa2b8..1d8b8e1a30 100644 --- a/libraries/AP_Proximity/AP_Proximity_Boundary_3D.h +++ b/libraries/AP_Proximity/AP_Proximity_Boundary_3D.h @@ -71,19 +71,11 @@ public: 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); } - // add a distance to the boundary if it is shorter than any other provided distance since the last time the boundary was reset - // pitch and yaw are in degrees, distance is in meters - void add_distance(float pitch, float yaw, float distance); - void add_distance(float yaw, float distance) { add_distance(0, yaw, distance); } - // 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 // the boundary point is set to the shortest distance found in the two adjacent sectors, this is a conservative boundary around the vehicle void update_boundary(const Face &face); - // update middle layer boundary points - void update_middle_boundary(); - // reset boundary. marks all distances as invalid void reset(); @@ -171,4 +163,31 @@ private: float _filter_freq; // cutoff freq of low pass filter }; -#endif // HAL_PROXIMITY_ENABLED +// This class gives an easy way of making a temporary boundary, used for "sorting" distances. +// When unkown number of distances at various orientations are sent we store the least distance in the temporary boundary. +// After all the messages are received, we copy the contents of the temporary boundary and put it in the main 3-D boundary. +class AP_Proximity_Temp_Boundary +{ +public: + // constructor. This incorporates initialisation as well. + AP_Proximity_Temp_Boundary() { reset(); } + + // reset the temporary boundary. This fills in distances with FLT_MAX + void reset(); + + // add a distance to the temp boundary if it is shorter than any other provided distance since the last time the boundary was reset + // pitch and yaw are in degrees, distance is in meters + void add_distance(const AP_Proximity_Boundary_3D::Face &face, float pitch, float yaw, float distance); + void add_distance(const AP_Proximity_Boundary_3D::Face &face, float yaw, float distance) { add_distance(face, 0.0f, PID_TUNING_YAW, distance); } + + // fill the original 3D boundary with the contents of this temporary boundary + void update_3D_boundary(AP_Proximity_Boundary_3D &boundary); + +private: + + float _distances[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // distance to closest object within each sector and layer. Will start with FLT_MAX, and then be changed to a valid distance if needed + float _angle[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // yaw angle in degrees to closest object within each sector and layer + float _pitch[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // pitch angle in degrees to the closest object within each sector and layer +}; + +#endif // HAL_PROXIMITY_ENABLED \ No newline at end of file diff --git a/libraries/AP_Proximity/AP_Proximity_MAV.cpp b/libraries/AP_Proximity/AP_Proximity_MAV.cpp index 2982be7d0b..18f765a363 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_3D_MSG_TIMEOUT_MS 50 // boundary will be reset if OBSTACLE_DISTANCE_3D message does not 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 // update the state of the sensor void AP_Proximity_MAV::update(void) @@ -73,24 +73,39 @@ void AP_Proximity_MAV::handle_distance_sensor_msg(const mavlink_message_t &msg) // store distance to appropriate sector based on orientation field if (packet.orientation <= MAV_SENSOR_ROTATION_YAW_315) { + const uint32_t previous_sys_time = _last_update_ms; + _last_update_ms = AP_HAL::millis(); + + // time_diff will check if the new message arrived significantly later than the last message + const uint32_t time_diff = _last_update_ms - previous_sys_time; + + const uint32_t previous_msg_timestamp = _last_msg_update_timestamp_ms; + _last_msg_update_timestamp_ms = packet.time_boot_ms; + + // 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 + temp_boundary.reset(); + } + // store in meters + const float distance = packet.current_distance * 0.01f; const uint8_t sector = packet.orientation; // get the face for this sector const float yaw_angle_deg = sector * 45; const AP_Proximity_Boundary_3D::Face face = boundary.get_face(yaw_angle_deg); - // store in meters - const float distance = packet.current_distance * 0.01f; _distance_min = packet.min_distance * 0.01f; _distance_max = packet.max_distance * 0.01f; const bool in_range = distance <= _distance_max && distance >= _distance_min; if (in_range && !check_obstacle_near_ground(yaw_angle_deg, distance)) { - boundary.set_face_attributes(face, yaw_angle_deg, distance); + temp_boundary.add_distance(face, yaw_angle_deg, distance); // update OA database database_push(yaw_angle_deg, distance); - } else { - // reset distance for this face - boundary.reset_face(face); } - _last_update_ms = AP_HAL::millis(); } // store upward distance @@ -204,23 +219,27 @@ void AP_Proximity_MAV::handle_obstacle_distance_3d_msg(const mavlink_message_t & const uint32_t previous_sys_time = _last_update_ms; _last_update_ms = AP_HAL::millis(); - + // time_diff will check if the new message arrived significantly later than the last message const uint32_t time_diff = _last_update_ms - previous_sys_time; - const uint32_t previous_msg_timestamp = _last_3d_msg_update_ms; - _last_3d_msg_update_ms = packet.time_boot_ms; + const uint32_t previous_msg_timestamp = _last_msg_update_timestamp_ms; + _last_msg_update_timestamp_ms = packet.time_boot_ms; if (packet.frame != MAV_FRAME_BODY_FRD) { - // we do not support this frame of reference yet + // we do not support this frame of reference yet 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_3D_MSG_TIMEOUT_MS - if ((previous_msg_timestamp != _last_3d_msg_update_ms) || (time_diff > PROXIMITY_3D_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)) { // 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 + temp_boundary.reset(); } _distance_min = packet.min_distance; @@ -250,13 +269,7 @@ void AP_Proximity_MAV::handle_obstacle_distance_3d_msg(const mavlink_message_t & // allot to correct layer and sector based on calculated pitch and yaw const AP_Proximity_Boundary_3D::Face face = boundary.get_face(pitch, yaw); - float face_distance; - if (boundary.get_distance(face, face_distance) && (face_distance < obstacle.length())) { - // we already have a shorter distance in this layer and sector - return; - } - - boundary.set_face_attributes(face, yaw, pitch, obstacle.length()); + temp_boundary.add_distance(face, pitch, yaw, obstacle.length()); if (database_ready) { database_push(yaw, pitch, obstacle.length(),_last_update_ms, current_pos, body_to_ned); diff --git a/libraries/AP_Proximity/AP_Proximity_MAV.h b/libraries/AP_Proximity/AP_Proximity_MAV.h index 97482d5f9d..7d594cf884 100644 --- a/libraries/AP_Proximity/AP_Proximity_MAV.h +++ b/libraries/AP_Proximity/AP_Proximity_MAV.h @@ -33,9 +33,11 @@ private: // handle mavlink OBSTACLE_DISTANCE_3D messages void handle_obstacle_distance_3d_msg(const mavlink_message_t &msg); + AP_Proximity_Temp_Boundary temp_boundary; + // horizontal distance support uint32_t _last_update_ms; // system time of last mavlink message received - uint32_t _last_3d_msg_update_ms; // last stored OBSTACLE_DISTANCE_3D message timestamp + uint32_t _last_msg_update_timestamp_ms; // last stored mavlink message timestamp float _distance_max; // max range of sensor in meters float _distance_min; // min range of sensor in meters