Line data Source code
1 : #pragma once
2 :
3 : #include "AC_Avoidance_config.h"
4 :
5 : #if AP_AVOIDANCE_ENABLED
6 :
7 : #include <AP_Common/AP_Common.h>
8 : #include <AP_Param/AP_Param.h>
9 : #include <AP_Math/AP_Math.h>
10 : #include <AC_AttitudeControl/AC_AttitudeControl.h> // Attitude controller library for sqrt controller
11 :
12 : #define AC_AVOID_ACCEL_CMSS_MAX 100.0f // maximum acceleration/deceleration in cm/s/s used to avoid hitting fence
13 :
14 : // bit masks for enabled fence types.
15 : #define AC_AVOID_DISABLED 0 // avoidance disabled
16 : #define AC_AVOID_STOP_AT_FENCE 1 // stop at fence
17 : #define AC_AVOID_USE_PROXIMITY_SENSOR 2 // stop based on proximity sensor output
18 : #define AC_AVOID_STOP_AT_BEACON_FENCE 4 // stop based on beacon perimeter
19 : #define AC_AVOID_DEFAULT (AC_AVOID_STOP_AT_FENCE | AC_AVOID_USE_PROXIMITY_SENSOR)
20 :
21 : // definitions for non-GPS avoidance
22 : #define AC_AVOID_NONGPS_DIST_MAX_DEFAULT 5.0f // objects over 5m away are ignored (default value for DIST_MAX parameter)
23 : #define AC_AVOID_ANGLE_MAX_PERCENT 0.75f // object avoidance max lean angle as a percentage (expressed in 0 ~ 1 range) of total vehicle max lean angle
24 :
25 : #define AC_AVOID_ACTIVE_LIMIT_TIMEOUT_MS 500 // if limiting is active if last limit is happened in the last x ms
26 : #define AC_AVOID_ACCEL_TIMEOUT_MS 200 // stored velocity used to calculate acceleration will be reset if avoidance is active after this many ms
27 :
28 : /*
29 : * This class prevents the vehicle from leaving a polygon fence or hitting proximity-based obstacles
30 : * Additionally the vehicle may back up if the margin to obstacle is breached
31 : */
32 : class AC_Avoid {
33 : public:
34 : AC_Avoid();
35 :
36 : /* Do not allow copies */
37 : CLASS_NO_COPY(AC_Avoid);
38 :
39 : // get singleton instance
40 8815316 : static AC_Avoid *get_singleton() {
41 8815316 : return _singleton;
42 : }
43 :
44 : // return true if any avoidance feature is enabled
45 : bool enabled() const { return _enabled != AC_AVOID_DISABLED; }
46 :
47 : // parameter conversion
48 : void convert_params();
49 :
50 : // Adjusts the desired velocity so that the vehicle can stop
51 : // before the fence/object.
52 : // kP, accel_cmss are for the horizontal axis
53 : // kP_z, accel_z_cmss are for vertical axis
54 : void adjust_velocity(Vector3f &desired_vel_neu_cms, bool &backing_up, float kP, float accel_cmss, float kP_z, float accel_z_cmss, float dt);
55 2296480 : void adjust_velocity(Vector3f &desired_vel_neu_cms, float kP, float accel_cmss, float kP_z, float accel_z_cmss, float dt) {
56 2296480 : bool backing_up = false;
57 2296480 : adjust_velocity(desired_vel_neu_cms, backing_up, kP, accel_cmss, kP_z, accel_z_cmss, dt);
58 2296480 : }
59 252924 : void adjust_velocity_NED_m(Vector3f &desired_vel_ned_ms, float kP, float accel_mss, float kP_z, float accel_z_mss, float dt) {
60 252924 : bool backing_up = false;
61 : Vector3f desired_vel_neu_cms{
62 252924 : desired_vel_ned_ms.x * 100.0f,
63 252924 : desired_vel_ned_ms.y * 100.0f,
64 252924 : -desired_vel_ned_ms.z * 100.0f
65 252924 : };
66 252924 : adjust_velocity(desired_vel_neu_cms, backing_up, kP, accel_mss * 100.0, kP_z, accel_z_mss * 100.0, dt);
67 252924 : desired_vel_ned_ms = Vector3f{desired_vel_neu_cms.x, desired_vel_neu_cms.y, -desired_vel_neu_cms.z} * 0.01;
68 252924 : }
69 :
70 : // This method limits velocity and calculates backaway velocity from various supported fences
71 : // Also limits vertical velocity using adjust_velocity_z method
72 : void adjust_velocity_fence(float kP, float accel_cmss, Vector3f &desired_vel_neu_cms, Vector3f &backup_vel_cms, float kP_z, float accel_z_cmss, float dt);
73 :
74 : // adjust desired horizontal speed so that the vehicle stops before the fence or object
75 : // accel (maximum acceleration/deceleration) is in m/s/s
76 : // heading is in radians
77 : // speed is in m/s
78 : // kP should be zero for linear response, non-zero for non-linear response
79 : // dt is the time since the last call in seconds
80 : void adjust_speed(float kP, float accel_mss, float heading_rad, float &speed_ms, float dt);
81 :
82 : // adjust vertical climb rate so vehicle does not break the vertical fence
83 : void adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_cms, float& backup_speed_cms, float dt);
84 : void adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_cms, float dt);
85 :
86 : #if AP_AVOIDANCE_ALTHOLD_ENABLED
87 : // adjust roll-pitch to push vehicle away from objects
88 : // roll and pitch value are in radians
89 : // veh_angle_max_rad is the user defined maximum lean angle for the vehicle in radians
90 : void adjust_roll_pitch_rad(float &roll_rad, float &pitch_rad, float veh_angle_max_rad) const;
91 : #endif
92 :
93 : // enable/disable proximity based avoidance
94 6 : void proximity_avoidance_enable(bool on_off) { _proximity_enabled = on_off; }
95 3697304 : bool proximity_avoidance_enabled() const { return (_proximity_enabled && (_enabled & AC_AVOID_USE_PROXIMITY_SENSOR) > 0); }
96 228158 : void proximity_alt_avoidance_enable(bool on_off) { _proximity_alt_enabled = on_off; }
97 :
98 : // helper functions
99 :
100 : // Limits the component of desired_vel_neu_cms in the direction of the unit vector
101 : // limit_direction to be at most the maximum speed permitted by the limit_distance_cm.
102 : // uses velocity adjustment idea from Randy's second email on this thread:
103 : // https://groups.google.com/forum/#!searchin/drones-discuss/obstacle/drones-discuss/QwUXz__WuqY/qo3G8iTLSJAJ
104 : void limit_velocity_NE(float kP, float accel_cmss, Vector2f &desired_vel_neu_cms, const Vector2f& limit_direction, float limit_distance_cm, float dt) const;
105 :
106 : // Note: This method is used to limit velocity horizontally and vertically given a 3D desired velocity vector
107 : // Limits the component of desired_vel_neu_cms in the direction of the obstacle_vector based on the passed value of "margin"
108 : void limit_velocity_NEU(float kP, float accel_cmss, Vector3f &desired_vel_neu_cms, const Vector3f& limit_direction, float limit_distance_cm, float kP_z, float accel_z_cmss ,float dt) const;
109 :
110 : // compute the speed such that the stopping distance of the vehicle will
111 : // be exactly the input distance.
112 : // kP should be non-zero for Copter which has a non-linear response
113 : float get_max_speed(float kP, float accel, float distance, float dt) const;
114 :
115 : // return minimum alt (in meters) above which avoidance will be active
116 11478 : float get_min_alt() const { return _alt_min_m; }
117 :
118 : // return true if limiting is active
119 2659268 : bool limits_active() const {return (AP_HAL::millis() - _last_limit_time) < AC_AVOID_ACTIVE_LIMIT_TIMEOUT_MS;};
120 :
121 : static const struct AP_Param::GroupInfo var_info[];
122 :
123 : private:
124 : // behaviour types (see BEHAVE parameter)
125 : enum BehaviourType {
126 : BEHAVIOR_SLIDE = 0,
127 : BEHAVIOR_STOP = 1
128 : };
129 :
130 : /*
131 : * Limit acceleration so that change of velocity output by avoidance library is controlled
132 : * This helps reduce jerks and sudden movements in the vehicle
133 : */
134 : void limit_accel_NEU_cm(const Vector3f &original_vel, Vector3f &modified_vel, float dt);
135 :
136 : /*
137 : * Adjusts the desired velocity for the circular fence.
138 : */
139 : void adjust_velocity_circle_fence(float kP, float accel_cmss, Vector2f &desired_vel_neu_cms, Vector2f &backup_vel_cms, float dt);
140 :
141 : /*
142 : * Adjusts the desired velocity for inclusion and exclusion polygon fences
143 : */
144 : void adjust_velocity_inclusion_and_exclusion_polygons(float kP, float accel_cmss, Vector2f &desired_vel_neu_cms, Vector2f &backup_vel, float dt);
145 :
146 : /*
147 : * Adjusts the desired velocity for the inclusion and exclusion circles
148 : */
149 : void adjust_velocity_inclusion_circles(float kP, float accel_cmss, Vector2f &desired_vel_neu_cms, Vector2f &backup_vel, float dt);
150 : void adjust_velocity_exclusion_circles(float kP, float accel_cmss, Vector2f &desired_vel_neu_cms, Vector2f &backup_vel, float dt);
151 :
152 : /*
153 : * Adjusts the desired velocity for the beacon fence.
154 : */
155 : void adjust_velocity_beacon_fence(float kP, float accel_cmss, Vector2f &desired_vel_neu_cms, Vector2f &backup_vel, float dt);
156 :
157 : /*
158 : * Adjusts the desired velocity based on output from the proximity sensor
159 : */
160 : void adjust_velocity_proximity(float kP, float accel_cmss, Vector3f &desired_vel_neu_cms, Vector3f &backup_vel, float kP_z, float accel_z_cmss, float dt);
161 :
162 : /*
163 : * Adjusts the desired velocity given an array of boundary points
164 : * The boundary must be in Earth Frame
165 : * margin is the distance (in meters) that the vehicle should stop short of the polygon
166 : * stay_inside should be true for fences, false for exclusion polygons
167 : */
168 : void adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &desired_vel_neu_cms, Vector2f &backup_vel, const Vector2f* boundary, uint16_t num_points, float margin, float dt, bool stay_inside);
169 :
170 : /*
171 : * Computes distance required to stop, given current speed.
172 : */
173 : float get_stopping_distance(float kP, float accel_cmss, float speed_cms) const;
174 :
175 : /*
176 : * Compute the back away velocity required to avoid breaching margin
177 : * INPUT: This method requires the breach in margin distance (back_distance_cm), direction towards the breach (limit_direction)
178 : * It then calculates the desired backup velocity and passes it on to "find_max_quadrant_velocity" method to distribute the velocity vector into respective quadrants
179 : * OUTPUT: The method then outputs four velocities (quad1/2/3/4_back_vel_cms), which correspond to the final desired backup velocity in each quadrant
180 : */
181 : void calc_backup_velocity_2D(float kP, float accel_cmss, Vector2f &quad1_back_vel_cms, Vector2f &qua2_back_vel_cms, Vector2f &quad3_back_vel_cms, Vector2f &quad4_back_vel_cms, float back_distance_cm, Vector2f limit_direction, float dt) const;
182 :
183 : /*
184 : * Compute the back away velocity required to avoid breaching margin, including vertical component
185 : * min_z_vel is <= 0, and stores the greatest velocity in the downwards direction
186 : * max_z_vel is >= 0, and stores the greatest velocity in the upwards direction
187 : * eventually max_z_vel + min_z_vel will give the final desired Z backaway velocity
188 : */
189 : void calc_backup_velocity_3D(float kP, float accel_cmss, Vector2f &quad1_back_vel_cms, Vector2f &quad2_back_vel_cms, Vector2f &quad3_back_vel_cms, Vector2f &quad4_back_vel_cms, float back_distance_cms, Vector3f limit_direction, float kp_z, float accel_z_cmss, float back_distance_z, float& min_z_vel, float& max_z_vel, float dt) const;
190 :
191 : /*
192 : * Calculate maximum velocity vector that can be formed in each quadrant
193 : * This method takes the desired backup velocity, and four other velocities corresponding to each quadrant
194 : * The desired velocity is then fit into one of the 4 quadrant velocities as per the sign of its components
195 : * This ensures that we have multiple backup velocities, we can get the maximum of all of those velocities in each quadrant
196 : */
197 : void find_max_quadrant_velocity(Vector2f &desired_vel, Vector2f &quad1_vel, Vector2f &quad2_vel, Vector2f &quad3_vel, Vector2f &quad4_vel) const;
198 :
199 : /*
200 : * Calculate maximum velocity vector that can be formed in each quadrant and separately store max & min of vertical components
201 : */
202 : void find_max_quadrant_velocity_3D(Vector3f &desired_vel, Vector2f &quad1_vel, Vector2f &quad2_vel, Vector2f &quad3_vel, Vector2f &quad4_vel, float &max_z_vel, float &min_z_vel) const;
203 :
204 : /*
205 : * methods for avoidance in non-GPS flight modes
206 : */
207 :
208 : #if AP_AVOIDANCE_ALTHOLD_ENABLED
209 : // convert distance (in meters) to a lean percentage (in 0~1 range) for use in manual flight modes
210 : float distance_m_to_lean_norm(float dist_m) const;
211 :
212 : // returns the maximum positive and negative roll and pitch percentages (in -1 ~ +1 range) based on the proximity sensor
213 : void get_proximity_roll_pitch_norm(float &roll_positive, float &roll_negative, float &pitch_positive, float &pitch_negative) const;
214 : #endif
215 :
216 : // Logging function
217 : void Write_SimpleAvoidance(const uint8_t state, const Vector3f& desired_vel, const Vector3f& modified_vel, const bool back_up) const;
218 :
219 : // parameters
220 : AP_Int8 _enabled;
221 : #if AP_AVOIDANCE_ALTHOLD_ENABLED
222 : AP_Float _angle_max_deg; // maximum lean angle in degrees to avoid obstacles (only used in non-GPS flight modes)
223 : #endif
224 : AP_Float _dist_max_m; // distance (in meters) from object at which obstacle avoidance will begin in non-GPS modes
225 : AP_Float _margin_m; // vehicle will attempt to stay this distance (in meters) from objects while in GPS modes
226 : AP_Int8 _behavior; // avoidance behaviour (slide or stop)
227 : AP_Float _backup_speed_max_ne_ms; // Maximum speed that will be used to back away horizontally (in m/s)
228 : AP_Float _backup_speed_max_u_ms; // Maximum speed that will be used to back away verticality (in m/s)
229 : AP_Float _alt_min_m; // alt below which Proximity based avoidance is turned off
230 : AP_Float _accel_max_mss; // maximum acceleration while simple avoidance is active
231 : AP_Float _backup_deadzone_m; // distance beyond AVOID_MARGIN parameter, after which vehicle will backaway from obstacles
232 :
233 : bool _proximity_enabled = true; // true if proximity sensor based avoidance is enabled (used to allow pilot to enable/disable)
234 : bool _proximity_alt_enabled = true; // true if proximity sensor based avoidance is enabled based on altitude
235 : uint32_t _last_limit_time; // the last time a limit was active
236 : uint32_t _last_log_ms; // the last time simple avoidance was logged
237 : Vector3f _prev_avoid_vel_neu_cms; // copy of avoidance adjusted velocity
238 :
239 : static AC_Avoid *_singleton;
240 : };
241 :
242 : namespace AP {
243 : AC_Avoid *ac_avoid();
244 : };
245 :
246 : #endif // AP_AVOIDANCE_ENABLED
|
