2020-12-06 08:20:02 -04:00
/*
This program is free software : you can redistribute it and / or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation , either version 3 of the License , or
( at your option ) any later version .
This program is distributed in the hope that it will be useful ,
but WITHOUT ANY WARRANTY ; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE . See the
GNU General Public License for more details .
You should have received a copy of the GNU General Public License
along with this program . If not , see < http : //www.gnu.org/licenses/>.
*/
# pragma once
2021-02-27 12:49:27 -04:00
# include <Filter/LowPassFilter.h>
2020-12-06 08:20:02 -04:00
# define PROXIMITY_NUM_SECTORS 8 // number of sectors
2020-12-14 02:51:18 -04:00
# 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
2021-01-10 14:14:24 -04:00
# define PROXIMITY_SECTOR_WIDTH_DEG (360.0f / PROXIMITY_NUM_SECTORS) // width of sectors in degrees
2020-12-06 08:20:02 -04:00
# 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
2021-02-27 12:49:27 -04:00
# define PROXIMITY_FILT_RESET_TIME 1000 // reset filter if last distance was pushed more than this many ms away
2020-12-06 08:20:02 -04:00
class AP_Proximity_Boundary_3D
{
public :
// constructor. This incorporates initialisation as well.
AP_Proximity_Boundary_3D ( ) ;
2020-12-14 02:51:18 -04:00
// stores the layer and sector as a single object to access and modify the 3-D boundary
2021-01-10 14:14:24 -04:00
// Objects of this class are used temporarily to modify the boundary, i,e they are not persistant or stored anywhere
2020-12-14 02:51:18 -04:00
class Face
{
2020-12-06 08:20:02 -04:00
public :
2020-12-14 02:51:18 -04:00
// constructor, invalidate id and distance
Face ( ) { layer = sector = UINT8_MAX ; }
Face ( uint8_t _layer , uint8_t _sector ) { layer = _layer ; sector = _sector ; }
// return true if face has valid layer and sector values
bool valid ( ) const { return ( ( layer < PROXIMITY_NUM_LAYERS ) & & ( sector < PROXIMITY_NUM_SECTORS ) ) ; }
// comparison operator
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 ) ) ; }
2021-01-10 14:14:24 -04:00
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.
2020-12-14 02:51:18 -04:00
} ;
// 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?)
// 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)
Face get_face ( float pitch , float yaw ) const ;
Face get_face ( float yaw ) const { return get_face ( 0 , yaw ) ; }
2020-12-06 08:20:02 -04:00
// Set the actual body-frame angle(yaw), pitch, and distance of the detected object.
2020-12-14 02:51:18 -04:00
// This method will also mark the sector and layer to be "valid",
2020-12-06 08:20:02 -04:00
// This distance can then be used for Obstacle Avoidance
2020-12-14 02:51:18 -04:00
// Assume detected obstacle is horizontal (zero pitch), if no pitch is passed
2021-01-10 14:14:24 -04:00
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 ) ; }
2020-12-14 02:51:18 -04:00
// 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
void add_distance ( float pitch , float yaw , float distance ) ;
void add_distance ( float yaw , float distance ) { add_distance ( 0 , yaw , distance ) ; }
2020-12-06 08:20:02 -04:00
2020-12-14 02:51:18 -04:00
// update boundary points used for simple avoidance based on a single sector and pitch distance changing
2020-12-06 08:20:02 -04:00
// 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
2021-01-10 14:14:24 -04:00
void update_boundary ( const Face & face ) ;
2020-12-06 08:20:02 -04:00
2020-12-14 02:51:18 -04:00
// update middle layer boundary points
void update_middle_boundary ( ) ;
// reset boundary. marks all distances as invalid
void reset ( ) ;
// Reset this location, specified by Face object, back to default
2021-01-10 14:14:24 -04:00
// i.e Distance is marked as not-valid
void reset_face ( const Face & face ) ;
2020-12-14 02:51:18 -04:00
// get distance for a face. returns true on success and fills in distance argument with distance in meters
2021-01-10 14:14:24 -04:00
bool get_distance ( const Face & face , float & distance ) const ;
2020-12-14 02:51:18 -04:00
2020-12-06 08:20:02 -04:00
// Get the total number of obstacles
2020-12-27 12:21:58 -04:00
uint8_t get_obstacle_count ( ) const ;
2020-12-06 08:20:02 -04:00
2021-01-10 14:14:24 -04:00
// Returns a body frame vector (in cm) to an obstacle
// False is returned if the obstacle_num provided does not produce a valid obstacle
2020-12-27 12:21:58 -04:00
bool get_obstacle ( uint8_t obstacle_num , Vector3f & vec_to_boundary ) const ;
2020-12-06 08:20:02 -04:00
2021-01-10 14:14:24 -04:00
// 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
2020-12-06 08:20:02 -04:00
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 ;
2021-02-27 12:49:27 -04:00
// get number of layers
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 ;
2020-12-06 08:20:02 -04:00
// sectors
2021-01-10 14:14:24 -04:00
static_assert ( PROXIMITY_NUM_SECTORS = = 8 , " PROXIMITY_NUM_SECTOR must be 8 " ) ;
2020-12-06 08:20:02 -04:00
const uint16_t _sector_middle_deg [ PROXIMITY_NUM_SECTORS ] { 0 , 45 , 90 , 135 , 180 , 225 , 270 , 315 } ; // middle angle of each sector
// layers
2021-01-10 14:14:24 -04:00
static_assert ( PROXIMITY_NUM_LAYERS = = 5 , " PROXIMITY_NUM_LAYERS must be 5 " ) ;
2020-12-06 08:20:02 -04:00
const int16_t _pitch_middle_deg [ PROXIMITY_NUM_LAYERS ] { - 60 , - 30 , 0 , 30 , 60 } ;
private :
2020-12-14 02:51:18 -04:00
2020-12-06 08:20:02 -04:00
// initialise the boundary and sector_edge_vector array used for object avoidance
2020-12-14 02:51:18 -04:00
void init ( ) ;
2020-12-27 12:21:58 -04:00
// get the next sector which is CW to the passed sector
uint8_t get_next_sector ( uint8_t sector ) const { return ( ( sector > = PROXIMITY_NUM_SECTORS - 1 ) ? 0 : sector + 1 ) ; }
// get the prev sector which is CCW to the passed sector
uint8_t get_prev_sector ( uint8_t sector ) const { return ( ( sector < = 0 ) ? PROXIMITY_NUM_SECTORS - 1 : sector - 1 ) ; }
// Converts obstacle_num passed from avoidance library into appropriate face of the boundary
// Returns false if the face is invalid
// "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"
2021-01-10 14:14:24 -04:00
bool convert_obstacle_num_to_face ( uint8_t obstacle_num , Face & face ) const WARN_IF_UNUSED ;
2020-12-14 02:51:18 -04:00
2021-02-27 12:49:27 -04:00
// Apply low pass filter on the raw distance
void set_filtered_distance ( const Face & face , float distance ) ;
// Return filtered distance for the passed in face
bool get_filtered_distance ( const Face & face , float & distance ) const ;
2020-12-14 02:51:18 -04:00
Vector3f _sector_edge_vector [ PROXIMITY_NUM_LAYERS ] [ PROXIMITY_NUM_SECTORS ] ;
Vector3f _boundary_points [ PROXIMITY_NUM_LAYERS ] [ PROXIMITY_NUM_SECTORS ] ;
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
float _distance [ PROXIMITY_NUM_LAYERS ] [ PROXIMITY_NUM_SECTORS ] ; // distance to closest object within each sector and layer
bool _distance_valid [ PROXIMITY_NUM_LAYERS ] [ PROXIMITY_NUM_SECTORS ] ; // true if a valid distance received for each sector and layer
2021-02-27 12:49:27 -04:00
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
2020-12-06 08:20:02 -04:00
} ;