AP_Proximity: move boundary to frontend

Co-authored-by: Rishabh <f20171602@hyderabad.bits-pilani.ac.in>
This commit is contained in:
Randy Mackay 2022-07-07 12:35:43 +09:00 committed by Andrew Tridgell
parent eff86c88ab
commit 7432a20394
16 changed files with 87 additions and 189 deletions

View File

@ -130,9 +130,14 @@ void AP_Proximity::update()
continue; continue;
} }
drivers[i]->update(); drivers[i]->update();
drivers[i]->boundary_3D_checks();
} }
// set boundary cutoff freq for low pass filter
boundary.set_filter_freq(get_filter_freq());
// check if any face has valid distance when it should not
boundary.check_face_timeout();
// work out primary instance - first sensor returning good data // work out primary instance - first sensor returning good data
for (int8_t i=num_instances-1; i>=0; i--) { for (int8_t i=num_instances-1; i>=0; i--) {
if (drivers[i] != nullptr && (state[i].status == Status::Good)) { if (drivers[i] != nullptr && (state[i].status == Status::Good)) {
@ -207,90 +212,46 @@ float AP_Proximity::distance_min() const
// get distances in 8 directions. used for sending distances to ground station // get distances in 8 directions. used for sending distances to ground station
bool AP_Proximity::get_horizontal_distances(Proximity_Distance_Array &prx_dist_array) const bool AP_Proximity::get_horizontal_distances(Proximity_Distance_Array &prx_dist_array) const
{ {
if (!valid_instance(primary_instance)) { Proximity_Distance_Array prx_filt_dist_array; // unused
return false; return boundary.get_layer_distances(PROXIMITY_MIDDLE_LAYER, distance_max(), prx_dist_array, prx_filt_dist_array);
}
// get distances from backend
return drivers[primary_instance]->get_horizontal_distances(prx_dist_array);
}
// get number of layers.
uint8_t AP_Proximity::get_num_layers() const
{
if (!valid_instance(primary_instance)) {
return 0;
}
return drivers[primary_instance]->get_num_layers();
}
// get raw and filtered distances in 8 directions per layer. used for logging
bool AP_Proximity::get_active_layer_distances(uint8_t layer, AP_Proximity::Proximity_Distance_Array &prx_dist_array, AP_Proximity::Proximity_Distance_Array &prx_filt_dist_array) const
{
if (!valid_instance(primary_instance)) {
return false;
}
// get distances from backend
return drivers[primary_instance]->get_active_layer_distances(layer, prx_dist_array, prx_filt_dist_array);
} }
// get total number of obstacles, used in GPS based Simple Avoidance // get total number of obstacles, used in GPS based Simple Avoidance
uint8_t AP_Proximity::get_obstacle_count() const uint8_t AP_Proximity::get_obstacle_count() const
{ {
if (!valid_instance(primary_instance)) { return boundary.get_obstacle_count();
return 0;
}
return drivers[primary_instance]->get_obstacle_count();
} }
// get vector to obstacle based on obstacle_num passed, used in GPS based Simple Avoidance // get vector to obstacle based on obstacle_num passed, used in GPS based Simple Avoidance
bool AP_Proximity::get_obstacle(uint8_t obstacle_num, Vector3f& vec_to_obstacle) const bool AP_Proximity::get_obstacle(uint8_t obstacle_num, Vector3f& vec_to_obstacle) const
{ {
if (!valid_instance(primary_instance)) { return boundary.get_obstacle(obstacle_num, vec_to_obstacle);
return false;
}
return drivers[primary_instance]->get_obstacle(obstacle_num, vec_to_obstacle);
} }
// returns shortest distance to "obstacle_num" obstacle, from a line segment formed between "seg_start" and "seg_end" // returns shortest distance to "obstacle_num" obstacle, from a line segment formed between "seg_start" and "seg_end"
// used in GPS based Simple Avoidance // returns FLT_MAX if it's an invalid instance.
bool AP_Proximity::closest_point_from_segment_to_obstacle(uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const bool AP_Proximity::closest_point_from_segment_to_obstacle(uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const
{ {
if (!valid_instance(primary_instance)) { return boundary.closest_point_from_segment_to_obstacle(obstacle_num , seg_start, seg_end, closest_point);
return false;
}
return drivers[primary_instance]->closest_point_from_segment_to_obstacle(obstacle_num, seg_start, seg_end, closest_point);
} }
// get distance and angle to closest object (used for pre-arm check) // get distance and angle to closest object (used for pre-arm check)
// returns true on success, false if no valid readings // returns true on success, false if no valid readings
bool AP_Proximity::get_closest_object(float& angle_deg, float &distance) const bool AP_Proximity::get_closest_object(float& angle_deg, float &distance) const
{ {
if (!valid_instance(primary_instance)) { return boundary.get_closest_object(angle_deg, distance);
return false;
}
// get closest object from backend
return drivers[primary_instance]->get_closest_object(angle_deg, distance);
} }
// get number of objects, used for non-GPS avoidance // get number of objects, angle and distance - used for non-GPS avoidance
uint8_t AP_Proximity::get_object_count() const uint8_t AP_Proximity::get_object_count() const
{ {
if (!valid_instance(primary_instance)) { return boundary.get_horizontal_object_count();
return 0;
}
// get count from backend
return drivers[primary_instance]->get_horizontal_object_count();
} }
// get an object's angle and distance, used for non-GPS avoidance // get number of objects, angle and distance - used for non-GPS avoidance
// returns false if no angle or distance could be returned for some reason
bool AP_Proximity::get_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const bool AP_Proximity::get_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const
{ {
if (!valid_instance(primary_instance)) { return boundary.get_horizontal_object_angle_and_distance(object_number, angle_deg, distance);
return false;
}
// get angle and distance from backend
return drivers[primary_instance]->get_horizontal_object_angle_and_distance(object_number, angle_deg, distance);
} }
// handle mavlink DISTANCE_SENSOR messages // handle mavlink DISTANCE_SENSOR messages
@ -355,8 +316,8 @@ void AP_Proximity::log()
Proximity_Distance_Array dist_array{}; // raw distances stored here Proximity_Distance_Array dist_array{}; // raw distances stored here
Proximity_Distance_Array filt_dist_array{}; //filtered distances stored here Proximity_Distance_Array filt_dist_array{}; //filtered distances stored here
auto &logger { AP::logger() }; auto &logger { AP::logger() };
for (uint8_t i = 0; i < get_num_layers(); i++) { for (uint8_t i = 0; i < boundary.get_num_layers(); i++) {
const bool active = get_active_layer_distances(i, dist_array, filt_dist_array); const bool active = boundary.get_layer_distances(i, distance_max(), dist_array, filt_dist_array);
if (!active) { if (!active) {
// nothing on this layer // nothing on this layer
continue; continue;

View File

@ -23,9 +23,9 @@
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
#include <GCS_MAVLink/GCS_MAVLink.h> #include <GCS_MAVLink/GCS_MAVLink.h>
#include "AP_Proximity_Params.h" #include "AP_Proximity_Params.h"
#include "AP_Proximity_Boundary_3D.h"
#define PROXIMITY_MAX_INSTANCES 2 // Maximum number of proximity sensor instances available on this platform #define PROXIMITY_MAX_INSTANCES 2 // Maximum number of proximity sensor instances available on this platform
#define PROXIMITY_MAX_DIRECTION 8
#define PROXIMITY_SENSOR_ID_START 10 #define PROXIMITY_SENSOR_ID_START 10
class AP_Proximity_Backend; class AP_Proximity_Backend;
@ -93,27 +93,9 @@ public:
// 3D boundary related methods // 3D boundary related methods
// //
// structure holding distances in PROXIMITY_MAX_DIRECTION directions. used for sending distances to ground station
struct Proximity_Distance_Array {
uint8_t orientation[PROXIMITY_MAX_DIRECTION]; // orientation (i.e. rough direction) of the distance (see MAV_SENSOR_ORIENTATION)
float distance[PROXIMITY_MAX_DIRECTION]; // distance in meters
bool valid(uint8_t offset) const {
// returns true if the distance stored at offset is valid
return (offset < 8 && (offset_valid & (1U<<offset)));
};
uint8_t offset_valid; // bitmask
};
// get distances in PROXIMITY_MAX_DIRECTION directions. used for sending distances to ground station // get distances in PROXIMITY_MAX_DIRECTION directions. used for sending distances to ground station
bool get_horizontal_distances(Proximity_Distance_Array &prx_dist_array) const; bool get_horizontal_distances(Proximity_Distance_Array &prx_dist_array) const;
// get number of layers in the 3D boundary
uint8_t get_num_layers() const;
// get raw and filtered distances in 8 directions per layer. used for logging
bool get_active_layer_distances(uint8_t layer, AP_Proximity::Proximity_Distance_Array &prx_dist_array, AP_Proximity::Proximity_Distance_Array &prx_filt_dist_array) const;
// get total number of obstacles, used in GPS based Simple Avoidance // get total number of obstacles, used in GPS based Simple Avoidance
uint8_t get_obstacle_count() const; uint8_t get_obstacle_count() const;
@ -170,6 +152,9 @@ public:
static AP_Proximity *get_singleton(void) { return _singleton; }; static AP_Proximity *get_singleton(void) { return _singleton; };
// 3D boundary
AP_Proximity_Boundary_3D boundary;
protected: protected:
// parameters for backends // parameters for backends

View File

@ -37,7 +37,7 @@ void AP_Proximity_AirSimSITL::update(void)
set_status(AP_Proximity::Status::Good); set_status(AP_Proximity::Status::Good);
// reset all faces to default so that it can be filled with the fresh lidar data // reset all faces to default so that it can be filled with the fresh lidar data
boundary.reset(); frontend.boundary.reset();
// precalculate sq of min distance // precalculate sq of min distance
const float distance_min_sq = sq(distance_min()); const float distance_min_sq = sq(distance_min());
@ -62,7 +62,7 @@ void AP_Proximity_AirSimSITL::update(void)
// add distance to the 3D boundary // add distance to the 3D boundary
const float yaw_angle_deg = wrap_360(degrees(atan2f(point.y, point.x))); const float yaw_angle_deg = wrap_360(degrees(atan2f(point.y, point.x)));
const AP_Proximity_Boundary_3D::Face face = boundary.get_face(yaw_angle_deg); const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(yaw_angle_deg);
// store the min distance in each face in a temp boundary // store the min distance in each face in a temp boundary
temp_boundary.add_distance(face, yaw_angle_deg, safe_sqrt(distance_sq)); temp_boundary.add_distance(face, yaw_angle_deg, safe_sqrt(distance_sq));
@ -77,7 +77,7 @@ void AP_Proximity_AirSimSITL::update(void)
} }
} }
// copy temp boundary to real boundary // copy temp boundary to real boundary
temp_boundary.update_3D_boundary(boundary); temp_boundary.update_3D_boundary(state.instance, frontend.boundary);
} }
// get maximum and minimum distances (in meters) of primary sensor // get maximum and minimum distances (in meters) of primary sensor

View File

@ -38,39 +38,12 @@ AP_Proximity_Backend::AP_Proximity_Backend(AP_Proximity& _frontend, AP_Proximity
static_assert(PROXIMITY_MAX_DIRECTION <= 8, static_assert(PROXIMITY_MAX_DIRECTION <= 8,
"get_horizontal_distances assumes 8-bits is enough for validity bitmask"); "get_horizontal_distances assumes 8-bits is enough for validity bitmask");
// get distances in PROXIMITY_MAX_DIRECTION directions horizontally. used for sending distances to ground station
bool AP_Proximity_Backend::get_horizontal_distances(AP_Proximity::Proximity_Distance_Array &prx_dist_array) const
{
AP_Proximity::Proximity_Distance_Array prx_filt_dist_array; // unused
return boundary.get_layer_distances(PROXIMITY_MIDDLE_LAYER, distance_max(), prx_dist_array, prx_filt_dist_array);
}
// get distances in PROXIMITY_MAX_DIRECTION directions at a layer. used for logging
bool AP_Proximity_Backend::get_active_layer_distances(uint8_t layer, AP_Proximity::Proximity_Distance_Array &prx_dist_array, AP_Proximity::Proximity_Distance_Array &prx_filt_dist_array) const
{
return boundary.get_layer_distances(layer, distance_max(), prx_dist_array, prx_filt_dist_array);
}
// set status and update valid count // set status and update valid count
void AP_Proximity_Backend::set_status(AP_Proximity::Status status) void AP_Proximity_Backend::set_status(AP_Proximity::Status status)
{ {
state.status = status; state.status = status;
} }
// timeout faces that have not received data recently and update filter frequencies
void AP_Proximity_Backend::boundary_3D_checks()
{
// set the cutoff freq for low pass filter
boundary.set_filter_freq(frontend.get_filter_freq());
// check if any face has valid distance when it should not
const uint32_t now_ms = AP_HAL::millis();
// run this check every PROXIMITY_BOUNDARY_3D_TIMEOUT_MS
if ((now_ms - _last_timeout_check_ms) > PROXIMITY_BOUNDARY_3D_TIMEOUT_MS) {
_last_timeout_check_ms = now_ms;
boundary.check_face_timeout();
}
}
// correct an angle (in degrees) based on the orientation and yaw correction parameters // correct an angle (in degrees) based on the orientation and yaw correction parameters
float AP_Proximity_Backend::correct_angle_for_orientation(float angle_degrees) const float AP_Proximity_Backend::correct_angle_for_orientation(float angle_degrees) const
{ {

View File

@ -18,11 +18,9 @@
#if HAL_PROXIMITY_ENABLED #if HAL_PROXIMITY_ENABLED
#include <AP_Common/AP_Common.h> #include <AP_Common/AP_Common.h>
#include "AP_Proximity_Boundary_3D.h"
#define PROXIMITY_GND_DETECT_THRESHOLD 1.0f // set ground detection threshold to be 1 meters #define PROXIMITY_GND_DETECT_THRESHOLD 1.0f // set ground detection threshold to be 1 meters
#define PROXIMITY_ALT_DETECT_TIMEOUT_MS 500 // alt readings should arrive within this much time #define PROXIMITY_ALT_DETECT_TIMEOUT_MS 500 // alt readings should arrive within this much time
#define PROXIMITY_BOUNDARY_3D_TIMEOUT_MS 750 // we should check the 3D boundary faces after every this many ms
class AP_Proximity_Backend class AP_Proximity_Backend
{ {
@ -37,9 +35,6 @@ public:
// update the state structure // update the state structure
virtual void update() = 0; virtual void update() = 0;
// timeout faces that have not received data recently and update filter frequencies
void boundary_3D_checks();
// get maximum and minimum distances (in meters) of sensor // get maximum and minimum distances (in meters) of sensor
virtual float distance_max() const = 0; virtual float distance_max() const = 0;
virtual float distance_min() const = 0; virtual float distance_min() const = 0;
@ -50,33 +45,6 @@ public:
// handle mavlink DISTANCE_SENSOR messages // handle mavlink DISTANCE_SENSOR messages
virtual void handle_msg(const mavlink_message_t &msg) {} virtual void handle_msg(const mavlink_message_t &msg) {}
// get total number of obstacles, used in GPS based Simple Avoidance
uint8_t get_obstacle_count() { return boundary.get_obstacle_count(); }
// get vector to obstacle based on obstacle_num passed, used in GPS based Simple Avoidance
bool get_obstacle(uint8_t obstacle_num, Vector3f& vec_to_obstacle) const { return boundary.get_obstacle(obstacle_num, vec_to_obstacle); }
// returns shortest distance to "obstacle_num" obstacle, from a line segment formed between "seg_start" and "seg_end"
// used in GPS based Simple Avoidance
bool closest_point_from_segment_to_obstacle(const uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const { return boundary.closest_point_from_segment_to_obstacle(obstacle_num , seg_start, seg_end, closest_point); }
// get distance and angle to closest object (used for pre-arm check)
// returns true on success, false if no valid readings
bool get_closest_object(float& angle_deg, float &distance) const { return boundary.get_closest_object(angle_deg, distance); }
// get number of objects, angle and distance - used for non-GPS avoidance
uint8_t get_horizontal_object_count() const {return boundary.get_horizontal_object_count(); }
bool get_horizontal_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const { return boundary.get_horizontal_object_angle_and_distance(object_number, angle_deg, distance); }
// get distances in 8 directions. used for sending distances to ground station
bool get_horizontal_distances(AP_Proximity::Proximity_Distance_Array &prx_dist_array) const;
// get raw and filtered distances in 8 directions per layer. used for logging
bool get_active_layer_distances(uint8_t layer, AP_Proximity::Proximity_Distance_Array &prx_dist_array, AP_Proximity::Proximity_Distance_Array &prx_filt_dist_array) const;
// get number of layers
uint8_t get_num_layers() const { return boundary.get_num_layers(); }
// store rangefinder values // store rangefinder values
void set_rangefinder_alt(bool use, bool healthy, float alt_cm); void set_rangefinder_alt(bool use, bool healthy, float alt_cm);
@ -111,8 +79,6 @@ protected:
}; };
static void database_push(float angle, float pitch, float distance, uint32_t timestamp_ms, const Vector3f &current_pos, const Matrix3f &body_to_ned); static void database_push(float angle, float pitch, float distance, uint32_t timestamp_ms, const Vector3f &current_pos, const Matrix3f &body_to_ned);
uint32_t _last_timeout_check_ms; // time when boundary was checked for non-updated valid faces
// used for ground detection // used for ground detection
uint32_t _last_downward_update_ms; uint32_t _last_downward_update_ms;
bool _rangefinder_use; bool _rangefinder_use;
@ -122,9 +88,6 @@ protected:
AP_Proximity &frontend; AP_Proximity &frontend;
AP_Proximity::Proximity_State &state; // reference to this instances state AP_Proximity::Proximity_State &state; // reference to this instances state
AP_Proximity_Params &params; // parameters for this backend AP_Proximity_Params &params; // parameters for this backend
// Methods to manipulate 3D boundary in this class
AP_Proximity_Boundary_3D boundary;
}; };
#endif // HAL_PROXIMITY_ENABLED #endif // HAL_PROXIMITY_ENABLED

View File

@ -15,8 +15,7 @@
#include "AP_Proximity_Boundary_3D.h" #include "AP_Proximity_Boundary_3D.h"
#if HAL_PROXIMITY_ENABLED #define PROXIMITY_BOUNDARY_3D_TIMEOUT_MS 750 // we should check the 3D boundary faces after this many ms
#include "AP_Proximity_Backend.h"
/* /*
Constructor. Constructor.
@ -185,6 +184,13 @@ void AP_Proximity_Boundary_3D::reset_face(const Face &face)
// check if a face has valid distance even if it was updated a long time back // check if a face has valid distance even if it was updated a long time back
void AP_Proximity_Boundary_3D::check_face_timeout() void AP_Proximity_Boundary_3D::check_face_timeout()
{ {
// exit immediately if already checked recently
const uint32_t now_ms = AP_HAL::millis();
if ((now_ms - _last_check_face_timeout_ms) < PROXIMITY_BOUNDARY_3D_TIMEOUT_MS) {
return;
}
_last_check_face_timeout_ms = now_ms;
for (uint8_t layer=0; layer < PROXIMITY_NUM_LAYERS; layer++) { for (uint8_t layer=0; layer < PROXIMITY_NUM_LAYERS; layer++) {
for (uint8_t sector=0; sector < PROXIMITY_NUM_SECTORS; sector++) { for (uint8_t sector=0; sector < PROXIMITY_NUM_SECTORS; sector++) {
if (_distance_valid[layer][sector]) { if (_distance_valid[layer][sector]) {
@ -363,7 +369,7 @@ bool AP_Proximity_Boundary_3D::get_filtered_distance(const Face &face, float &di
} }
// Get raw and filtered distances in 8 directions per layer // Get raw and filtered distances in 8 directions per layer
bool AP_Proximity_Boundary_3D::get_layer_distances(uint8_t layer_number, float dist_max, AP_Proximity::Proximity_Distance_Array &prx_dist_array, AP_Proximity::Proximity_Distance_Array &prx_filt_dist_array) const bool AP_Proximity_Boundary_3D::get_layer_distances(uint8_t layer_number, float dist_max, Proximity_Distance_Array &prx_dist_array, Proximity_Distance_Array &prx_filt_dist_array) const
{ {
// cycle through all sectors filling in distances and orientations // cycle through all sectors filling in distances and orientations
// see MAV_SENSOR_ORIENTATION for orientations (0 = forward, 1 = 45 degree clockwise from north, etc) // see MAV_SENSOR_ORIENTATION for orientations (0 = forward, 1 = 45 degree clockwise from north, etc)
@ -423,4 +429,3 @@ void AP_Proximity_Temp_Boundary::update_3D_boundary(AP_Proximity_Boundary_3D &bo
} }
} }
#endif // HAL_PROXIMITY_ENABLED

View File

@ -15,10 +15,8 @@
#pragma once #pragma once
#include "AP_Proximity.h" #include <AP_Common/AP_Common.h>
#include <AP_Math/AP_Math.h>
#if HAL_PROXIMITY_ENABLED
#include <Filter/LowPassFilter.h> #include <Filter/LowPassFilter.h>
#define PROXIMITY_NUM_SECTORS 8 // number of sectors #define PROXIMITY_NUM_SECTORS 8 // number of sectors
@ -31,6 +29,19 @@
#define PROXIMITY_FILT_RESET_TIME 1000 // reset filter if last distance was pushed more than this many ms away #define PROXIMITY_FILT_RESET_TIME 1000 // reset filter if last distance was pushed more than this many ms away
#define PROXIMITY_FACE_RESET_MS 1000 // face will be reset if not updated within this many ms #define PROXIMITY_FACE_RESET_MS 1000 // face will be reset if not updated within this many ms
// structure holding distances in PROXIMITY_MAX_DIRECTION directions. used for sending distances to ground station
#define PROXIMITY_MAX_DIRECTION 8
struct Proximity_Distance_Array {
uint8_t orientation[PROXIMITY_MAX_DIRECTION]; // orientation (i.e. rough direction) of the distance (see MAV_SENSOR_ORIENTATION)
float distance[PROXIMITY_MAX_DIRECTION]; // distance in meters
bool valid(uint8_t offset) const {
// returns true if the distance stored at offset is valid
return (offset < 8 && (offset_valid & (1U<<offset)));
};
uint8_t offset_valid; // bitmask
};
class AP_Proximity_Boundary_3D class AP_Proximity_Boundary_3D
{ {
public: public:
@ -112,7 +123,7 @@ public:
uint8_t get_num_layers() const { return PROXIMITY_NUM_LAYERS; } uint8_t get_num_layers() const { return PROXIMITY_NUM_LAYERS; }
// get raw and filtered distances in 8 directions per layer. // get raw and filtered distances in 8 directions per layer.
bool get_layer_distances(uint8_t layer_number, float dist_max, AP_Proximity::Proximity_Distance_Array &prx_dist_array, AP_Proximity::Proximity_Distance_Array &prx_filt_dist_array) const; bool get_layer_distances(uint8_t layer_number, float dist_max, Proximity_Distance_Array &prx_dist_array, Proximity_Distance_Array &prx_filt_dist_array) const;
// pass down filter cut-off freq from params // pass down filter cut-off freq from params
void set_filter_freq(float filt_freq) { _filter_freq = filt_freq; } void set_filter_freq(float filt_freq) { _filter_freq = filt_freq; }
@ -161,6 +172,7 @@ private:
uint32_t _last_update_ms[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // time when distance was last updated uint32_t _last_update_ms[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // time when distance was last updated
LowPassFilterFloat _filtered_distance[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // low pass filter LowPassFilterFloat _filtered_distance[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // low pass filter
float _filter_freq; // cutoff freq of low pass filter float _filter_freq; // cutoff freq of low pass filter
uint32_t _last_check_face_timeout_ms; // system time to throttle check_face_timeout method
}; };
// This class gives an easy way of making a temporary boundary, used for "sorting" distances. // This class gives an easy way of making a temporary boundary, used for "sorting" distances.
@ -189,5 +201,3 @@ private:
float _angle[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // yaw angle in degrees to closest object within each sector and layer float _angle[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // yaw angle in degrees to closest object within each sector and layer
float _pitch[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // pitch angle in degrees to the closest object within each sector and layer float _pitch[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // pitch angle in degrees to the closest object within each sector and layer
}; };
#endif // HAL_PROXIMITY_ENABLED

View File

@ -118,7 +118,7 @@ bool AP_Proximity_Cygbot_D1::parse_byte(uint8_t data)
// checksum is valid, parse payload // checksum is valid, parse payload
_last_distance_received_ms = AP_HAL::millis(); _last_distance_received_ms = AP_HAL::millis();
parse_payload(); parse_payload();
_temp_boundary.update_3D_boundary(boundary); _temp_boundary.update_3D_boundary(frontend.boundary);
reset(); reset();
return true; return true;
} }
@ -149,7 +149,8 @@ void AP_Proximity_Cygbot_D1::parse_payload()
continue; continue;
} }
// convert angle to face // convert angle to face
const AP_Proximity_Boundary_3D::Face face = boundary.get_face(corrected_angle); const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(corrected_angle);
// push face to temp boundary // push face to temp boundary
_temp_boundary.add_distance(face, corrected_angle, distance_m); _temp_boundary.add_distance(face, corrected_angle, distance_m);
// push to OA_DB // push to OA_DB

View File

@ -320,16 +320,16 @@ void AP_Proximity_LightWareSF40C::process_message()
const uint16_t idx = 14 + (i * 2); const uint16_t idx = 14 + (i * 2);
const int16_t dist_cm = (int16_t)buff_to_uint16(_msg.payload[idx], _msg.payload[idx+1]); const int16_t dist_cm = (int16_t)buff_to_uint16(_msg.payload[idx], _msg.payload[idx+1]);
const float angle_deg = wrap_360((point_start_index + i) * angle_inc_deg * angle_sign + angle_correction); const float angle_deg = wrap_360((point_start_index + i) * angle_inc_deg * angle_sign + angle_correction);
const AP_Proximity_Boundary_3D::Face face = boundary.get_face(angle_deg); const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(angle_deg);
// if point is on a new face then finish off previous face // if point is on a new face then finish off previous face
if (face != _face) { if (face != _face) {
// update boundary used for avoidance // update boundary used for avoidance
if (_face_distance_valid) { if (_face_distance_valid) {
boundary.set_face_attributes(_face, _face_yaw_deg, _face_distance); frontend.boundary.set_face_attributes(_face, _face_yaw_deg, _face_distance);
} else { } else {
// mark previous face invalid // mark previous face invalid
boundary.reset_face(_face); frontend.boundary.reset_face(_face);
} }
// init for new face // init for new face
_face = face; _face = face;

View File

@ -141,13 +141,13 @@ void AP_Proximity_LightWareSF45B::process_message()
// if distance is from a new face then update distance, angle and boundary for previous face // if distance is from a new face then update distance, angle and boundary for previous face
// get face from 3D boundary based on yaw angle to the object // get face from 3D boundary based on yaw angle to the object
const AP_Proximity_Boundary_3D::Face face = boundary.get_face(angle_deg); const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(angle_deg);
if (face != _face) { if (face != _face) {
if (_face_distance_valid) { if (_face_distance_valid) {
boundary.set_face_attributes(_face, _face_yaw_deg, _face_distance); frontend.boundary.set_face_attributes(_face, _face_yaw_deg, _face_distance);
} else { } else {
// mark previous face invalid // mark previous face invalid
boundary.reset_face(_face); frontend.boundary.reset_face(_face);
} }
// record updated face // record updated face
_face = face; _face = face;

View File

@ -86,7 +86,7 @@ void AP_Proximity_MAV::handle_distance_sensor_msg(const mavlink_message_t &msg)
// provided we got the new obstacle in less than PROXIMITY_TIMESTAMP_MSG_TIMEOUT_MS // provided we got the new obstacle in less than PROXIMITY_TIMESTAMP_MSG_TIMEOUT_MS
if ((previous_msg_timestamp != _last_msg_update_timestamp_ms) || (time_diff > PROXIMITY_TIMESTAMP_MSG_TIMEOUT_MS)) { if ((previous_msg_timestamp != _last_msg_update_timestamp_ms) || (time_diff > PROXIMITY_TIMESTAMP_MSG_TIMEOUT_MS)) {
// push data from temp boundary to the main 3-D proximity boundary // push data from temp boundary to the main 3-D proximity boundary
temp_boundary.update_3D_boundary(boundary); temp_boundary.update_3D_boundary(frontend.boundary);
// clear temp boundary for new data // clear temp boundary for new data
temp_boundary.reset(); temp_boundary.reset();
} }
@ -95,7 +95,7 @@ void AP_Proximity_MAV::handle_distance_sensor_msg(const mavlink_message_t &msg)
const uint8_t sector = packet.orientation; const uint8_t sector = packet.orientation;
// get the face for this sector // get the face for this sector
const float yaw_angle_deg = sector * 45; const float yaw_angle_deg = sector * 45;
const AP_Proximity_Boundary_3D::Face face = boundary.get_face(yaw_angle_deg); const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(yaw_angle_deg);
_distance_min = packet.min_distance * 0.01f; _distance_min = packet.min_distance * 0.01f;
_distance_max = packet.max_distance * 0.01f; _distance_max = packet.max_distance * 0.01f;
const bool in_range = distance <= _distance_max && distance >= _distance_min; const bool in_range = distance <= _distance_max && distance >= _distance_min;
@ -158,7 +158,7 @@ void AP_Proximity_MAV::handle_obstacle_distance_msg(const mavlink_message_t &msg
bool face_distance_valid = false; bool face_distance_valid = false;
// reset this boundary to fill with new data // reset this boundary to fill with new data
boundary.reset(); frontend.boundary.reset();
// iterate over message's sectors // iterate over message's sectors
for (uint8_t j = 0; j < total_distances; j++) { for (uint8_t j = 0; j < total_distances; j++) {
@ -174,13 +174,13 @@ void AP_Proximity_MAV::handle_obstacle_distance_msg(const mavlink_message_t &msg
} }
// get face for this latest reading // get face for this latest reading
AP_Proximity_Boundary_3D::Face latest_face = boundary.get_face(mid_angle); AP_Proximity_Boundary_3D::Face latest_face = frontend.boundary.get_face(mid_angle);
if (latest_face != face) { if (latest_face != face) {
// store previous face // store previous face
if (face_distance_valid) { if (face_distance_valid) {
boundary.set_face_attributes(face, face_yaw_deg, face_distance); frontend.boundary.set_face_attributes(face, face_yaw_deg, face_distance);
} else { } else {
boundary.reset_face(face); frontend.boundary.reset_face(face);
} }
// init for latest face // init for latest face
face = latest_face; face = latest_face;
@ -202,9 +202,9 @@ void AP_Proximity_MAV::handle_obstacle_distance_msg(const mavlink_message_t &msg
// process the last face // process the last face
if (face_distance_valid) { if (face_distance_valid) {
boundary.set_face_attributes(face, face_yaw_deg, face_distance); frontend.boundary.set_face_attributes(face, face_yaw_deg, face_distance);
} else { } else {
boundary.reset_face(face); frontend.boundary.reset_face(face);
} }
return; return;
} }
@ -231,7 +231,7 @@ void AP_Proximity_MAV::handle_obstacle_distance_3d_msg(const mavlink_message_t &
if ((previous_msg_timestamp != _last_msg_update_timestamp_ms) || (time_diff > PROXIMITY_TIMESTAMP_MSG_TIMEOUT_MS)) { if ((previous_msg_timestamp != _last_msg_update_timestamp_ms) || (time_diff > PROXIMITY_TIMESTAMP_MSG_TIMEOUT_MS)) {
// push data from temp boundary to the main 3-D proximity boundary because a new timestamp has arrived // push data from temp boundary to the main 3-D proximity boundary because a new timestamp has arrived
temp_boundary.update_3D_boundary(boundary); temp_boundary.update_3D_boundary(frontend.boundary);
// clear temp boundary for new data // clear temp boundary for new data
temp_boundary.reset(); temp_boundary.reset();
} }
@ -263,7 +263,7 @@ void AP_Proximity_MAV::handle_obstacle_distance_3d_msg(const mavlink_message_t &
} }
// allot to correct layer and sector based on calculated pitch and yaw // allot to correct layer and sector based on calculated pitch and yaw
const AP_Proximity_Boundary_3D::Face face = boundary.get_face(pitch, yaw); const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(pitch, yaw);
temp_boundary.add_distance(face, pitch, yaw, obstacle.length()); temp_boundary.add_distance(face, pitch, yaw, obstacle.length());
if (database_ready) { if (database_ready) {

View File

@ -314,15 +314,15 @@ void AP_Proximity_RPLidarA2::parse_response_data()
#endif #endif
_last_distance_received_ms = AP_HAL::millis(); _last_distance_received_ms = AP_HAL::millis();
if (!ignore_reading(angle_deg, distance_m)) { if (!ignore_reading(angle_deg, distance_m)) {
const AP_Proximity_Boundary_3D::Face face = boundary.get_face(angle_deg); const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(angle_deg);
if (face != _last_face) { if (face != _last_face) {
// distance is for a new face, the previous one can be updated now // distance is for a new face, the previous one can be updated now
if (_last_distance_valid) { if (_last_distance_valid) {
boundary.set_face_attributes(_last_face, _last_angle_deg, _last_distance_m); frontend.boundary.set_face_attributes(_last_face, _last_angle_deg, _last_distance_m);
} else { } else {
// reset distance from last face // reset distance from last face
boundary.reset_face(face); frontend.boundary.reset_face(face);
} }
// initialize the new face // initialize the new face

View File

@ -45,17 +45,17 @@ void AP_Proximity_RangeFinder::update(void)
if (sensor->orientation() <= ROTATION_YAW_315) { if (sensor->orientation() <= ROTATION_YAW_315) {
const uint8_t sector = (uint8_t)sensor->orientation(); const uint8_t sector = (uint8_t)sensor->orientation();
const float angle = sector * 45; const float angle = sector * 45;
const AP_Proximity_Boundary_3D::Face face = boundary.get_face(angle); const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(angle);
// distance in meters // distance in meters
const float distance = sensor->distance(); const float distance = sensor->distance();
_distance_min = sensor->min_distance_cm() * 0.01f; _distance_min = sensor->min_distance_cm() * 0.01f;
_distance_max = sensor->max_distance_cm() * 0.01f; _distance_max = sensor->max_distance_cm() * 0.01f;
if ((distance <= _distance_max) && (distance >= _distance_min) && !ignore_reading(angle, distance, false)) { if ((distance <= _distance_max) && (distance >= _distance_min) && !ignore_reading(angle, distance, false)) {
boundary.set_face_attributes(face, angle, distance); frontend.boundary.set_face_attributes(face, angle, distance);
// update OA database // update OA database
database_push(angle, distance); database_push(angle, distance);
} else { } else {
boundary.reset_face(face); frontend.boundary.reset_face(face);
} }
_last_update_ms = now; _last_update_ms = now;
} }

View File

@ -61,14 +61,14 @@ void AP_Proximity_SITL::update(void)
// update distance in each sector // update distance in each sector
for (uint8_t sector=0; sector < PROXIMITY_NUM_SECTORS; sector++) { for (uint8_t sector=0; sector < PROXIMITY_NUM_SECTORS; sector++) {
const float yaw_angle_deg = sector * 45.0f; const float yaw_angle_deg = sector * 45.0f;
AP_Proximity_Boundary_3D::Face face = boundary.get_face(yaw_angle_deg); AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(yaw_angle_deg);
float fence_distance; float fence_distance;
if (get_distance_to_fence(yaw_angle_deg, fence_distance)) { if (get_distance_to_fence(yaw_angle_deg, fence_distance)) {
boundary.set_face_attributes(face, yaw_angle_deg, fence_distance); frontend.boundary.set_face_attributes(face, yaw_angle_deg, fence_distance);
// update OA database // update OA database
database_push(yaw_angle_deg, fence_distance); database_push(yaw_angle_deg, fence_distance);
} else { } else {
boundary.reset_face(face); frontend.boundary.reset_face(face);
} }
} }
} else { } else {

View File

@ -95,13 +95,13 @@ bool AP_Proximity_TeraRangerTower::read_sensor_data()
void AP_Proximity_TeraRangerTower::update_sector_data(int16_t angle_deg, uint16_t distance_mm) void AP_Proximity_TeraRangerTower::update_sector_data(int16_t angle_deg, uint16_t distance_mm)
{ {
// Get location on 3-D boundary based on angle to the object // Get location on 3-D boundary based on angle to the object
const AP_Proximity_Boundary_3D::Face face = boundary.get_face(angle_deg); const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(angle_deg);
if ((distance_mm != 0xffff) && !ignore_reading(angle_deg, distance_mm * 0.001f, false)) { if ((distance_mm != 0xffff) && !ignore_reading(angle_deg, distance_mm * 0.001f, false)) {
boundary.set_face_attributes(face, angle_deg, ((float) distance_mm) / 1000); frontend.boundary.set_face_attributes(face, angle_deg, ((float) distance_mm) / 1000);
// update OA database // update OA database
database_push(angle_deg, ((float) distance_mm) / 1000); database_push(angle_deg, ((float) distance_mm) / 1000);
} else { } else {
boundary.reset_face(face); frontend.boundary.reset_face(face);
} }
_last_distance_received_ms = AP_HAL::millis(); _last_distance_received_ms = AP_HAL::millis();
} }

View File

@ -148,15 +148,15 @@ bool AP_Proximity_TeraRangerTowerEvo::read_sensor_data()
void AP_Proximity_TeraRangerTowerEvo::update_sector_data(int16_t angle_deg, uint16_t distance_mm) void AP_Proximity_TeraRangerTowerEvo::update_sector_data(int16_t angle_deg, uint16_t distance_mm)
{ {
// Get location on 3-D boundary based on angle to the object // Get location on 3-D boundary based on angle to the object
const AP_Proximity_Boundary_3D::Face face = boundary.get_face(angle_deg); const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(angle_deg);
//check for target too far, target too close and sensor not connected //check for target too far, target too close and sensor not connected
const bool valid = (distance_mm != 0xffff) && (distance_mm > 0x0001); const bool valid = (distance_mm != 0xffff) && (distance_mm > 0x0001);
if (valid && !ignore_reading(angle_deg, distance_mm * 0.001f, false)) { if (valid && !ignore_reading(angle_deg, distance_mm * 0.001f, false)) {
boundary.set_face_attributes(face, angle_deg, ((float) distance_mm) / 1000); frontend.boundary.set_face_attributes(face, angle_deg, ((float) distance_mm) / 1000);
// update OA database // update OA database
database_push(angle_deg, ((float) distance_mm) / 1000); database_push(angle_deg, ((float) distance_mm) / 1000);
} else { } else {
boundary.reset_face(face); frontend.boundary.reset_face(face);
} }
_last_distance_received_ms = AP_HAL::millis(); _last_distance_received_ms = AP_HAL::millis();
} }