Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Proximity: move sectors to backend 

This prepares for sectors to be re-used by the SITL driver
This commit is contained in:
Randy Mackay 2016-11-15 16:11:14 +09:00
parent f9845c93b1
commit f594a5a7d6
4 changed files with 72 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
libraries/AP_Proximity/AP_Proximity_Backend.cpp
View File

@ -28,8 +28,63 @@ AP_Proximity_Backend::AP_Proximity_Backend(AP_Proximity &_frontend, AP_Proximity
{
}
// 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;
}
// 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 &sector) 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;
}

17
libraries/AP_Proximity/AP_Proximity_Backend.h
View File

@ -18,6 +18,8 @@
#include <AP_HAL/AP_HAL.h>
#include "AP_Proximity.h"
#define PROXIMITY_SECTORS_MAX 12 // maximum number of sectors
class AP_Proximity_Backend
{
public:
@ -33,13 +35,26 @@ public:
// get distance in meters in a particular direction in degrees (0 is forward, clockwise)
// returns true on successful read and places distance in distance
virtual bool get_horizontal_distance(float angle_deg, float &distance) const = 0;
bool get_horizontal_distance(float angle_deg, float &distance) const;
protected:
// set status and update valid_count
void set_status(AP_Proximity::Proximity_Status status);
// find which sector a given angle falls into
bool convert_angle_to_sector(float angle_degrees, uint8_t &sector) const;
AP_Proximity &frontend;
AP_Proximity::Proximity_State &state; // reference to this instances state
// sectors
uint8_t _num_sectors = 8;
uint16_t _sector_middle_deg[PROXIMITY_SECTORS_MAX] = {0, 45, 90, 135, 180, 225, 270, 315, 0, 0, 0, 0}; // middle angle of each sector
uint8_t _sector_width_deg[PROXIMITY_SECTORS_MAX] = {45, 45, 45, 45, 45, 45, 45, 45, 0, 0, 0, 0}; // width (in degrees) of each sector
// sensor data
float _angle[PROXIMITY_SECTORS_MAX]; // angle to closest object within each sector
float _distance[PROXIMITY_SECTORS_MAX]; // distance to closest object within each sector
bool _distance_valid[PROXIMITY_SECTORS_MAX]; // true if a valid distance received for each sector
};

55
libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp
View File

@ -43,19 +43,6 @@ bool AP_Proximity_LightWareSF40C::detect(AP_SerialManager &serial_manager)
return serial_manager.find_serial(AP_SerialManager::SerialProtocol_Lidar360, 0) != nullptr;
}
// get distance in meters in a particular direction in degrees (0 is forward, angles increase in the clockwise direction)
bool AP_Proximity_LightWareSF40C::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;
}
// update the state of the sensor
void AP_Proximity_LightWareSF40C::update(void)
{
@ -376,45 +363,3 @@ void AP_Proximity_LightWareSF40C::clear_buffers()
element_num = 0;
memset(element_buf, 0, sizeof(element_buf));
}
bool AP_Proximity_LightWareSF40C::convert_angle_to_sector(float angle_degrees, uint8_t &sector) 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;
}

15
libraries/AP_Proximity/AP_Proximity_LightWareSF40C.h
View File

@ -3,8 +3,6 @@
#include "AP_Proximity.h"
#include "AP_Proximity_Backend.h"
#define PROXIMITY_SF40C_SECTORS_MAX 8 // maximum number of sectors
#define PROXIMITY_SF40C_SECTOR_WIDTH_DEG (360/PROXIMITY_SF40C_SECTORS_MAX) // angular width of each sector
#define PROXIMITY_SF40C_TIMEOUT_MS 200 // requests timeout after 0.2 seconds
class AP_Proximity_LightWareSF40C : public AP_Proximity_Backend
@ -17,10 +15,6 @@ public:
// static detection function
static bool detect(AP_SerialManager &serial_manager);
// get distance in meters in a particular direction in degrees (0 is forward, clockwise)
// returns true on successful read and places distance in distance
bool get_horizontal_distance(float angle_deg, float &distance) const;
// update state
void update(void);
@ -50,7 +44,6 @@ private:
bool check_for_reply();
bool process_reply();
void clear_buffers();
bool convert_angle_to_sector(float angle_degrees, uint8_t &sector) const;
// reply related variables
AP_HAL::UARTDriver *uart = nullptr;
@ -90,14 +83,8 @@ private:
uint16_t value;
} _sensor_status;
// sensor data
// sensor config
uint8_t _motor_speed; // motor speed as reported by lidar
uint8_t _motor_direction = 99; // motor direction as reported by lidar
int16_t _forward_direction = 999; // forward direction as reported by lidar
uint8_t _num_sectors = PROXIMITY_SF40C_SECTORS_MAX; // number of sectors we will search
uint16_t _sector_middle_deg[PROXIMITY_SF40C_SECTORS_MAX] = {0, 45, 90, 135, 180, 225, 270, 315}; // middle angle of each sector
uint8_t _sector_width_deg[PROXIMITY_SF40C_SECTORS_MAX] = {45, 45, 45, 45, 45, 45, 45, 45}; // width (in degrees) of each sector
float _angle[PROXIMITY_SF40C_SECTORS_MAX]; // angle to closest object within each sector
float _distance[PROXIMITY_SF40C_SECTORS_MAX]; // distance to closest object within each sector
bool _distance_valid[PROXIMITY_SF40C_SECTORS_MAX]; // true if a valid distance received for each sector
};