mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-04 15:08:28 -04:00
115 lines
3.3 KiB
C++
115 lines
3.3 KiB
C++
/*
|
|
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_Common/AP_Common.h>
|
|
#include <AP_HAL/AP_HAL.h>
|
|
#include "AP_Proximity.h"
|
|
#include "AP_Proximity_Backend.h"
|
|
|
|
/*
|
|
base class constructor.
|
|
This incorporates initialisation as well.
|
|
*/
|
|
AP_Proximity_Backend::AP_Proximity_Backend(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state) :
|
|
frontend(_frontend),
|
|
state(_state)
|
|
{
|
|
}
|
|
|
|
// get distance in meters in a particular direction in degrees (0 is forward, angles increase in the clockwise direction)
|
|
bool AP_Proximity_Backend::get_horizontal_distance(float angle_deg, float &distance) const
|
|
{
|
|
uint8_t sector;
|
|
if (convert_angle_to_sector(angle_deg, sector)) {
|
|
if (_distance_valid[sector]) {
|
|
distance = _distance[sector];
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
// get distance and angle to closest object (used for pre-arm check)
|
|
// returns true on success, false if no valid readings
|
|
bool AP_Proximity_Backend::get_closest_object(float& angle_deg, float &distance) const
|
|
{
|
|
bool sector_found = false;
|
|
uint8_t sector = 0;
|
|
|
|
// check all sectors for shorter distance
|
|
for (uint8_t i=0; i<_num_sectors; i++) {
|
|
if (_distance_valid[i]) {
|
|
if (!sector_found || (_distance[i] < _distance[sector])) {
|
|
sector = i;
|
|
sector_found = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (sector_found) {
|
|
angle_deg = _angle[sector];
|
|
distance = _distance[sector];
|
|
}
|
|
return sector_found;
|
|
}
|
|
|
|
// set status and update valid count
|
|
void AP_Proximity_Backend::set_status(AP_Proximity::Proximity_Status status)
|
|
{
|
|
state.status = status;
|
|
}
|
|
|
|
bool AP_Proximity_Backend::convert_angle_to_sector(float angle_degrees, uint8_t §or) const
|
|
{
|
|
// sanity check angle
|
|
if (angle_degrees > 360.0f || angle_degrees < -180.0f) {
|
|
return false;
|
|
}
|
|
|
|
// convert to 0 ~ 360
|
|
if (angle_degrees < 0.0f) {
|
|
angle_degrees += 360.0f;
|
|
}
|
|
|
|
bool closest_found = false;
|
|
uint8_t closest_sector;
|
|
float closest_angle;
|
|
|
|
// search for which sector angle_degrees falls into
|
|
for (uint8_t i = 0; i < _num_sectors; i++) {
|
|
float angle_diff = fabsf(wrap_180(_sector_middle_deg[i] - angle_degrees));
|
|
|
|
// record if closest
|
|
if (!closest_found || angle_diff < closest_angle) {
|
|
closest_found = true;
|
|
closest_sector = i;
|
|
closest_angle = angle_diff;
|
|
}
|
|
|
|
if (fabsf(angle_diff) <= _sector_width_deg[i] / 2.0f) {
|
|
sector = i;
|
|
return true;
|
|
}
|
|
}
|
|
|
|
// angle_degrees might have been within a gap between sectors
|
|
if (closest_found) {
|
|
sector = closest_sector;
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|