5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 09:28:31 -04:00

AP_Proximity: MAV backend uses modified Boundary_3D interface

This commit is contained in:
Randy Mackay 2020-12-14 16:59:11 +09:00
parent a65e0affe4
commit 2d77adf720

View File

@ -22,6 +22,7 @@ extern const AP_HAL::HAL& hal;
#define PROXIMITY_MAV_TIMEOUT_MS 500 // distance messages must arrive within this many milliseconds #define PROXIMITY_MAV_TIMEOUT_MS 500 // distance messages must arrive within this many milliseconds
#define PROXIMITY_3D_MSG_TIMEOUT_MS 50 // boundary will be reset if OBSTACLE_DISTANCE_3D message does not arrive within this many milliseconds #define PROXIMITY_3D_MSG_TIMEOUT_MS 50 // boundary will be reset if OBSTACLE_DISTANCE_3D message does not arrive within this many milliseconds
// update the state of the sensor // update the state of the sensor
void AP_Proximity_MAV::update(void) void AP_Proximity_MAV::update(void)
{ {
@ -54,24 +55,22 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
// store distance to appropriate sector based on orientation field // store distance to appropriate sector based on orientation field
if (packet.orientation <= MAV_SENSOR_ROTATION_YAW_315) { if (packet.orientation <= MAV_SENSOR_ROTATION_YAW_315) {
const uint8_t sector = packet.orientation; const uint8_t sector = packet.orientation;
// create a boundary location object based on this sector // get the face for this sector
const boundary_location bnd_loc{sector}; const float yaw_angle_deg = sector * 45;
const AP_Proximity_Boundary_3D::Face face = boundary.get_face(yaw_angle_deg);
// store in meters // store in meters
const uint16_t distance = packet.current_distance * 0.01f; const uint16_t distance = packet.current_distance * 0.01f;
_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;
// reset data on this sector, to be filled with new data
boundary.reset_sector(bnd_loc);
if (distance <= _distance_max && distance >= _distance_max) { if (distance <= _distance_max && distance >= _distance_max) {
boundary.set_attributes(bnd_loc, sector * 45, distance); boundary.set_face_attributes(face, yaw_angle_deg, distance);
// update OA database // update OA database
database_push(boundary.get_angle(bnd_loc), distance); database_push(yaw_angle_deg, distance);
} else {
// reset distance for this face
boundary.reset_face(face);
} }
_last_update_ms = AP_HAL::millis(); _last_update_ms = AP_HAL::millis();
// update boundary used for Obstacle Avoidance
boundary.update_boundary(bnd_loc);
} }
// store upward distance // store upward distance
@ -117,10 +116,11 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
Matrix3f body_to_ned; Matrix3f body_to_ned;
const bool database_ready = database_prepare_for_push(current_pos, body_to_ned); const bool database_ready = database_prepare_for_push(current_pos, body_to_ned);
// initialise updated array and proximity sector angles (to closest object) and distances // variables to calculate closest angle and distance for each face
bool sector_updated[PROXIMITY_NUM_SECTORS]; AP_Proximity_Boundary_3D::Face face;
memset(sector_updated, 0, sizeof(sector_updated)); float face_distance;
boundary.reset_all_horizontal_sectors(); float face_yaw_deg;
bool face_distance_valid = false;
// 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++) {
@ -137,33 +137,25 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
const float packet_distance_m = distance_cm * 0.01f; const float packet_distance_m = distance_cm * 0.01f;
const float mid_angle = wrap_360((float)j * increment + yaw_correction); const float mid_angle = wrap_360((float)j * increment + yaw_correction);
// iterate over proximity sectors // get face for this latest reading
for (uint8_t i = 0; i < PROXIMITY_NUM_SECTORS; i++) { AP_Proximity_Boundary_3D::Face latest_face = boundary.get_face(mid_angle);
// update distance array sector with shortest distance from message if (latest_face != face) {
const float angle_diff = wrap_180(boundary._sector_middle_deg[i] - mid_angle); // store previous face
if (fabsf(angle_diff) > PROXIMITY_SECTOR_WIDTH_DEG*0.5f) { if (face_distance_valid) {
// not even in this sector boundary.set_face_attributes(face, face_yaw_deg, face_distance);
continue; } else {
} boundary.reset_face(face);
if (is_equal(angle_diff, -PROXIMITY_SECTOR_WIDTH_DEG*0.5f)) {
// on the upper boundary is *out* to avoid
// ambiguity. The distance is considered to be in
// the next sector. We should never be within an
// epsilon of the boundary, so is_equal should be
// safe.
continue;
}
if (packet_distance_m >= boundary.get_distance(i)) {
// this is no closer than a previous distance
// found from the packet
continue;
} }
// init for latest face
face = latest_face;
face_distance_valid = false;
}
// this is the shortest distance we've found in the packet so far // update minimum distance found so far
// create a location packet if (!face_distance_valid || (packet_distance_m < face_distance)) {
const boundary_location bnd_loc{i}; face_yaw_deg = mid_angle;
boundary.set_attributes(bnd_loc, mid_angle, packet_distance_m); face_distance = packet_distance_m;
sector_updated[i] = true; face_distance_valid = true;
} }
// update Object Avoidance database with Earth-frame point // update Object Avoidance database with Earth-frame point
@ -172,13 +164,13 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
} }
} }
// update proximity sectors validity and boundary point // process the last face
for (uint8_t i = 0; i < PROXIMITY_NUM_SECTORS; i++) { if (face_distance_valid) {
if (sector_updated[i]) { boundary.set_face_attributes(face, face_yaw_deg, face_distance);
const boundary_location bnd_loc{i}; } else {
boundary.update_boundary(bnd_loc); boundary.reset_face(face);
}
} }
return;
} }
if (msg.msgid == MAVLINK_MSG_ID_OBSTACLE_DISTANCE_3D) { if (msg.msgid == MAVLINK_MSG_ID_OBSTACLE_DISTANCE_3D) {
@ -193,7 +185,7 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
const uint32_t previous_msg_timestamp = _last_3d_msg_update_ms; const uint32_t previous_msg_timestamp = _last_3d_msg_update_ms;
_last_3d_msg_update_ms = packet.time_boot_ms; _last_3d_msg_update_ms = packet.time_boot_ms;
bool clear_fence = false; bool clear_fence = false;
// we will add on to the last fence if the time stamp is the same // 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 // 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)) { if ((previous_msg_timestamp != _last_3d_msg_update_ms) || (time_diff > PROXIMITY_3D_MSG_TIMEOUT_MS)) {
@ -202,14 +194,14 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
_distance_min = packet.min_distance; _distance_min = packet.min_distance;
_distance_max = packet.max_distance; _distance_max = packet.max_distance;
Vector3f current_pos; Vector3f current_pos;
Matrix3f body_to_ned; Matrix3f body_to_ned;
const bool database_ready = database_prepare_for_push(current_pos, body_to_ned); const bool database_ready = database_prepare_for_push(current_pos, body_to_ned);
if (clear_fence) { if (clear_fence) {
// cleared fence back to defaults since we have a new timestamp // cleared fence back to defaults since we have a new timestamp
boundary.reset_all_sectors_and_stacks(); boundary.reset();
} }
const Vector3f obstacle(packet.x, packet.y, packet.z); const Vector3f obstacle(packet.x, packet.y, packet.z);
@ -221,16 +213,15 @@ void AP_Proximity_MAV::handle_msg(const mavlink_message_t &msg)
const float yaw = wrap_360(degrees(atan2f(obstacle.y, obstacle.x))); 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))); const float pitch = wrap_180(degrees(M_PI_2 - atan2f(norm(obstacle.x, obstacle.y), obstacle.z)));
// allot them correct stack and sector based on calculated pitch and yaw // allot them correct layer and sector based on calculated pitch and yaw
const boundary_location bnd_loc = boundary.get_sector(yaw, pitch); const AP_Proximity_Boundary_3D::Face face = boundary.get_face(pitch, yaw);
float face_distance;
if (boundary.get_distance(bnd_loc) < obstacle.length()) { if (boundary.get_distance(face, face_distance) && (face_distance < obstacle.length())) {
// we already have a shorter distance in this stack and sector // we already have a shorter distance in this layer and sector
return; return;
} }
boundary.set_attributes(bnd_loc, yaw, pitch, obstacle.length()); boundary.set_face_attributes(face, yaw, pitch, obstacle.length());
boundary.update_boundary(bnd_loc);
if (database_ready) { if (database_ready) {
database_push(yaw, obstacle.length(),_last_update_ms, current_pos, body_to_ned, pitch); database_push(yaw, obstacle.length(),_last_update_ms, current_pos, body_to_ned, pitch);