Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : #pragma once
17 :
18 : #include <AP_Common/AP_Common.h>
19 : #include <AP_Math/AP_Math.h>
20 : #include <Filter/LowPassFilter.h>
21 :
22 : #define PROXIMITY_NUM_SECTORS 8 // number of sectors
23 : #define PROXIMITY_NUM_LAYERS 5 // num of layers in a sector
24 : #define PROXIMITY_MIDDLE_LAYER 2 // middle layer
25 : #define PROXIMITY_PITCH_WIDTH_DEG 30 // width between each layer in degrees
26 : #define PROXIMITY_SECTOR_WIDTH_DEG (360.0f/PROXIMITY_NUM_SECTORS) // width of sectors in degrees
27 : #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.
28 : #define PROXIMITY_BOUNDARY_DIST_DEFAULT 100 // if we have no data for a sector, boundary is placed 100m out
29 : #define PROXIMITY_FILT_RESET_TIME 1000 // reset filter if last distance was pushed more than this many ms away
30 : #define PROXIMITY_FACE_RESET_MS 1000 // face will be reset if not updated within this many ms
31 :
32 : // structure holding distances in PROXIMITY_MAX_DIRECTION directions. used for sending distances to ground station
33 : #define PROXIMITY_MAX_DIRECTION 8
34 : struct Proximity_Distance_Array {
35 : uint8_t orientation[PROXIMITY_MAX_DIRECTION]; // orientation (i.e. rough direction) of the distance (see MAV_SENSOR_ORIENTATION)
36 : float distance[PROXIMITY_MAX_DIRECTION]; // distance in meters
37 23512 : bool valid(uint8_t offset) const {
38 : // returns true if the distance stored at offset is valid
39 23512 : return (offset < 8 && (offset_valid & (1U<<offset)));
40 : };
41 :
42 : uint8_t offset_valid; // bitmask
43 : };
44 :
45 : class AP_Proximity_Boundary_3D
46 : {
47 : public:
48 : // constructor. This incorporates initialisation as well.
49 : AP_Proximity_Boundary_3D();
50 :
51 : // stores the layer and sector as a single object to access and modify the 3-D boundary
52 : // Objects of this class are used temporarily to modify the boundary, i,e they are not persistant or stored anywhere
53 : class Face
54 : {
55 : public:
56 :
57 : // constructor, invalidate id and distance
58 80263584 : Face() { layer = sector = UINT8_MAX; }
59 756068 : Face(uint8_t _layer, uint8_t _sector) { layer = _layer; sector = _sector; }
60 :
61 : // return true if face has valid layer and sector values
62 1697580 : bool valid() const { return ((layer < PROXIMITY_NUM_LAYERS) && (sector < PROXIMITY_NUM_SECTORS)); }
63 :
64 : // comparison operator
65 : bool operator ==(const Face &other) const { return ((layer == other.layer) && (sector == other.sector)); }
66 97160 : bool operator !=(const Face &other) const { return ((layer != other.layer) || (sector != other.sector)); }
67 :
68 : 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
69 : uint8_t sector; // horizontal "steps" on the 3D Boundary. 0th sector is directly in front of the vehicle. Each sector is 45 degrees wide.
70 : };
71 :
72 : // returns face corresponding to the provided yaw and (optionally) pitch
73 : // pitch is the vertical body-frame angle (in degrees) to the obstacle (0=directly ahead, 90 is above the vehicle?)
74 : // 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)
75 : Face get_face(float pitch, float yaw) const;
76 406993 : Face get_face(float yaw) const { return get_face(0, yaw); }
77 :
78 : // Set the actual body-frame angle(yaw), pitch, and distance of the detected object.
79 : // This method will also mark the sector and layer to be "valid",
80 : // This distance can then be used for Obstacle Avoidance
81 : // Assume detected obstacle is horizontal (zero pitch), if no pitch is passed
82 : // prx_instance should be set to the proximity sensor backend instance number
83 : void set_face_attributes(const Face &face, float pitch, float yaw, float distance, uint8_t prx_instance);
84 314496 : void set_face_attributes(const Face &face, float yaw, float distance, uint8_t prx_instance) { set_face_attributes(face, 0, yaw, distance, prx_instance); }
85 :
86 : // update boundary points used for simple avoidance based on a single sector and pitch distance changing
87 : // the boundary points lie on the line between sectors meaning two boundary points may be updated based on a single sector's distance changing
88 : // the boundary point is set to the shortest distance found in the two adjacent sectors, this is a conservative boundary around the vehicle
89 : void update_boundary(const Face &face);
90 :
91 : // reset boundary. marks all distances as invalid
92 : void reset();
93 :
94 : // Reset this location, specified by Face object, back to default
95 : // i.e Distance is marked as not-valid
96 : // prx_instance should be set to the proximity sensor's backend instance number
97 : void reset_face(const Face &face, uint8_t prx_instance);
98 :
99 : // check if a face has valid distance even if it was updated a long time back
100 : void check_face_timeout();
101 :
102 : // get distance for a face. returns true on success and fills in distance argument with distance in meters
103 : bool get_distance(const Face &face, float &distance) const;
104 :
105 : // Get the total number of obstacles
106 : uint8_t get_obstacle_count() const;
107 :
108 : // Returns a body frame vector (in cm) to an obstacle
109 : // False is returned if the obstacle_num provided does not produce a valid obstacle
110 : bool get_obstacle(uint8_t obstacle_num, Vector3f& vec_to_boundary) const;
111 :
112 : // Returns a body frame vector (in cm) nearest to obstacle, in betwen seg_start and seg_end
113 : // True is returned if the segment intersects a plane formed by considering the "closest point" as normal vector to the plane.
114 : bool closest_point_from_segment_to_obstacle(uint8_t obstacle_num, const Vector3f& seg_start, const Vector3f& seg_end, Vector3f& closest_point) const;
115 :
116 : // get distance and angle to closest object (used for pre-arm check)
117 : // returns true on success, false if no valid readings
118 : bool get_closest_object(float& angle_deg, float &distance) const;
119 :
120 : // get number of objects horizontally
121 : uint8_t get_horizontal_object_count() const;
122 : bool get_horizontal_object_angle_and_distance(uint8_t object_number, float& angle_deg, float &distance) const;
123 :
124 : // get obstacle info for AP_Periph
125 : bool get_obstacle_info(uint8_t obstacle_num, float &angle_deg, float &pitch_deg, float &distance) const;
126 :
127 : // get number of layers
128 48192 : uint8_t get_num_layers() const { return PROXIMITY_NUM_LAYERS; }
129 :
130 : // get raw and filtered distances in 8 directions per layer.
131 : bool get_layer_distances(uint8_t layer_number, float dist_max, Proximity_Distance_Array &prx_dist_array, Proximity_Distance_Array &prx_filt_dist_array) const;
132 :
133 : // pass down filter cut-off freq from params
134 15835009 : void set_filter_freq(float filt_freq) { _filter_freq = filt_freq; }
135 :
136 : // sectors
137 : static_assert(PROXIMITY_NUM_SECTORS == 8, "PROXIMITY_NUM_SECTOR must be 8");
138 : const uint16_t _sector_middle_deg[PROXIMITY_NUM_SECTORS] {0, 45, 90, 135, 180, 225, 270, 315}; // middle angle of each sector
139 : // layers
140 : static_assert(PROXIMITY_NUM_LAYERS == 5, "PROXIMITY_NUM_LAYERS must be 5");
141 : const int16_t _pitch_middle_deg[PROXIMITY_NUM_LAYERS] {-60, -30, 0, 30, 60};
142 :
143 : private:
144 :
145 : // initialise the boundary and sector_edge_vector array used for object avoidance
146 : void init();
147 :
148 : // get the next sector which is CW to the passed sector
149 240455253 : uint8_t get_next_sector(uint8_t sector) const {return ((sector >= PROXIMITY_NUM_SECTORS-1) ? 0 : sector+1); }
150 :
151 : // get the prev sector which is CCW to the passed sector
152 630020 : uint8_t get_prev_sector(uint8_t sector) const {return ((sector <= 0) ? PROXIMITY_NUM_SECTORS-1 : sector-1); }
153 :
154 : // Converts obstacle_num passed from avoidance library into appropriate face of the boundary
155 : // Returns false if the face is invalid
156 : // "update_boundary" method manipulates two sectors ccw and one sector cw from any valid face.
157 : // Any boundary that does not fall into these manipulated faces are useless, and will be marked as false
158 : // The resultant is packed into a Boundary Location object and returned by reference as "face"
159 : bool convert_obstacle_num_to_face(uint8_t obstacle_num, Face& face) const WARN_IF_UNUSED;
160 :
161 : // Apply a new cutoff_freq to low-pass filter
162 : void apply_filter_freq(float cutoff_freq);
163 :
164 : // Apply low pass filter on the raw distance
165 : void set_filtered_distance(const Face &face, float distance);
166 :
167 : // Return filtered distance for the passed in face
168 : bool get_filtered_distance(const Face &face, float &distance) const;
169 :
170 : Vector3f _sector_edge_vector[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS];
171 : Vector3f _boundary_points[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS];
172 :
173 : float _angle_deg[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // yaw angle in degrees to closest object within each sector and layer
174 : float _pitch_deg[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // pitch angle in degrees to the closest object within each sector and layer
175 : float _distance[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // distance to closest object within each sector and layer
176 : bool _distance_valid[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // true if a valid distance received for each sector and layer
177 : uint32_t _last_update_ms[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // time when distance was last updated
178 : uint8_t _prx_instance[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // proximity sensor backend instance that provided the distance
179 : LowPassFilterFloat _filtered_distance[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // low pass filter
180 : float _filter_freq; // cutoff freq of low pass filter
181 : uint32_t _last_check_face_timeout_ms; // system time to throttle check_face_timeout method
182 : };
183 :
184 : // This class gives an easy way of making a temporary boundary, used for "sorting" distances.
185 : // When unknown number of distances at various orientations are sent we store the least distance in the temporary boundary.
186 : // After all the messages are received, we copy the contents of the temporary boundary and put it in the main 3-D boundary.
187 : class AP_Proximity_Temp_Boundary
188 : {
189 : public:
190 : // constructor. This incorporates initialisation as well.
191 4 : AP_Proximity_Temp_Boundary() { reset(); }
192 :
193 : // reset the temporary boundary. This fills in distances with FLT_MAX
194 : void reset();
195 :
196 : // add a distance to the temp boundary if it is shorter than any other provided distance since the last time the boundary was reset
197 : // pitch and yaw are in degrees, distance is in meters
198 : void add_distance(const AP_Proximity_Boundary_3D::Face &face, float pitch_deg, float yaw_deg, float distance_m);
199 81 : void add_distance(const AP_Proximity_Boundary_3D::Face &face, float yaw_deg, float distance_m) { add_distance(face, 0.0f, yaw_deg, distance_m); }
200 :
201 : // fill the original 3D boundary with the contents of this temporary boundary
202 : // prx_instance should be set to the proximity sensor's backend instance number
203 : void update_3D_boundary(uint8_t prx_instance, AP_Proximity_Boundary_3D &boundary);
204 :
205 : private:
206 :
207 : float _distances[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // distance to closest object within each sector and layer. Will start with FLT_MAX, and then be changed to a valid distance if needed
208 : float _angle_deg[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // yaw angle in degrees to closest object within each sector and layer
209 : float _pitch_deg[PROXIMITY_NUM_LAYERS][PROXIMITY_NUM_SECTORS]; // pitch angle in degrees to the closest object within each sector and layer
210 : };
|
