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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

79
libraries/AP_Proximity/AP_Proximity.cpp
View File

@ -130,9 +130,14 @@ void AP_Proximity::update()
continue;
}
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
for (int8_t i=num_instances-1; i>=0; i--) {
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
bool AP_Proximity::get_horizontal_distances(Proximity_Distance_Array &prx_dist_array) const
{
if (!valid_instance(primary_instance)) {
return false;
}
// 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);
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 total number of obstacles, used in GPS based Simple Avoidance
uint8_t AP_Proximity::get_obstacle_count() const
{
if (!valid_instance(primary_instance)) {
return 0;
}
return drivers[primary_instance]->get_obstacle_count();
{
return boundary.get_obstacle_count();
}
// 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
{
if (!valid_instance(primary_instance)) {
return false;
}
return drivers[primary_instance]->get_obstacle(obstacle_num, vec_to_obstacle);
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
// 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
{
if (!valid_instance(primary_instance)) {
return false;
}
return drivers[primary_instance]->closest_point_from_segment_to_obstacle(obstacle_num, seg_start, seg_end, closest_point);
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 AP_Proximity::get_closest_object(float& angle_deg, float &distance) const
{
if (!valid_instance(primary_instance)) {
return false;
}
// get closest object from backend
return drivers[primary_instance]->get_closest_object(angle_deg, distance);
return boundary.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
{
if (!valid_instance(primary_instance)) {
return 0;
}
// get count from backend
return drivers[primary_instance]->get_horizontal_object_count();
return boundary.get_horizontal_object_count();
}
// get an object's angle and distance, used for non-GPS avoidance
// returns false if no angle or distance could be returned for some reason
// get number of objects, angle and distance - used for non-GPS avoidance
bool AP_Proximity::get_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const
{
if (!valid_instance(primary_instance)) {
return false;
}
// get angle and distance from backend
return drivers[primary_instance]->get_horizontal_object_angle_and_distance(object_number, angle_deg, distance);
return boundary.get_horizontal_object_angle_and_distance(object_number, angle_deg, distance);
}
// 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 filt_dist_array{}; //filtered distances stored here
auto &logger { AP::logger() };
for (uint8_t i = 0; i < get_num_layers(); i++) {
const bool active = get_active_layer_distances(i, dist_array, filt_dist_array);
for (uint8_t i = 0; i < boundary.get_num_layers(); i++) {
const bool active = boundary.get_layer_distances(i, distance_max(), dist_array, filt_dist_array);
if (!active) {
// nothing on this layer
continue;

23
libraries/AP_Proximity/AP_Proximity.h
View File

@ -23,9 +23,9 @@
#include <AP_Math/AP_Math.h>
#include <GCS_MAVLink/GCS_MAVLink.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_DIRECTION 8
#define PROXIMITY_SENSOR_ID_START 10
class AP_Proximity_Backend;
@ -93,27 +93,9 @@ public:
// 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
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
uint8_t get_obstacle_count() const;
@ -170,6 +152,9 @@ public:
static AP_Proximity *get_singleton(void) { return _singleton; };
// 3D boundary
AP_Proximity_Boundary_3D boundary;
protected:
// parameters for backends

6
libraries/AP_Proximity/AP_Proximity_AirSimSITL.cpp
View File

@ -37,7 +37,7 @@ void AP_Proximity_AirSimSITL::update(void)
set_status(AP_Proximity::Status::Good);
// 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
const float distance_min_sq = sq(distance_min());
@ -62,7 +62,7 @@ void AP_Proximity_AirSimSITL::update(void)
// add distance to the 3D boundary
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
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
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

27
libraries/AP_Proximity/AP_Proximity_Backend.cpp
View File

@ -38,39 +38,12 @@ AP_Proximity_Backend::AP_Proximity_Backend(AP_Proximity& _frontend, AP_Proximity
static_assert(PROXIMITY_MAX_DIRECTION <= 8,
"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
void AP_Proximity_Backend::set_status(AP_Proximity::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
float AP_Proximity_Backend::correct_angle_for_orientation(float angle_degrees) const
{

37
libraries/AP_Proximity/AP_Proximity_Backend.h
View File

@ -18,11 +18,9 @@
#if HAL_PROXIMITY_ENABLED
#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_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
{
@ -37,9 +35,6 @@ public:
// update the state structure
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
virtual float distance_max() const = 0;
virtual float distance_min() const = 0;
@ -50,33 +45,6 @@ public:
// handle mavlink DISTANCE_SENSOR messages
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
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);
uint32_t _last_timeout_check_ms; // time when boundary was checked for non-updated valid faces
// used for ground detection
uint32_t _last_downward_update_ms;
bool _rangefinder_use;
@ -122,9 +88,6 @@ protected:
AP_Proximity &frontend;
AP_Proximity::Proximity_State &state; // reference to this instances state
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

13
libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp
View File

@ -15,8 +15,7 @@
#include "AP_Proximity_Boundary_3D.h"
#if HAL_PROXIMITY_ENABLED
#include "AP_Proximity_Backend.h"
#define PROXIMITY_BOUNDARY_3D_TIMEOUT_MS 750 // we should check the 3D boundary faces after this many ms
/*
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
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 sector=0; sector < PROXIMITY_NUM_SECTORS; 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
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
// 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

24
libraries/AP_Proximity/AP_Proximity_Boundary_3D.h
View File

@ -15,10 +15,8 @@
#pragma once
#include "AP_Proximity.h"
#if HAL_PROXIMITY_ENABLED
#include <AP_Common/AP_Common.h>
#include <AP_Math/AP_Math.h>
#include <Filter/LowPassFilter.h>
#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_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
{
public:
@ -112,7 +123,7 @@ public:
uint8_t get_num_layers() const { return PROXIMITY_NUM_LAYERS; }
// 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
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
LowPassFilterFloat _filtered_distance[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // 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.
@ -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 _pitch[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // pitch angle in degrees to the closest object within each sector and layer
};
#endif // HAL_PROXIMITY_ENABLED

5
libraries/AP_Proximity/AP_Proximity_Cygbot_D1.cpp
View File

@ -118,7 +118,7 @@ bool AP_Proximity_Cygbot_D1::parse_byte(uint8_t data)
// checksum is valid, parse payload
_last_distance_received_ms = AP_HAL::millis();
parse_payload();
_temp_boundary.update_3D_boundary(boundary);
_temp_boundary.update_3D_boundary(frontend.boundary);
reset();
return true;
}
@ -149,7 +149,8 @@ void AP_Proximity_Cygbot_D1::parse_payload()
continue;
}
// 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
_temp_boundary.add_distance(face, corrected_angle, distance_m);
// push to OA_DB

6
libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp
View File

@ -320,16 +320,16 @@ void AP_Proximity_LightWareSF40C::process_message()
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 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 (face != _face) {
// update boundary used for avoidance
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 {
// mark previous face invalid
boundary.reset_face(_face);
frontend.boundary.reset_face(_face);
}
// init for new face
_face = face;

6
libraries/AP_Proximity/AP_Proximity_LightWareSF45B.cpp
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
// 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_distance_valid) {
boundary.set_face_attributes(_face, _face_yaw_deg, _face_distance);
frontend.boundary.set_face_attributes(_face, _face_yaw_deg, _face_distance);
} else {
// mark previous face invalid
boundary.reset_face(_face);
frontend.boundary.reset_face(_face);
}
// record updated face
_face = face;

20
libraries/AP_Proximity/AP_Proximity_MAV.cpp
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
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
temp_boundary.update_3D_boundary(boundary);
temp_boundary.update_3D_boundary(frontend.boundary);
// clear temp boundary for new data
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;
// get the face for this sector
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_max = packet.max_distance * 0.01f;
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;
// reset this boundary to fill with new data
boundary.reset();
frontend.boundary.reset();
// iterate over message's sectors
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
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) {
// store previous face
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 {
boundary.reset_face(face);
frontend.boundary.reset_face(face);
}
// init for 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
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 {
boundary.reset_face(face);
frontend.boundary.reset_face(face);
}
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)) {
// 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
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
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());
if (database_ready) {

6
libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp
View File

@ -314,15 +314,15 @@ void AP_Proximity_RPLidarA2::parse_response_data()
#endif
_last_distance_received_ms = AP_HAL::millis();
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) {
// distance is for a new face, the previous one can be updated now
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 {
// reset distance from last face
boundary.reset_face(face);
frontend.boundary.reset_face(face);
}
// initialize the new face

6
libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp
View File

@ -45,17 +45,17 @@ void AP_Proximity_RangeFinder::update(void)
if (sensor->orientation() <= ROTATION_YAW_315) {
const uint8_t sector = (uint8_t)sensor->orientation();
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
const float distance = sensor->distance();
_distance_min = sensor->min_distance_cm() * 0.01f;
_distance_max = sensor->max_distance_cm() * 0.01f;
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
database_push(angle, distance);
} else {
boundary.reset_face(face);
frontend.boundary.reset_face(face);
}
_last_update_ms = now;
}

6
libraries/AP_Proximity/AP_Proximity_SITL.cpp
View File

@ -61,14 +61,14 @@ void AP_Proximity_SITL::update(void)
// update distance in each sector
for (uint8_t sector=0; sector < PROXIMITY_NUM_SECTORS; sector++) {
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;
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
database_push(yaw_angle_deg, fence_distance);
} else {
boundary.reset_face(face);
frontend.boundary.reset_face(face);
}
}
} else {

6
libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp
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)
{
// 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)) {
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
database_push(angle_deg, ((float) distance_mm) / 1000);
} else {
boundary.reset_face(face);
frontend.boundary.reset_face(face);
}
_last_distance_received_ms = AP_HAL::millis();
}

6
libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp
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)
{
// 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
const bool valid = (distance_mm != 0xffff) && (distance_mm > 0x0001);
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
database_push(angle_deg, ((float) distance_mm) / 1000);
} else {
boundary.reset_face(face);
frontend.boundary.reset_face(face);
}
_last_distance_received_ms = AP_HAL::millis();
}