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

AP_Proximity: refactor changes done for 3-D Boundary

This commit is contained in:
Rishabh 2021-01-10 23:44:24 +05:30 committed by Randy Mackay
parent 37a14a78df
commit de67e2b70c
8 changed files with 262 additions and 229 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_Proximity/AP_Proximity.h
View File

@ -95,6 +95,7 @@ public:
bool get_obstacle(uint8_t obstacle_num, Vector3f& vec_to_obstacle) const;
// returns shortest distance to "obstacle_num" obstacle, from a line segment formed between "seg_start" and "seg_end"
// returns FLT_MAX if it's an invalid instance.
float distance_to_obstacle(uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const;
// get distance and angle to closest object (used for pre-arm check)

4
libraries/AP_Proximity/AP_Proximity_AirSimSITL.cpp
View File

@ -21,7 +21,7 @@
extern const AP_HAL::HAL& hal;
#define PROXIMITY_MAX_RANGE 100.0f
#define PROXIMITY_ACCURACY 0.1f // minimum distance between objects sent to object database
#define PROXIMITY_ACCURACY 0.1f // minimum distance (in meters) between objects sent to object database
// update the state of the sensor
void AP_Proximity_AirSimSITL::update(void)
@ -52,7 +52,7 @@ void AP_Proximity_AirSimSITL::update(void)
}
// calculate distance to point and check larger than min distance
const Vector2f new_pos = Vector2f(point.x, point.y);
const Vector2f new_pos = Vector2f{point.x, point.y};
const float distance_sq = new_pos.length_squared();
if (distance_sq > distance_min_sq) {

2
libraries/AP_Proximity/AP_Proximity_Backend.cpp
View File

@ -107,7 +107,7 @@ void AP_Proximity_Backend::database_push(float angle, float distance)
// update Object Avoidance database with Earth-frame point
// pitch can be optionally provided if needed
void AP_Proximity_Backend::database_push(float angle, float distance, uint32_t timestamp_ms, const Vector3f &current_pos, const Matrix3f &body_to_ned, float pitch)
void AP_Proximity_Backend::database_push(float angle, float pitch, float distance, uint32_t timestamp_ms, const Vector3f &current_pos, const Matrix3f &body_to_ned)
{
AP_OADatabase *oaDb = AP::oadatabase();
if (oaDb == nullptr || !oaDb->healthy()) {

7
libraries/AP_Proximity/AP_Proximity_Backend.h
View File

@ -80,8 +80,11 @@ protected:
static bool database_prepare_for_push(Vector3f &current_pos, Matrix3f &body_to_ned);
// Note: "angle" refers to yaw (in body frame) towards the obstacle
static void database_push(float angle, float distance);
static void database_push(float angle, float distance, uint32_t timestamp_ms, const Vector3f &current_pos, const Matrix3f &body_to_ned, float pitch = 0.0f);
static void database_push(float angle, float distance, uint32_t timestamp_ms, const Vector3f &current_pos, const Matrix3f &body_to_ned) {
database_push(angle, 0.0f, distance, timestamp_ms, current_pos, 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);
AP_Proximity &frontend;
AP_Proximity::Proximity_State &state; // reference to this instances state

57
libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp
View File

@ -16,9 +16,9 @@ AP_Proximity_Boundary_3D::AP_Proximity_Boundary_3D()
void AP_Proximity_Boundary_3D::init()
{
for (uint8_t layer=0; layer < PROXIMITY_NUM_LAYERS; layer++) {
const float pitch = ((float)_pitch_middle_deg[layer]);
for (uint8_t sector=0; sector < PROXIMITY_NUM_SECTORS; sector++) {
float angle_rad = ((float)_sector_middle_deg[sector]+(PROXIMITY_SECTOR_WIDTH_DEG/2.0f));
float pitch = ((float)_pitch_middle_deg[layer]);
const float angle_rad = ((float)_sector_middle_deg[sector]+(PROXIMITY_SECTOR_WIDTH_DEG/2.0f));
_sector_edge_vector[layer][sector].offset_bearing(angle_rad, pitch, 100.0f);
_boundary_points[layer][sector] = _sector_edge_vector[layer][sector] * PROXIMITY_BOUNDARY_DIST_DEFAULT;
}
@ -26,19 +26,19 @@ void AP_Proximity_Boundary_3D::init()
}
// returns face corresponding to the provided yaw and (optionally) pitch
// pitch is the vertical body-frame angle (in degrees) to the obstacle (0=directly ahead, 90 is above the vehicle?)
// pitch is the vertical body-frame angle (in degrees) to the obstacle (0=directly ahead, 90 is above the vehicle)
// yaw is the horizontal body-frame angle (in degrees) to the obstacle (0=directly ahead of the vehicle, 90 is to the right of the vehicle)
AP_Proximity_Boundary_3D::Face AP_Proximity_Boundary_3D::get_face(float pitch, float yaw) const
{
const uint8_t sector = wrap_360(yaw + (PROXIMITY_SECTOR_WIDTH_DEG * 0.5f)) / 45.0f;
const float pitch_limited = constrain_float(pitch, -75.0f, 74.9f);
const uint8_t layer = (pitch_limited + 75.0f)/PROXIMITY_PITCH_WIDTH_DEG;
return Face(layer, sector);
return Face{layer, sector};
}
// Set the actual body-frame angle(yaw), pitch, and distance of the detected object.
// This method will also mark the sector and layer to be "valid", so this distance can be used for Obstacle Avoidance
void AP_Proximity_Boundary_3D::set_face_attributes(Face face, float angle, float pitch, float distance)
void AP_Proximity_Boundary_3D::set_face_attributes(const Face &face, float pitch, float angle, float distance)
{
if (!face.valid()) {
return;
@ -58,16 +58,22 @@ void AP_Proximity_Boundary_3D::set_face_attributes(Face face, float angle, float
void AP_Proximity_Boundary_3D::add_distance(float pitch, float yaw, float distance)
{
Face face = get_face(pitch, yaw);
if (!face.valid()) {
return;
}
if (!_distance_valid[face.layer][face.sector] || (distance < _distance[face.layer][face.sector])) {
_distance[face.layer][face.sector] = distance;
_distance_valid[face.layer][face.sector] = true;
_angle[face.layer][face.sector] = yaw;
_pitch[face.layer][face.sector] = pitch;
}
}
// update boundary points used for object avoidance based on a single sector and pitch distance changing
// the boundary points lie on the line between sectors meaning two boundary points may be updated based on a single sector's distance changing
// the boundary point is set to the shortest distance found in the two adjacent sectors, this is a conservative boundary around the vehicle
void AP_Proximity_Boundary_3D::update_boundary(const Face face)
void AP_Proximity_Boundary_3D::update_boundary(const Face &face)
{
// sanity check
if (!face.valid()) {
@ -111,7 +117,7 @@ void AP_Proximity_Boundary_3D::update_boundary(const Face face)
}
_boundary_points[layer][prev_sector] = _sector_edge_vector[layer][prev_sector] * shortest_distance;
// if the sector counter-clockwise from the previous sector has an invalid distance, set boundary to create a cup like boundary
// if the sector counter-clockwise from the previous sector has an invalid distance, set boundary to create a cup-like boundary
const uint8_t prev_sector_ccw = get_prev_sector(prev_sector);
if (!_distance_valid[layer][prev_sector_ccw]) {
_boundary_points[layer][prev_sector_ccw] = _sector_edge_vector[layer][prev_sector_ccw] * shortest_distance;
@ -122,7 +128,7 @@ void AP_Proximity_Boundary_3D::update_boundary(const Face face)
void AP_Proximity_Boundary_3D::update_middle_boundary()
{
for (uint8_t sector=0; sector < PROXIMITY_NUM_SECTORS; sector++) {
update_boundary(Face(PROXIMITY_MIDDLE_LAYER, sector));
update_boundary(Face{PROXIMITY_MIDDLE_LAYER, sector});
}
}
@ -138,7 +144,7 @@ void AP_Proximity_Boundary_3D::reset()
// Reset this location, specified by Face object, back to default
// i.e Distance is marked as not-valid, and set to a large number.
void AP_Proximity_Boundary_3D::reset_face(Face face)
void AP_Proximity_Boundary_3D::reset_face(const Face &face)
{
if (!face.valid()) {
return;
@ -150,7 +156,7 @@ void AP_Proximity_Boundary_3D::reset_face(Face face)
}
// get distance for a face. returns true on success and fills in distance argument with distance in meters
bool AP_Proximity_Boundary_3D::get_distance(Face face, float &distance) const
bool AP_Proximity_Boundary_3D::get_distance(const Face &face, float &distance) const
{
if (!face.valid()) {
return false;
@ -183,23 +189,14 @@ bool AP_Proximity_Boundary_3D::convert_obstacle_num_to_face(uint8_t obstacle_num
face.sector = sector;
face.layer = layer;
// if this face is valid (sensor is directly pushing values towards this face), return true
if (_distance_valid[layer][sector]) {
return true;
}
// if face cw from this face is valid, then "update_boundary" must have manipulated this face also.
// return true
const uint8_t next_sector = get_next_sector(sector);
if (_distance_valid[layer][next_sector]) {
return true;
}
// if face cw twice from this face is valid, then "update_boundary" must have manipulated this face also.
// return true
const uint8_t next_to_next_sector = get_next_sector(next_sector);
if (_distance_valid[layer][next_to_next_sector]) {
return true;
uint8_t valid_sector = sector;
// check for 3 adjacent sectors
for (uint8_t i=0; i < 2; i++) {
if (_distance_valid[layer][valid_sector]) {
// update boundary has manipulated this face
return true;
}
valid_sector = get_next_sector(valid_sector);
}
// this face was not manipulated by "update_boundary" and is stale. Don't use it
@ -210,6 +207,7 @@ bool AP_Proximity_Boundary_3D::convert_obstacle_num_to_face(uint8_t obstacle_num
// This function then draws a line between this sector, and sector + 1 at the given layer
// Then returns the closest point on this line from vehicle, in body-frame.
// Used by GPS based Simple Avoidance
// False is returned if the obstacle_num provided does not produce a valid obstacle
bool AP_Proximity_Boundary_3D::get_obstacle(uint8_t obstacle_num, Vector3f& vec_to_obstacle) const
{
Face face;
@ -222,7 +220,7 @@ bool AP_Proximity_Boundary_3D::get_obstacle(uint8_t obstacle_num, Vector3f& vec_
const Vector3f start = _boundary_points[face.layer][sector_start];
const Vector3f end = _boundary_points[face.layer][sector_end];
vec_to_obstacle = Vector3f::closest_point_between_line_and_point(start, end, Vector3f{0.0f, 0.0f, 0.0f});
vec_to_obstacle = Vector3f::point_on_line_closest_to_other_point(start, end, Vector3f{});
return true;
}
@ -230,6 +228,7 @@ bool AP_Proximity_Boundary_3D::get_obstacle(uint8_t obstacle_num, Vector3f& vec_
// This function then draws a line between this sector, and sector + 1 at the given layer
// Then returns the closest point on this line from the segment that was passed, in body-frame.
// Used by GPS based Simple Avoidance - for "brake mode"
// FLT_MAX is returned if the obstacle_num provided does not produce a valid obstacle
float AP_Proximity_Boundary_3D::distance_to_obstacle(uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const
{
Face face;
@ -283,7 +282,7 @@ uint8_t AP_Proximity_Boundary_3D::get_horizontal_object_count() const
// 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
bool AP_Proximity_Boundary_3D::get_horizontal_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const
bool AP_Proximity_Boundary_3D::get_horizontal_object_angle_and_distance(uint8_t object_number, float &angle_deg, float &distance) const
{
if ((object_number < PROXIMITY_NUM_SECTORS) && _distance_valid[PROXIMITY_MIDDLE_LAYER][object_number]) {
angle_deg = _angle[PROXIMITY_MIDDLE_LAYER][object_number];

34
libraries/AP_Proximity/AP_Proximity_Boundary_3D.h
View File

@ -19,7 +19,7 @@
#define PROXIMITY_NUM_LAYERS 5 // num of layers in a sector
#define PROXIMITY_MIDDLE_LAYER 2 // middle layer
#define PROXIMITY_PITCH_WIDTH_DEG 30 // width between each layer in degrees
#define PROXIMITY_SECTOR_WIDTH_DEG 45.0f // width of sectors in degrees
#define PROXIMITY_SECTOR_WIDTH_DEG (360.0f/PROXIMITY_NUM_SECTORS) // width of sectors in degrees
#define PROXIMITY_BOUNDARY_DIST_MIN 0.6f // minimum distance for a boundary point. This ensures the object avoidance code doesn't think we are outside the boundary.
#define PROXIMITY_BOUNDARY_DIST_DEFAULT 100 // if we have no data for a sector, boundary is placed 100m out
@ -30,6 +30,7 @@ public:
AP_Proximity_Boundary_3D();
// stores the layer and sector as a single object to access and modify the 3-D boundary
// Objects of this class are used temporarily to modify the boundary, i,e they are not persistant or stored anywhere
class Face
{
public:
@ -45,8 +46,8 @@ public:
bool operator ==(const Face &other) const { return ((layer == other.layer) && (sector == other.sector)); }
bool operator !=(const Face &other) const { return ((layer != other.layer) || (sector != other.sector)); }
uint8_t layer; // vertical "steps" on the 3D Boundary
uint8_t sector; // horizontal "steps" on the 3D Boundary
uint8_t layer; // vertical "steps" on the 3D Boundary. 0th layer is the bottom most layer, 1st layer is 30 degrees above (in body frame) and so on
uint8_t sector; // horizontal "steps" on the 3D Boundary. 0th sector is directly in front of the vehicle. Each sector is 45 degrees wide.
};
// returns face corresponding to the provided yaw and (optionally) pitch
@ -59,8 +60,8 @@ public:
// This method will also mark the sector and layer to be "valid",
// This distance can then be used for Obstacle Avoidance
// Assume detected obstacle is horizontal (zero pitch), if no pitch is passed
void set_face_attributes(Face face, float pitch, float yaw, float distance);
void set_face_attributes(Face face, float yaw, float distance) { set_face_attributes(face, 0, yaw, distance); }
void set_face_attributes(const Face &face, float pitch, float yaw, float distance);
void set_face_attributes(const Face &face, float yaw, float distance) { set_face_attributes(face, 0, yaw, distance); }
// add a distance to the boundary if it is shorter than any other provided distance since the last time the boundary was reset
// pitch and yaw are in degrees, distance is in meters
@ -70,7 +71,7 @@ public:
// update boundary points used for simple avoidance based on a single sector and pitch distance changing
// the boundary points lie on the line between sectors meaning two boundary points may be updated based on a single sector's distance changing
// the boundary point is set to the shortest distance found in the two adjacent sectors, this is a conservative boundary around the vehicle
void update_boundary(Face face);
void update_boundary(const Face &face);
// update middle layer boundary points
void update_middle_boundary();
@ -79,24 +80,21 @@ public:
void reset();
// Reset this location, specified by Face object, back to default
// i.e Distance is marked as not-valid, and set to a large number.
void reset_face(Face face);
// i.e Distance is marked as not-valid
void reset_face(const Face &face);
// get distance for a face. returns true on success and fills in distance argument with distance in meters
bool get_distance(Face face, float &distance) const;
bool get_distance(const Face &face, float &distance) const;
// Get the total number of obstacles
uint8_t get_obstacle_count() const;
// Appropriate layer and sector are found from the passed obstacle_num
// This function then draws a line between this sector, and sector + 1 at the given layer
// Then returns the closest point on this line from vehicle, in body-frame.
// Returns a body frame vector (in cm) to an obstacle
// False is returned if the obstacle_num provided does not produce a valid obstacle
bool get_obstacle(uint8_t obstacle_num, Vector3f& vec_to_boundary) const;
// Appropriate layer and sector are found from the passed obstacle_num
// This function then draws a line between this sector, and sector + 1 at the given layer
// Then returns the closest point on this line from the segment that was passed, in body-frame.
// Used by GPS based Simple Avoidance - for "brake mode"
// Returns a body frame vector (in cm) nearest to obstacle, in betwen seg_start and seg_end
// FLT_MAX is returned if the obstacle_num provided does not produce a valid obstacle
float distance_to_obstacle(uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const;
// get distance and angle to closest object (used for pre-arm check)
@ -108,8 +106,10 @@ public:
bool get_horizontal_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const;
// sectors
static_assert(PROXIMITY_NUM_SECTORS == 8, "PROXIMITY_NUM_SECTOR must be 8");
const uint16_t _sector_middle_deg[PROXIMITY_NUM_SECTORS] {0, 45, 90, 135, 180, 225, 270, 315}; // middle angle of each sector
// layers
static_assert(PROXIMITY_NUM_LAYERS == 5, "PROXIMITY_NUM_LAYERS must be 5");
const int16_t _pitch_middle_deg[PROXIMITY_NUM_LAYERS] {-60, -30, 0, 30, 60};
private:
@ -128,7 +128,7 @@ private:
// "update_boundary" method manipulates two sectors ccw and one sector cw from any valid face.
// Any boundary that does not fall into these manipulated faces are useless, and will be marked as false
// The resultant is packed into a Boundary Location object and returned by reference as "face"
bool convert_obstacle_num_to_face(uint8_t obstacle_num, Face& face) const;
bool convert_obstacle_num_to_face(uint8_t obstacle_num, Face& face) const WARN_IF_UNUSED;
Vector3f _sector_edge_vector[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS];
Vector3f _boundary_points[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS];

378
libraries/AP_Proximity/AP_Proximity_MAV.cpp
View File

@ -45,186 +45,208 @@ bool AP_Proximity_MAV::get_upward_distance(float &distance) const
return false;
}
// handle mavlink DISTANCE_SENSOR messages
// handle mavlink messages
void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
{
if (msg.msgid == MAVLINK_MSG_ID_DISTANCE_SENSOR) {
mavlink_distance_sensor_t packet;
mavlink_msg_distance_sensor_decode(&msg, &packet);
{
switch (msg.msgid) {
case (MAVLINK_MSG_ID_DISTANCE_SENSOR):
handle_distance_sensor_msg(msg);
break;
// store distance to appropriate sector based on orientation field
if (packet.orientation <= MAV_SENSOR_ROTATION_YAW_315) {
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);
// store in meters
const uint16_t distance = packet.current_distance * 0.01f;
_distance_min = packet.min_distance * 0.01f;
_distance_max = packet.max_distance * 0.01f;
if (distance <= _distance_max && distance >= _distance_max) {
boundary.set_face_attributes(face, yaw_angle_deg, distance);
// update OA database
database_push(yaw_angle_deg, distance);
} else {
// reset distance for this face
boundary.reset_face(face);
}
_last_update_ms = AP_HAL::millis();
}
case (MAVLINK_MSG_ID_OBSTACLE_DISTANCE):
handle_obstacle_distance_msg(msg);
break;
// store upward distance
if (packet.orientation == MAV_SENSOR_ROTATION_PITCH_90) {
_distance_upward = packet.current_distance * 0.01f;
_last_upward_update_ms = AP_HAL::millis();
}
return;
}
if (msg.msgid == MAVLINK_MSG_ID_OBSTACLE_DISTANCE) {
mavlink_obstacle_distance_t packet;
mavlink_msg_obstacle_distance_decode(&msg, &packet);
// check increment (message's sector width)
float increment;
if (!is_zero(packet.increment_f)) {
// use increment float
increment = packet.increment_f;
} else if (packet.increment != 0) {
// use increment uint8_t
increment = packet.increment;
} else {
// invalid increment
return;
}
const uint8_t total_distances = MIN(((360.0f / fabsf(increment)) + 0.5f), MAVLINK_MSG_OBSTACLE_DISTANCE_FIELD_DISTANCES_LEN); // usually 72
// set distance min and max
_distance_min = packet.min_distance * 0.01f;
_distance_max = packet.max_distance * 0.01f;
_last_update_ms = AP_HAL::millis();
// get user configured yaw correction from front end
const float param_yaw_offset = constrain_float(frontend.get_yaw_correction(state.instance), -360.0f, +360.0f);
const float yaw_correction = wrap_360(param_yaw_offset + packet.angle_offset);
if (frontend.get_orientation(state.instance) != 0) {
increment *= -1;
}
Vector3f current_pos;
Matrix3f body_to_ned;
const bool database_ready = database_prepare_for_push(current_pos, body_to_ned);
// variables to calculate closest angle and distance for each face
AP_Proximity_Boundary_3D::Face face;
float face_distance;
float face_yaw_deg;
bool face_distance_valid = false;
// iterate over message's sectors
for (uint8_t j = 0; j < total_distances; j++) {
const uint16_t distance_cm = packet.distances[j];
if (distance_cm == 0 ||
distance_cm == 65535 ||
distance_cm < packet.min_distance ||
distance_cm > packet.max_distance)
{
// sanity check failed, ignore this distance value
continue;
}
const float packet_distance_m = distance_cm * 0.01f;
const float mid_angle = wrap_360((float)j * increment + yaw_correction);
// get face for this latest reading
AP_Proximity_Boundary_3D::Face latest_face = 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);
} else {
boundary.reset_face(face);
}
// init for latest face
face = latest_face;
face_distance_valid = false;
}
// update minimum distance found so far
if (!face_distance_valid || (packet_distance_m < face_distance)) {
face_yaw_deg = mid_angle;
face_distance = packet_distance_m;
face_distance_valid = true;
}
// update Object Avoidance database with Earth-frame point
if (database_ready) {
database_push(mid_angle, packet_distance_m, _last_update_ms, current_pos, body_to_ned);
}
}
// process the last face
if (face_distance_valid) {
boundary.set_face_attributes(face, face_yaw_deg, face_distance);
} else {
boundary.reset_face(face);
}
return;
}
if (msg.msgid == MAVLINK_MSG_ID_OBSTACLE_DISTANCE_3D) {
mavlink_obstacle_distance_3d_t packet;
mavlink_msg_obstacle_distance_3d_decode(&msg, &packet);
const uint32_t previous_sys_time = _last_update_ms;
_last_update_ms = AP_HAL::millis();
// time_diff will check if the new message arrived significantly later than the last message
const uint32_t time_diff = _last_update_ms - previous_sys_time;
const uint32_t previous_msg_timestamp = _last_3d_msg_update_ms;
_last_3d_msg_update_ms = packet.time_boot_ms;
bool clear_fence = false;
// we will add on to the last fence if the time stamp is the same
// provided we got the new obstacle in less than PROXIMITY_3D_MSG_TIMEOUT_MS
if ((previous_msg_timestamp != _last_3d_msg_update_ms) || (time_diff > PROXIMITY_3D_MSG_TIMEOUT_MS)) {
clear_fence = true;
}
_distance_min = packet.min_distance;
_distance_max = packet.max_distance;
Vector3f current_pos;
Matrix3f body_to_ned;
const bool database_ready = database_prepare_for_push(current_pos, body_to_ned);
if (clear_fence) {
// cleared fence back to defaults since we have a new timestamp
boundary.reset();
}
const Vector3f obstacle(packet.x, packet.y, packet.z);
if (obstacle.length() < _distance_min || obstacle.length() > _distance_max || obstacle.is_zero()) {
// message isn't healthy
return;
}
// extract yaw and pitch from Obstacle Vector
const float yaw = wrap_360(degrees(atan2f(obstacle.y, obstacle.x)));
const float pitch = wrap_180(degrees(M_PI_2 - atan2f(norm(obstacle.x, obstacle.y), obstacle.z)));
// allot them correct layer and sector based on calculated pitch and yaw
const AP_Proximity_Boundary_3D::Face face = boundary.get_face(pitch, yaw);
float face_distance;
if (boundary.get_distance(face, face_distance) && (face_distance < obstacle.length())) {
// we already have a shorter distance in this layer and sector
return;
}
boundary.set_face_attributes(face, yaw, pitch, obstacle.length());
if (database_ready) {
database_push(yaw, obstacle.length(),_last_update_ms, current_pos, body_to_ned, pitch);
}
case (MAVLINK_MSG_ID_OBSTACLE_DISTANCE_3D):
handle_obstacle_distance_3d_msg(msg);
break;
}
}
// handle mavlink DISTANCE_SENSOR messages
void AP_Proximity_MAV::handle_distance_sensor_msg(const mavlink_message_t &msg)
{
mavlink_distance_sensor_t packet;
mavlink_msg_distance_sensor_decode(&msg, &packet);
// store distance to appropriate sector based on orientation field
if (packet.orientation <= MAV_SENSOR_ROTATION_YAW_315) {
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);
// store in meters
const float distance = packet.current_distance * 0.01f;
_distance_min = packet.min_distance * 0.01f;
_distance_max = packet.max_distance * 0.01f;
if (distance <= _distance_max && distance >= _distance_min) {
boundary.set_face_attributes(face, yaw_angle_deg, distance);
// update OA database
database_push(yaw_angle_deg, distance);
} else {
// reset distance for this face
boundary.reset_face(face);
}
_last_update_ms = AP_HAL::millis();
}
// store upward distance
if (packet.orientation == MAV_SENSOR_ROTATION_PITCH_90) {
_distance_upward = packet.current_distance * 0.01f;
_last_upward_update_ms = AP_HAL::millis();
}
return;
}
// handle mavlink OBSTACLE_DISTANCE messages
void AP_Proximity_MAV::handle_obstacle_distance_msg(const mavlink_message_t &msg)
{
mavlink_obstacle_distance_t packet;
mavlink_msg_obstacle_distance_decode(&msg, &packet);
// check increment (message's sector width)
float increment;
if (!is_zero(packet.increment_f)) {
// use increment float
increment = packet.increment_f;
} else if (packet.increment != 0) {
// use increment uint8_t
increment = packet.increment;
} else {
// invalid increment
return;
}
const uint8_t total_distances = MIN(((360.0f / fabsf(increment)) + 0.5f), MAVLINK_MSG_OBSTACLE_DISTANCE_FIELD_DISTANCES_LEN); // usually 72
// set distance min and max
_distance_min = packet.min_distance * 0.01f;
_distance_max = packet.max_distance * 0.01f;
_last_update_ms = AP_HAL::millis();
// get user configured yaw correction from front end
const float param_yaw_offset = constrain_float(frontend.get_yaw_correction(state.instance), -360.0f, +360.0f);
const float yaw_correction = wrap_360(param_yaw_offset + packet.angle_offset);
if (frontend.get_orientation(state.instance) != 0) {
increment *= -1;
}
Vector3f current_pos;
Matrix3f body_to_ned;
const bool database_ready = database_prepare_for_push(current_pos, body_to_ned);
// variables to calculate closest angle and distance for each face
AP_Proximity_Boundary_3D::Face face;
float face_distance;
float face_yaw_deg;
bool face_distance_valid = false;
// iterate over message's sectors
for (uint8_t j = 0; j < total_distances; j++) {
const uint16_t distance_cm = packet.distances[j];
if (distance_cm == 0 ||
distance_cm == 65535 ||
distance_cm < packet.min_distance ||
distance_cm > packet.max_distance)
{
// sanity check failed, ignore this distance value
continue;
}
const float packet_distance_m = distance_cm * 0.01f;
const float mid_angle = wrap_360((float)j * increment + yaw_correction);
// get face for this latest reading
AP_Proximity_Boundary_3D::Face latest_face = 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);
} else {
boundary.reset_face(face);
}
// init for latest face
face = latest_face;
face_distance_valid = false;
}
// update minimum distance found so far
if (!face_distance_valid || (packet_distance_m < face_distance)) {
face_yaw_deg = mid_angle;
face_distance = packet_distance_m;
face_distance_valid = true;
}
// update Object Avoidance database with Earth-frame point
if (database_ready) {
database_push(mid_angle, packet_distance_m, _last_update_ms, current_pos, body_to_ned);
}
}
// process the last face
if (face_distance_valid) {
boundary.set_face_attributes(face, face_yaw_deg, face_distance);
} else {
boundary.reset_face(face);
}
return;
}
// handle mavlink OBSTACLE_DISTANCE_3D messages
void AP_Proximity_MAV::handle_obstacle_distance_3d_msg(const mavlink_message_t &msg)
{
mavlink_obstacle_distance_3d_t packet;
mavlink_msg_obstacle_distance_3d_decode(&msg, &packet);
const uint32_t previous_sys_time = _last_update_ms;
_last_update_ms = AP_HAL::millis();
// time_diff will check if the new message arrived significantly later than the last message
const uint32_t time_diff = _last_update_ms - previous_sys_time;
const uint32_t previous_msg_timestamp = _last_3d_msg_update_ms;
_last_3d_msg_update_ms = packet.time_boot_ms;
if (packet.frame != MAV_FRAME_BODY_FRD) {
// we do not support this frame of reference yet
return;
}
// we will add on to the last fence if the time stamp is the same
// provided we got the new obstacle in less than PROXIMITY_3D_MSG_TIMEOUT_MS
if ((previous_msg_timestamp != _last_3d_msg_update_ms) || (time_diff > PROXIMITY_3D_MSG_TIMEOUT_MS)) {
// cleared fence back to defaults since we have a new timestamp
boundary.reset();
}
_distance_min = packet.min_distance;
_distance_max = packet.max_distance;
Vector3f current_pos;
Matrix3f body_to_ned;
const bool database_ready = database_prepare_for_push(current_pos, body_to_ned);
const Vector3f obstacle(packet.x, packet.y, packet.z * -1.0f);
if (obstacle.length() < _distance_min || obstacle.length() > _distance_max || obstacle.is_zero()) {
// message isn't healthy
return;
}
// extract yaw and pitch from Obstacle Vector
const float yaw = wrap_360(degrees(atan2f(obstacle.y, obstacle.x)));
const float pitch = wrap_180(degrees(M_PI_2 - atan2f(norm(obstacle.x, obstacle.y), obstacle.z)));
// allot to correct layer and sector based on calculated pitch and yaw
const AP_Proximity_Boundary_3D::Face face = boundary.get_face(pitch, yaw);
float face_distance;
if (boundary.get_distance(face, face_distance) && (face_distance < obstacle.length())) {
// we already have a shorter distance in this layer and sector
return;
}
boundary.set_face_attributes(face, yaw, pitch, obstacle.length());
if (database_ready) {
database_push(yaw, pitch, obstacle.length(),_last_update_ms, current_pos, body_to_ned);
}
return;
}

8
libraries/AP_Proximity/AP_Proximity_MAV.h
View File

@ -24,6 +24,14 @@ public:
void handle_msg(const mavlink_message_t &msg) override;
private:
// handle mavlink DISTANCE_SENSOR messages
void handle_distance_sensor_msg(const mavlink_message_t &msg);
// handle mavlink OBSTACLE_DISTANCE messages
void handle_obstacle_distance_msg(const mavlink_message_t &msg);
// handle mavlink OBSTACLE_DISTANCE_3D messages
void handle_obstacle_distance_3d_msg(const mavlink_message_t &msg);
// horizontal distance support
uint32_t _last_update_ms; // system time of last mavlink message received
uint32_t _last_3d_msg_update_ms; // last stored OBSTACLE_DISTANCE_3D message timestamp