aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Proximity: RPLidarA2 uses modified Boundary_3D interface

Browse Source
This commit is contained in:
Randy Mackay 2020-12-14 16:59:41 +09:00
parent babba91632
commit 518078c901
2 changed files with 27 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp
View File

@ -312,31 +312,30 @@ void AP_Proximity_RPLidarA2::parse_response_data()
#endif
_last_distance_received_ms = AP_HAL::millis();
if (!ignore_reading(angle_deg)) {
const boundary_location bnd_loc = boundary.get_sector(angle_deg);
const uint8_t sector = bnd_loc.sector;
if (distance_m > distance_min()) {
if (_last_sector == sector) {
if (_distance_m_last > distance_m) {
_distance_m_last = distance_m;
_angle_deg_last = angle_deg;
}
} else {
// a new sector started, the previous one can be updated now
// create a location packet
const boundary_location loc{_last_sector};
boundary.set_attributes(loc, _angle_deg_last, _distance_m_last);
// update boundary used for avoidance
boundary.update_boundary(loc);
// update OA database
database_push(_angle_deg_last, _distance_m_last);
const AP_Proximity_Boundary_3D::Face face = boundary.get_face(angle_deg);
// initialize the new sector
_last_sector = sector;
_distance_m_last = distance_m;
_angle_deg_last = angle_deg;
if (face != _last_face) {
// distance is for a new face, the previous one can be updated now
if (_last_distance_valid) {
boundary.set_face_attributes(face, _last_angle_deg, _last_distance_m);
} else {
// reset distance from last face
boundary.reset_face(face);
}
} else {
boundary.reset_sector(bnd_loc);
// initialize the new face
_last_face = face;
_last_distance_valid = false;
}
if (distance_m > distance_min()) {
// update shortest distance
if (!_last_distance_valid || (distance_m < _last_distance_m)) {
_last_distance_m = distance_m;
_last_distance_valid = true;
_last_angle_deg = angle_deg;
}
// update OA database
database_push(_last_angle_deg, _last_distance_m);
}
}
} else {

9
libraries/AP_Proximity/AP_Proximity_RPLidarA2.h
View File

@ -91,14 +91,15 @@ private:
// request related variables
enum ResponseType _response_type; ///< response from the lidar
enum rp_state _rp_state;
uint8_t _last_sector; ///< last sector requested
uint32_t _last_request_ms; ///< system time of last request
uint32_t _last_distance_received_ms; ///< system time of last distance measurement received from sensor
uint32_t _last_reset_ms;
// sector related variables
float _angle_deg_last;
float _distance_m_last;
// face related variables
AP_Proximity_Boundary_3D::Face _last_face;///< last face requested
float _last_angle_deg; ///< yaw angle (in degrees) of _last_distance_m
float _last_distance_m; ///< shortest distance for _last_face
bool _last_distance_valid; ///< true if _last_distance_m is valid
struct PACKED _sensor_scan {
uint8_t startbit : 1; ///< on the first revolution 1 else 0