You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
101 lines
3.1 KiB
101 lines
3.1 KiB
/* |
|
This program is free software: you can redistribute it and/or modify |
|
it under the terms of the GNU General Public License as published by |
|
the Free Software Foundation, either version 3 of the License, or |
|
(at your option) any later version. |
|
|
|
This program is distributed in the hope that it will be useful, |
|
but WITHOUT ANY WARRANTY; without even the implied warranty of |
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
|
GNU General Public License for more details. |
|
|
|
You should have received a copy of the GNU General Public License |
|
along with this program. If not, see <http://www.gnu.org/licenses/>. |
|
*/ |
|
|
|
#include <AP_HAL/AP_HAL.h> |
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL |
|
#include "AP_Proximity_AirSimSITL.h" |
|
#include <stdio.h> |
|
|
|
extern const AP_HAL::HAL& hal; |
|
|
|
#define PROXIMITY_MAX_RANGE 100.0f |
|
#define PROXIMITY_ACCURACY 0.1f |
|
|
|
// update the state of the sensor |
|
void AP_Proximity_AirSimSITL::update(void) |
|
{ |
|
SITL::vector3f_array &points = sitl->state.scanner.points; |
|
if (points.length == 0) { |
|
set_status(AP_Proximity::Status::NoData); |
|
return; |
|
} |
|
|
|
set_status(AP_Proximity::Status::Good); |
|
|
|
memset(_distance_valid, 0, sizeof(_distance_valid)); |
|
|
|
for (uint16_t i=0; i<points.length; i++) { |
|
Vector3f &point = points.data[i]; |
|
if (point.is_zero()) { |
|
continue; |
|
} |
|
const float angle_deg = wrap_360(degrees(atan2f(point.y, point.x))); |
|
const uint8_t sector = convert_angle_to_sector(angle_deg); |
|
|
|
const Vector2f v = Vector2f(point.x, point.y); |
|
const float distance_m = v.length(); |
|
|
|
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 { |
|
// 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; |
|
} |
|
} |
|
|
|
#if 0 |
|
printf("npoints=%u\n", points.length); |
|
for (uint16_t i=0; i<PROXIMITY_NUM_SECTORS; i++) { |
|
printf("sector[%u] ang=%.1f dist=%.1f\n", i, _angle[i], _distance[i]); |
|
} |
|
#endif |
|
} |
|
|
|
// get maximum and minimum distances (in meters) of primary sensor |
|
float AP_Proximity_AirSimSITL::distance_max() const |
|
{ |
|
return PROXIMITY_MAX_RANGE; |
|
} |
|
|
|
float AP_Proximity_AirSimSITL::distance_min() const |
|
{ |
|
return 0.0f; |
|
} |
|
|
|
// get distance upwards in meters. returns true on success |
|
bool AP_Proximity_AirSimSITL::get_upward_distance(float &distance) const |
|
{ |
|
// we don't have an upward facing laser |
|
return false; |
|
} |
|
|
|
#endif // CONFIG_HAL_BOARD
|
|
|