diff --git a/libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp b/libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp new file mode 100644 index 0000000000..a84fda1431 --- /dev/null +++ b/libraries/AP_Proximity/AP_Proximity_Boundary_3D.cpp @@ -0,0 +1,262 @@ +#include "AP_Proximity_Backend.h" +#include "AP_Proximity_Boundary_3D.h" + +/* + Constructor. + This incorporates initialisation as well. +*/ +AP_Proximity_Boundary_3D::AP_Proximity_Boundary_3D() +{ + // initialise sector edge vector used for building the boundary fence + init_boundary(); +} + +// initialise the boundary and sector_edge_vector array used for object avoidance +// should be called if the sector_middle_deg or _sector_width_deg arrays are changed +void AP_Proximity_Boundary_3D::init_boundary() +{ + for (uint8_t stack = 0; stack < PROXIMITY_NUM_LAYERS; stack ++) { + 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[stack]); + _sector_edge_vector[sector][stack].offset_bearing(angle_rad, pitch, 100.0f); + _boundary_points[sector][stack] = _sector_edge_vector[sector][stack] * PROXIMITY_BOUNDARY_DIST_DEFAULT; + } + } +} + +// returns Boundary_Location object consisting of appropriate stack and sector corresponding to the yaw and pitch. +// Pitch defaults to zero if only yaw is passed to this method +// Yaw is the horizontal body-frame angle the detected object makes with the vehicle +// Pitch is the vertical body-frame angle the detected object makes with the vehicle +boundary_location AP_Proximity_Boundary_3D::get_sector(float yaw, float pitch) +{ + const uint8_t sector = wrap_360(yaw + (PROXIMITY_SECTOR_WIDTH_DEG * 0.5f)) / 45.0f; + const float pitch_degrees = constrain_float(pitch, -75.0f, 74.9f); + const uint8_t stack = (pitch_degrees + 75.0f)/PROXIMITY_PITCH_WIDTH_DEG; + return boundary_location{sector, stack}; +} + +// Set the actual body-frame angle(yaw), pitch, and distance of the detected object. +// This method will also mark the sector and stack to be "valid", so this distance can be used for Obstacle Avoidance +void AP_Proximity_Boundary_3D::set_attributes(const Boundary_Location& bnd_loc, float angle, float pitch, float distance) +{ + const uint8_t sector = bnd_loc.sector; + const uint8_t stack = bnd_loc.stack; + _angle[sector][stack] = angle; + _pitch[sector][stack] = pitch; + _distance[sector][stack] = distance; + _distance_valid[sector][stack] = true; +} + +// 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 Boundary_Location& bnd_loc) +{ + const uint8_t sector = bnd_loc.sector; + const uint8_t layer = bnd_loc.stack; + + // sanity check + if (sector >= PROXIMITY_NUM_SECTORS) { + return; + } + + // find adjacent sector (clockwise) + uint8_t next_sector = sector + 1; + if (next_sector >= PROXIMITY_NUM_SECTORS) { + next_sector = 0; + } + + // boundary point lies on the line between the two sectors at the shorter distance found in the two sectors + float shortest_distance = PROXIMITY_BOUNDARY_DIST_DEFAULT; + if (_distance_valid[sector][layer] && _distance_valid[next_sector][layer]) { + shortest_distance = MIN(_distance[sector][layer], _distance[next_sector][layer]); + } else if (_distance_valid[sector][layer]) { + shortest_distance = _distance[sector][layer]; + } else if (_distance_valid[next_sector][layer]) { + shortest_distance = _distance[next_sector][layer]; + } + if (shortest_distance < PROXIMITY_BOUNDARY_DIST_MIN) { + shortest_distance = PROXIMITY_BOUNDARY_DIST_MIN; + } + _boundary_points[sector][layer] = _sector_edge_vector[sector][layer] * shortest_distance; + + // if the next sector (clockwise) has an invalid distance, set boundary to create a cup like boundary + if (!_distance_valid[next_sector][layer]) { + _boundary_points[next_sector][layer] = _sector_edge_vector[next_sector][layer] * shortest_distance; + } + + // repeat for edge between sector and previous sector + uint8_t prev_sector = (sector == 0) ? PROXIMITY_NUM_SECTORS-1 : sector-1; + shortest_distance = PROXIMITY_BOUNDARY_DIST_DEFAULT; + if (_distance_valid[prev_sector][layer] && _distance_valid[sector][layer]) { + shortest_distance = MIN(_distance[prev_sector][layer], _distance[sector][layer]); + } else if (_distance_valid[prev_sector][layer]) { + shortest_distance = _distance[prev_sector][layer]; + } else if (_distance_valid[sector][layer]) { + shortest_distance = _distance[sector][layer]; + } + _boundary_points[prev_sector][layer] = _sector_edge_vector[prev_sector][layer] * shortest_distance; + + // if the sector counter-clockwise from the previous sector has an invalid distance, set boundary to create a cup like boundary + uint8_t prev_sector_ccw = (prev_sector == 0) ? PROXIMITY_NUM_SECTORS - 1 : prev_sector - 1; + if (!_distance_valid[prev_sector_ccw][layer]) { + _boundary_points[prev_sector_ccw][layer] = _sector_edge_vector[prev_sector_ccw][layer] * shortest_distance; + } +} + +// Reset this location, specified by Boundary_Location object, back to default +// i.e Distance is marked as not-valid, and set to a large number. +void AP_Proximity_Boundary_3D::reset_sector(const Boundary_Location& bnd_loc) +{ + _distance[bnd_loc.sector][bnd_loc.stack] = DISTANCE_MAX; + _distance_valid[bnd_loc.sector][bnd_loc.stack] = false; +} + +// Reset all horizontal sectors +// i.e Distance is marked as not-valid, and set to a large number for all horizontal sectors. +void AP_Proximity_Boundary_3D::reset_all_horizontal_sectors() +{ + for (uint8_t i=0; i < PROXIMITY_NUM_SECTORS; i++) { + const Boundary_Location bnd_loc{i}; + reset_sector(bnd_loc); + } +} + +// Reset all stacks and sectors +// i.e Distance is marked as not-valid, and set to a large number for all stacks and sectors. +void AP_Proximity_Boundary_3D::reset_all_sectors_and_stacks() +{ + for (uint8_t j=0; j < PROXIMITY_NUM_LAYERS; j++) { + for (uint8_t i=0; i < PROXIMITY_NUM_SECTORS; i++) { + const Boundary_Location bnd_loc{i, j}; + reset_sector(bnd_loc); + } + } +} + +// get the total number of obstacles +// this method iterates through the entire 3-D boundary and checks which layer has atleast one valid distance +uint8_t AP_Proximity_Boundary_3D::get_obstacle_count() +{ + uint8_t obstacle_count = 0; + // reset entire array to false + memset(_active_layer, 0, sizeof(_active_layer)); + // check if this layer has atleast one valid sector + for (uint8_t j=0; j= PROXIMITY_NUM_SECTORS) { + sector_start = 0; + } + const Vector3f start = _boundary_points[sector_start][bnd_loc.stack]; + const Vector3f end = _boundary_points[sector_end][bnd_loc.stack]; + vec_to_obstacle = Vector3f::closest_point_between_line_and_point(start, end, Vector3f{0.0f, 0.0f, 0.0f}); +} + +// WARNING: This requires get_obstacle_count() to be called before calling this method +// Appropriate stack and sector are found from the passed obstacle_num +// This function then draws a line between this sector, and sector + 1 at the given stack +// 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" +float AP_Proximity_Boundary_3D::distance_to_obstacle(uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const +{ + const boundary_location bnd_loc = convert_obstacle_num_to_boundary_loc(obstacle_num); + const uint8_t sector_end = bnd_loc.sector; + uint8_t sector_start = bnd_loc.sector + 1; + if (sector_start >= PROXIMITY_NUM_SECTORS) { + sector_start = 0; + } + const Vector3f start = _boundary_points[sector_start][bnd_loc.stack]; + const Vector3f end = _boundary_points[sector_end][bnd_loc.stack]; + return Vector3f::segment_to_segment_dist(seg_start, seg_end, start, 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_Boundary_3D::get_closest_object(float& angle_deg, float &distance) const +{ + bool sector_found = false; + uint8_t sector = 0; + uint8_t stack = 0; + + // check boundary for shortest distance + // only check for middle layers and higher + // lower layers might contain ground, which will give false pre-arm failure + for (uint8_t j=PROXIMITY_MIDDLE_LAYER; j. + */ + +#pragma once + +#define PROXIMITY_NUM_SECTORS 8 // number of sectors +#define PROXIMITY_NUM_LAYERS 5 // num of stacks in a sector +#define PROXIMITY_MIDDLE_LAYER 2 // middle stack +#define PROXIMITY_PITCH_WIDTH_DEG 30 // width between each stack in degrees +#define PROXIMITY_SECTOR_WIDTH_DEG 45.0f // 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 +#define DISTANCE_MAX 999999.0f // arbritary "large" distance + +class AP_Proximity_Boundary_3D +{ +public: + // constructor. This incorporates initialisation as well. + AP_Proximity_Boundary_3D(); + + // This class is used to store the stack and sector as a single packet to access and modify the 3-D boundary + class Boundary_Location + { + public: + // constructor when both stack and sector are passed + Boundary_Location(uint8_t Sector, uint8_t Stack) { sector = Sector; stack = Stack; } + // constructor defaults to "middle(horizontal) layer" if only sector is passed + Boundary_Location(uint8_t Sector) { sector = Sector; stack = PROXIMITY_MIDDLE_LAYER; } + + uint8_t stack; // vertical "steps" on the 3D Boundary + uint8_t sector; // horizontal "steps" on the 3D Boundary + }; + + // returns Boundary_Location object consisting of appropriate stack and sector + // corresponding to the yaw and pitch. + // Pitch defaults to zero if only yaw is passed to this method + // Yaw is the horizontal body-frame angle the detected object makes with the vehicle + // Pitch is the vertical body-frame angle the detected object makes with the vehicle + Boundary_Location get_sector(float yaw, float pitch = 0.0f); + + // Set the actual body-frame angle(yaw), pitch, and distance of the detected object. + // This method will also mark the sector and stack to be "valid" + // This distance can then be used for Obstacle Avoidance + void set_attributes(const Boundary_Location& bnd_loc, float angle, float pitch, float distance); + + // Set the actual body-frame angle(yaw), pitch, and distance of the detected object. + // This method will also mark the sector and stack 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_attributes(const Boundary_Location& bnd_loc, float angle, float distance) { set_attributes(bnd_loc, angle, 0.0f, distance); } + + // 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 update_boundary(const Boundary_Location& bnd_loc); + + // Reset this location, specified by Boundary_Location object, back to default + // i.e Distance is marked as not-valid, and set to a large number. + void reset_sector(const Boundary_Location& bnd_loc); + // Reset all horizontal sectors + void reset_all_horizontal_sectors(); + // Reset all stacks and sectors + void reset_all_sectors_and_stacks(); + + // Get values given the stack and sector as a Boundary_Location object + float get_angle(const Boundary_Location& bnd_loc) const { return _angle[bnd_loc.sector][bnd_loc.stack]; } + float get_pitch(const Boundary_Location& bnd_loc) const { return _pitch[bnd_loc.sector][bnd_loc.stack]; } + float get_distance(const Boundary_Location& bnd_loc) const { return _distance[bnd_loc.sector][bnd_loc.stack]; } + bool check_distance_valid(const Boundary_Location& bnd_loc) const { return _distance_valid[bnd_loc.sector][bnd_loc.stack]; } + + // Get the total number of obstacles + // This method iterates through the entire 3-D boundary and checks which layer has atleast one valid distance + uint8_t get_obstacle_count(); + + // WARNING: This requires get_obstacle_count() to be called before calling this method + // Appropriate stack and sector are found from the passed obstacle_num + // This function then draws a line between this sector, and sector + 1 at the given stack + // Then returns the closest point on this line from vehicle, in body-frame. + void get_obstacle(uint8_t obstacle_num, Vector3f& vec_to_boundary) const; + + // WARNING: This requires get_obstacle_count() to be called before calling this method + // Appropriate stack and sector are found from the passed obstacle_num + // This function then draws a line between this sector, and sector + 1 at the given stack + // 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" + 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) + // returns true on success, false if no valid readings + bool get_closest_object(float& angle_deg, float &distance) const; + + // get number of objects, angle and distance - used for non-GPS avoidance + uint8_t get_horizontal_object_count() const; + bool get_horizontal_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const; + + // sectors + const uint16_t _sector_middle_deg[PROXIMITY_NUM_SECTORS] {0, 45, 90, 135, 180, 225, 270, 315}; // middle angle of each sector + // layers + const int16_t _pitch_middle_deg[PROXIMITY_NUM_LAYERS] {-60, -30, 0, 30, 60}; + +private: + // initialise the boundary and sector_edge_vector array used for object avoidance + void init_boundary(); + + // Converts obstacle_num passed from avoidance library into appropriate stack and sector + // This is packed into a Boundary Location object and returned + Boundary_Location convert_obstacle_num_to_boundary_loc(uint8_t obstacle_num) const; + + Vector3f _sector_edge_vector[PROXIMITY_NUM_SECTORS][PROXIMITY_NUM_LAYERS]; + Vector3f _boundary_points[PROXIMITY_NUM_SECTORS][PROXIMITY_NUM_LAYERS]; + + // sensor data + float _angle[PROXIMITY_NUM_SECTORS][PROXIMITY_NUM_LAYERS]; // angle to closest object within each sector and stack + float _pitch[PROXIMITY_NUM_SECTORS][PROXIMITY_NUM_LAYERS]; // pitch to the closest object within each sector and stack + float _distance[PROXIMITY_NUM_SECTORS][PROXIMITY_NUM_LAYERS]; // distance to closest object within each sector and stack + bool _distance_valid[PROXIMITY_NUM_SECTORS][PROXIMITY_NUM_LAYERS]; // true if a valid distance received for each sector and stack + bool _active_layer[PROXIMITY_NUM_LAYERS]; // layers which have atleast one valid distance are marked true +}; + +typedef AP_Proximity_Boundary_3D::Boundary_Location boundary_location;