diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h index ae4bb4f6c2..5d6d4dccaa 100644 --- a/libraries/AP_Proximity/AP_Proximity.h +++ b/libraries/AP_Proximity/AP_Proximity.h @@ -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) diff --git a/libraries/AP_Proximity/AP_Proximity_AirSimSITL.cpp b/libraries/AP_Proximity/AP_Proximity_AirSimSITL.cpp index 20265e3cf3..9f8434b791 100644 --- a/libraries/AP_Proximity/AP_Proximity_AirSimSITL.cpp +++ b/libraries/AP_Proximity/AP_Proximity_AirSimSITL.cpp @@ -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) { diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.cpp b/libraries/AP_Proximity/AP_Proximity_Backend.cpp index c73021afca..6be136d6a8 100644 --- a/libraries/AP_Proximity/AP_Proximity_Backend.cpp +++ b/libraries/AP_Proximity/AP_Proximity_Backend.cpp @@ -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 ¤t_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 ¤t_pos, const Matrix3f &body_to_ned) { AP_OADatabase *oaDb = AP::oadatabase(); if (oaDb == nullptr || !oaDb->healthy()) { diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.h b/libraries/AP_Proximity/AP_Proximity_Backend.h index 320fbea0b6..3dd84c9e24 100644 --- a/libraries/AP_Proximity/AP_Proximity_Backend.h +++ b/libraries/AP_Proximity/AP_Proximity_Backend.h @@ -80,8 +80,11 @@ protected: static bool database_prepare_for_push(Vector3f ¤t_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 ¤t_pos, const Matrix3f &body_to_ned, float pitch = 0.0f); - + static void database_push(float angle, float distance, uint32_t timestamp_ms, const Vector3f ¤t_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 ¤t_pos, const Matrix3f &body_to_ned); + AP_Proximity &frontend; AP_Proximity::Proximity_State &state; // reference to this instances state diff --git a/libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp b/libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp index faeab7eb66..da9893708f 100644 --- a/libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp +++ b/libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp @@ -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]; diff --git a/libraries/AP_Proximity/AP_Proximity_Boundary_3D.h b/libraries/AP_Proximity/AP_Proximity_Boundary_3D.h index 32d3775a82..db7224ee95 100644 --- a/libraries/AP_Proximity/AP_Proximity_Boundary_3D.h +++ b/libraries/AP_Proximity/AP_Proximity_Boundary_3D.h @@ -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]; diff --git a/libraries/AP_Proximity/AP_Proximity_MAV.cpp b/libraries/AP_Proximity/AP_Proximity_MAV.cpp index 7da2c2eebe..e99700a058 100644 --- a/libraries/AP_Proximity/AP_Proximity_MAV.cpp +++ b/libraries/AP_Proximity/AP_Proximity_MAV.cpp @@ -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; +} diff --git a/libraries/AP_Proximity/AP_Proximity_MAV.h b/libraries/AP_Proximity/AP_Proximity_MAV.h index 7ede3f2ea4..13f80a2e63 100644 --- a/libraries/AP_Proximity/AP_Proximity_MAV.h +++ b/libraries/AP_Proximity/AP_Proximity_MAV.h @@ -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