Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Proximity: Fix Airsim Proximity sector update

Browse Source 
Distance equals least distance in that sector rather than last point sampled
This commit is contained in:
Rajat Singhal 2019-12-13 20:59:30 +05:30 committed by Randy Mackay
parent d065840b63
commit dccb95efce
2 changed files with 33 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
libraries/AP_Proximity/AP_Proximity_AirSimSITL.cpp
View File

@ -35,23 +35,40 @@ void AP_Proximity_AirSimSITL::update(void)
set_status(AP_Proximity::Status::Good); set_status(AP_Proximity::Status::Good);
memset(_distance_valid, 0, sizeof(_distance_valid)); memset(_distance_valid, 0, sizeof(_distance_valid));
memset(_angle, 0, sizeof(_angle));
memset(_distance, 0, sizeof(_distance));
for (uint16_t i=0; i<points.length; i++) { for (uint16_t i=0; i<points.length; i++) {
Vector3f &point = points.data[i]; Vector3f &point = points.data[i];
if (point.is_zero()) { if (point.is_zero()) {
continue; continue;
} }
float angle_deg = wrap_360(degrees(atan2f(-point.y, point.x))); const float angle_deg = wrap_360(degrees(atan2f(-point.y, point.x)));
uint16_t angle_rounded = uint16_t(angle_deg+0.5); const uint8_t sector = convert_angle_to_sector(angle_deg);
const uint8_t sector = convert_angle_to_sector(angle_rounded);
if (!_distance_valid[sector] || PROXIMITY_MAX_RANGE < _distance[sector]) { const Vector2f v = Vector2f(point.x, point.y);
_distance_valid[sector] = true; const float distance_m = v.length();
const Vector2f v = Vector2f(point.x, point.y);
_distance[sector] = v.length(); if (distance_m > distance_min()) {
_angle[sector] = angle_deg; if (_last_sector == sector) {
update_boundary_for_sector(sector, true); if (_distance_m_last > distance_m) {
_distance_m_last = distance_m;
_angle_deg_last = angle_deg;
}
} else {
// new sector started, previous one can be updated
_distance_valid[_last_sector] = true;
_angle[_last_sector] = _angle_deg_last;
_distance[_last_sector] = _distance_m_last;
// update boundary
update_boundary_for_sector(_last_sector, true);
// initialize new sector
_last_sector = sector;
_distance_m_last = INT16_MAX;
_angle_deg_last = angle_deg;
}
} else {
_distance_valid[sector] = false;
} }
} }

5
libraries/AP_Proximity/AP_Proximity_AirSimSITL.h
View File

@ -40,5 +40,10 @@ public:
private: private:
SITL::SITL *sitl = AP::sitl(); SITL::SITL *sitl = AP::sitl();
// sector related variables
float _angle_deg_last;
float _distance_m_last;
uint8_t _last_sector;
}; };
#endif // CONFIG_HAL_BOARD #endif // CONFIG_HAL_BOARD