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 : #include "AR_PosControl.h"
17 :
18 : #include <AP_HAL/AP_HAL.h>
19 : #include <AP_Math/AP_Math.h>
20 : #include <AP_AHRS/AP_AHRS.h>
21 : #include <AP_Logger/AP_Logger.h>
22 : #include <GCS_MAVLink/GCS.h>
23 : #include <AC_Avoidance/AC_Avoid.h>
24 : #include <AC_AttitudeControl/AC_PosControl.h>
25 :
26 : #define AR_POSCON_TIMEOUT_MS 100 // timeout after 0.1 sec
27 : #define AR_POSCON_POS_P 0.2f // default position P gain
28 : #define AR_POSCON_VEL_P 1.0f // default velocity P gain
29 : #define AR_POSCON_VEL_I 0.0f // default velocity I gain
30 : #define AR_POSCON_VEL_D 0.0f // default velocity D gain
31 : #define AR_POSCON_VEL_FF 0.0f // default velocity FF gain
32 : #define AR_POSCON_VEL_IMAX 1.0f // default velocity IMAX
33 : #define AR_POSCON_VEL_FILT 5.0f // default velocity filter
34 : #define AR_POSCON_VEL_FILT_D 5.0f // default velocity D term filter
35 : #define AR_POSCON_DT 0.02f // default dt for PID controllers
36 :
37 : extern const AP_HAL::HAL& hal;
38 :
39 : AR_PosControl *AR_PosControl::_singleton;
40 :
41 : const AP_Param::GroupInfo AR_PosControl::var_info[] = {
42 :
43 : // @Param: _POS_P
44 : // @DisplayName: Position controller P gain
45 : // @Description: Position controller P gain. Converts the distance to the target location into a desired speed which is then passed to the loiter latitude rate controller
46 : // @Range: 0.500 2.000
47 : // @User: Standard
48 : AP_SUBGROUPINFO(_p_pos, "_POS_", 1, AR_PosControl, AC_P_2D),
49 :
50 : // @Param: _VEL_P
51 : // @DisplayName: Velocity (horizontal) P gain
52 : // @Description: Velocity (horizontal) P gain. Converts the difference between desired and actual velocity to a target acceleration
53 : // @Range: 0.1 6.0
54 : // @Increment: 0.1
55 : // @User: Advanced
56 :
57 : // @Param: _VEL_I
58 : // @DisplayName: Velocity (horizontal) I gain
59 : // @Description: Velocity (horizontal) I gain. Corrects long-term difference between desired and actual velocity to a target acceleration
60 : // @Range: 0.00 1.00
61 : // @Increment: 0.01
62 : // @User: Advanced
63 :
64 : // @Param: _VEL_D
65 : // @DisplayName: Velocity (horizontal) D gain
66 : // @Description: Velocity (horizontal) D gain. Corrects short-term changes in velocity
67 : // @Range: 0.00 1.00
68 : // @Increment: 0.001
69 : // @User: Advanced
70 :
71 : // @Param: _VEL_IMAX
72 : // @DisplayName: Velocity (horizontal) integrator maximum
73 : // @Description: Velocity (horizontal) integrator maximum. Constrains the target acceleration that the I gain will output
74 : // @Range: 0 4500
75 : // @Increment: 10
76 : // @Units: cm/s/s
77 : // @User: Advanced
78 :
79 : // @Param: _VEL_FLTE
80 : // @DisplayName: Velocity (horizontal) input filter
81 : // @Description: Velocity (horizontal) input filter. This filter (in Hz) is applied to the input for P and I terms
82 : // @Range: 0 100
83 : // @Units: Hz
84 : // @User: Advanced
85 :
86 : // @Param: _VEL_FLTD
87 : // @DisplayName: Velocity (horizontal) input filter
88 : // @Description: Velocity (horizontal) input filter. This filter (in Hz) is applied to the input for D term
89 : // @Range: 0 100
90 : // @Units: Hz
91 : // @User: Advanced
92 :
93 : // @Param: _VEL_FF
94 : // @DisplayName: Velocity (horizontal) feed forward gain
95 : // @Description: Velocity (horizontal) feed forward gain. Converts the difference between desired velocity to a target acceleration
96 : // @Range: 0 6
97 : // @Increment: 0.01
98 : // @User: Advanced
99 : AP_SUBGROUPINFO(_pid_vel, "_VEL_", 2, AR_PosControl, AC_PID_2D),
100 :
101 : AP_GROUPEND
102 : };
103 :
104 285 : AR_PosControl::AR_PosControl(AR_AttitudeControl& atc) :
105 285 : _atc(atc),
106 285 : _p_pos(AR_POSCON_POS_P),
107 285 : _pid_vel(AR_POSCON_VEL_P, AR_POSCON_VEL_I, AR_POSCON_VEL_D, AR_POSCON_VEL_FF, AR_POSCON_VEL_IMAX, AR_POSCON_VEL_FILT, AR_POSCON_VEL_FILT_D)
108 : {
109 285 : _singleton = this;
110 285 : AP_Param::setup_object_defaults(this, var_info);
111 285 : }
112 :
113 : // update navigation
114 179080 : void AR_PosControl::update(float dt)
115 : {
116 : // exit immediately if no current location, destination or disarmed
117 179080 : Vector3p curr_pos_NED_m;
118 179080 : Vector3f curr_vel_NED;
119 357538 : if (!hal.util->get_soft_armed() || !AP::ahrs().get_relative_position_NED_origin(curr_pos_NED_m) ||
120 178458 : !AP::ahrs().get_velocity_NED(curr_vel_NED)) {
121 622 : _desired_speed = _atc.get_desired_speed_accel_limited(0.0f, dt);
122 622 : _desired_lat_accel = 0.0f;
123 622 : _desired_turn_rate_rads = 0.0f;
124 622 : return;
125 : }
126 :
127 : // check for ekf xy position reset
128 178458 : handle_ekf_xy_reset();
129 :
130 : // if no recent calls reset velocity controller
131 178458 : if (!is_active()) {
132 80 : _pid_vel.reset_I();
133 80 : _pid_vel.reset_filter();
134 : }
135 178458 : _last_update_ms = AP_HAL::millis();
136 :
137 : // calculate position error and convert to desired velocity
138 178458 : _vel_target.zero();
139 178458 : if (_pos_target_valid) {
140 178458 : Vector2p pos_target = _pos_target;
141 178458 : _vel_target = _p_pos.update_all(pos_target, curr_pos_NED_m.xy());
142 : }
143 :
144 : // calculation velocity error
145 178458 : bool stopping = false;
146 178458 : if (_vel_desired_valid) {
147 : // add target velocity to desired velocity from position error
148 178458 : _vel_target += _vel_desired;
149 178458 : stopping = _vel_desired.is_zero();
150 : }
151 :
152 : // limit velocity to maximum speed
153 178458 : _vel_target.limit_length(get_speed_max());
154 :
155 : // Limit the velocity to prevent fence violations
156 178458 : bool backing_up = false;
157 : #if AP_AVOIDANCE_ENABLED
158 178458 : AC_Avoid *avoid = AP::ac_avoid();
159 178458 : if (avoid != nullptr) {
160 178458 : Vector3f vel_3d_cms{_vel_target.x * 100.0f, _vel_target.y * 100.0f, 0.0f};
161 178458 : const float accel_max_cmss = MIN(_accel_max, _lat_accel_max) * 100.0;
162 178458 : avoid->adjust_velocity(vel_3d_cms, backing_up, _p_pos.kP(), accel_max_cmss, _p_pos.kP(), accel_max_cmss, dt);
163 178458 : _vel_target.x = vel_3d_cms.x * 0.01;
164 178458 : _vel_target.y = vel_3d_cms.y * 0.01;
165 : }
166 : #endif // AP_AVOIDANCE_ENABLED
167 :
168 : // calculate limit vector based on steering limits
169 178458 : Vector2f steering_limit_vec;
170 178458 : if (_atc.steering_limit_left()) {
171 30500 : steering_limit_vec = AP::ahrs().body_to_earth2D(Vector2f{0, _reversed ? 1.0f : -1.0f});
172 147958 : } else if (_atc.steering_limit_right()) {
173 1876 : steering_limit_vec = AP::ahrs().body_to_earth2D(Vector2f{0, _reversed ? -1.0f : 1.0f});
174 : }
175 :
176 : // calculate desired acceleration
177 178458 : _accel_target = _pid_vel.update_all(_vel_target, curr_vel_NED.xy(), dt, steering_limit_vec);
178 178458 : if (_accel_desired_valid) {
179 178458 : _accel_target += _accel_desired;
180 : }
181 : // velocity controller I-term zeroed in forward-back direction
182 178458 : const Vector2f lat_vec_ef = AP::ahrs().body_to_earth2D(Vector2f{0, 1});
183 178458 : const Vector2f vel_i = _pid_vel.get_i().projected(lat_vec_ef);
184 178458 : _pid_vel.set_integrator(vel_i);
185 :
186 : // convert desired acceleration to desired forward-back speed, desired lateral speed and desired turn rate
187 :
188 : // rotate acceleration into body frame using current heading
189 178458 : const Vector2f accel_target_FR = AP::ahrs().earth_to_body2D(_accel_target);
190 :
191 : // calculate minimum turn speed which is the max speed the vehicle could turn through the corner
192 : // given the vehicle's turn radius and half its max lateral acceleration
193 : // todo: remove MAX of zero when safe_sqrt fixed
194 178458 : float turn_speed_min = MAX(safe_sqrt(_atc.get_turn_lat_accel_max() * 0.5 * _turn_radius), 0);
195 :
196 : // rotate target velocity from earth-frame to body frame
197 178458 : const Vector2f vel_target_FR = AP::ahrs().earth_to_body2D(_vel_target);
198 :
199 : // desired speed is normally the forward component (only) of the target velocity
200 178458 : float des_speed = vel_target_FR.x;
201 178458 : if (!stopping) {
202 : // do not let target speed fall below the minimum turn speed unless the vehicle is slowing down
203 178241 : const float abs_des_speed_min = MIN(_vel_target.length(), turn_speed_min);
204 178241 : if (_reversed != backing_up) {
205 : // if reversed or backing up desired speed will be negative
206 800 : des_speed = MIN(-abs_des_speed_min, vel_target_FR.x);
207 : } else {
208 177441 : des_speed = MAX(abs_des_speed_min, vel_target_FR.x);
209 : }
210 : }
211 178458 : _desired_speed = _atc.get_desired_speed_accel_limited(des_speed, dt);
212 :
213 : // calculate turn rate from desired lateral acceleration
214 178458 : _desired_lat_accel = stopping ? 0 : accel_target_FR.y;
215 178458 : _desired_turn_rate_rads = _atc.get_turn_rate_from_lat_accel(_desired_lat_accel, _desired_speed);
216 : }
217 :
218 : // true if update has been called recently
219 636320 : bool AR_PosControl::is_active() const
220 : {
221 636320 : return ((AP_HAL::millis() - _last_update_ms) < AR_POSCON_TIMEOUT_MS);
222 : }
223 :
224 : // set limits
225 148 : void AR_PosControl::set_limits(float speed_max, float accel_max, float lat_accel_max, float jerk_max)
226 : {
227 148 : _speed_max = MAX(speed_max, 0);
228 148 : _accel_max = MAX(accel_max, 0);
229 148 : _lat_accel_max = MAX(lat_accel_max, 0);
230 148 : _jerk_max = MAX(jerk_max, 0);
231 :
232 : // set position P controller limits
233 148 : _p_pos.set_limits(_speed_max, MIN(_accel_max, _lat_accel_max), _jerk_max);
234 148 : }
235 :
236 : // setter to allow vehicle code to provide turn related param values to this library (should be updated regularly)
237 12526 : void AR_PosControl::set_turn_params(float turn_radius, bool pivot_possible)
238 : {
239 12526 : if (pivot_possible) {
240 1745 : _turn_radius = 0;
241 : } else {
242 10781 : _turn_radius = turn_radius;
243 : }
244 12526 : }
245 :
246 : // initialise the position controller to the current position, velocity, acceleration and attitude
247 : // this should be called before the input shaping methods are used
248 85 : bool AR_PosControl::init()
249 : {
250 : // get current position and velocity from AHRS
251 85 : Vector2f pos_NE;
252 85 : Vector3f vel_NED;
253 85 : if (!AP::ahrs().get_relative_position_NE_origin_float(pos_NE) || !AP::ahrs().get_velocity_NED(vel_NED)) {
254 0 : return false;
255 : }
256 :
257 : // set target position to current position
258 85 : _pos_target.x = pos_NE.x;
259 85 : _pos_target.y = pos_NE.y;
260 :
261 : // set target velocity and acceleration
262 85 : _vel_desired = vel_NED.xy();
263 85 : _vel_target.zero();
264 85 : _accel_desired = AP::ahrs().get_accel_ef().xy();
265 85 : _accel_target.zero();
266 :
267 : // clear reversed setting
268 85 : _reversed = false;
269 :
270 : // initialise ekf xy reset handler
271 85 : init_ekf_xy_reset();
272 :
273 85 : return true;
274 : }
275 :
276 : // adjust position, velocity and acceleration targets smoothly using input shaping
277 : // pos is the target position as an offset from the EKF origin (in meters)
278 : // vel is the target velocity in m/s. accel is the target acceleration in m/s/s
279 : // dt should be the update rate in seconds
280 : // init should be called once before starting to use these methods
281 46694 : void AR_PosControl::input_pos_target(const Vector2p &pos, float dt)
282 : {
283 46694 : Vector2f vel;
284 46694 : Vector2f accel;
285 46694 : input_pos_vel_accel_target(pos, vel, accel, dt);
286 46694 : }
287 :
288 : // adjust position, velocity and acceleration targets smoothly using input shaping
289 : // pos is the target position as an offset from the EKF origin (in meters)
290 : // vel is the target velocity in m/s. accel is the target acceleration in m/s/s
291 : // dt should be the update rate in seconds
292 : // init should be called once before starting to use these methods
293 0 : void AR_PosControl::input_pos_vel_target(const Vector2p &pos, const Vector2f &vel, float dt)
294 : {
295 0 : Vector2f accel;
296 0 : input_pos_vel_accel_target(pos, vel, accel, dt);
297 0 : }
298 :
299 : // adjust position, velocity and acceleration targets smoothly using input shaping
300 : // pos is the target position as an offset from the EKF origin (in meters)
301 : // vel is the target velocity in m/s. accel is the target acceleration in m/s/s
302 : // dt should be the update rate in seconds
303 : // init should be called once before starting to use these methods
304 46694 : void AR_PosControl::input_pos_vel_accel_target(const Vector2p &pos, const Vector2f &vel, const Vector2f &accel, float dt)
305 : {
306 : // adjust target position, velocity and acceleration forward by dt
307 46694 : update_pos_vel_accel_xy(_pos_target, _vel_desired, _accel_desired, dt, Vector2f(), Vector2f(), Vector2f());
308 :
309 : // call shape_pos_vel_accel_xy to pull target towards final destination
310 46694 : const float accel_max = MIN(_accel_max, _lat_accel_max);
311 46694 : shape_pos_vel_accel_xy(pos, vel, accel, _pos_target, _vel_desired, _accel_desired,
312 : _speed_max, accel_max, _jerk_max, dt, false);
313 :
314 : // set flags so update will consume target position, desired velocity and desired acceleration
315 46694 : _pos_target_valid = true;
316 46694 : _vel_desired_valid = true;
317 46694 : _accel_desired_valid = true;
318 46694 : }
319 :
320 : // set target position, desired velocity and acceleration. These should be from an externally created path and are not "input shaped"
321 132386 : void AR_PosControl::set_pos_vel_accel_target(const Vector2p &pos, const Vector2f &vel, const Vector2f &accel)
322 : {
323 132386 : _pos_target = pos;
324 132386 : _vel_desired = vel;
325 132386 : _accel_desired = accel;
326 132386 : _pos_target_valid = true;
327 132386 : _vel_desired_valid = true;
328 132386 : _accel_desired_valid = true;
329 132386 : }
330 :
331 : // returns desired velocity vector (i.e. feed forward) in cm/s in lat and lon direction
332 130879 : Vector2f AR_PosControl::get_desired_velocity() const
333 : {
334 130879 : if (_vel_desired_valid) {
335 130850 : return _vel_desired;
336 : }
337 29 : return Vector2f();
338 : }
339 :
340 : // return desired acceleration vector in m/s in lat and lon direction
341 0 : Vector2f AR_PosControl::get_desired_accel() const
342 : {
343 0 : if (_accel_desired_valid) {
344 0 : return _accel_desired;
345 : }
346 0 : return Vector2f();
347 : }
348 :
349 : /// get position error as a vector from the current position to the target position
350 131890 : Vector2p AR_PosControl::get_pos_error() const
351 : {
352 : // return zero error is not active or no position estimate
353 131890 : Vector2f curr_pos_NE;
354 131890 : if (!is_active() ||!AP::ahrs().get_relative_position_NE_origin_float(curr_pos_NE)) {
355 93 : return Vector2p{};
356 : }
357 :
358 : // get current position
359 131797 : return (_pos_target - curr_pos_NE.topostype());
360 : }
361 :
362 : #if HAL_LOGGING_ENABLED
363 : // write PSC logs
364 35761 : void AR_PosControl::write_log()
365 : {
366 : // exit immediately if not active
367 35761 : if (!is_active()) {
368 0 : return;
369 : }
370 :
371 : // exit immediately if no position or velocity estimate
372 35761 : Vector3f curr_pos_NED;
373 35761 : Vector3f curr_vel_NED;
374 35761 : if (!AP::ahrs().get_relative_position_NED_origin_float(curr_pos_NED) || !AP::ahrs().get_velocity_NED(curr_vel_NED)) {
375 0 : return;
376 : }
377 :
378 : // get acceleration
379 35761 : const Vector3f curr_accel_NED = AP::ahrs().get_accel_ef() * 100.0;
380 :
381 : // convert position to required format
382 35761 : Vector2f pos_target_2d_cm = get_pos_target().tofloat() * 100.0;
383 :
384 : // reuse logging from AC_PosControl:
385 35761 : AC_PosControl::Write_PSCN(0.0, // position desired
386 : pos_target_2d_cm.x, // position target
387 35761 : curr_pos_NED.x * 100.0, // position
388 35761 : _vel_desired.x * 100.0, // desired velocity
389 35761 : _vel_target.x * 100.0, // target velocity
390 35761 : curr_vel_NED.x * 100.0, // velocity
391 35761 : _accel_desired.x * 100.0, // desired accel
392 35761 : _accel_target.x * 100.0, // target accel
393 35761 : curr_accel_NED.x); // accel
394 35761 : AC_PosControl::Write_PSCE(0.0, // position desired
395 : pos_target_2d_cm.y, // position target
396 35761 : curr_pos_NED.y * 100.0, // position
397 35761 : _vel_desired.y * 100.0, // desired velocity
398 35761 : _vel_target.y * 100.0, // target velocity
399 35761 : curr_vel_NED.y * 100.0, // velocity
400 35761 : _accel_desired.y * 100.0, // desired accel
401 35761 : _accel_target.y * 100.0, // target accel
402 35761 : curr_accel_NED.y); // accel
403 : }
404 : #endif
405 :
406 : /// initialise ekf xy position reset check
407 85 : void AR_PosControl::init_ekf_xy_reset()
408 : {
409 85 : _ekf_xy_reset_count = AP::ahrs().get_position_NE_reset_count();
410 85 : }
411 :
412 : /// handle_ekf_xy_reset - check for ekf position reset and adjust loiter or brake target position
413 178458 : void AR_PosControl::handle_ekf_xy_reset()
414 : {
415 : // check for position reset
416 178458 : const uint16_t reset_count = AP::ahrs().get_position_NE_reset_count();
417 178458 : if (reset_count != _ekf_xy_reset_count) {
418 0 : Vector2p pos_ne_m;
419 0 : if (!AP::ahrs().get_relative_position_NE_origin(pos_ne_m)) {
420 0 : return;
421 : }
422 0 : _pos_target = pos_ne_m + _p_pos.get_error().topostype();
423 :
424 0 : Vector3f vel_NED;
425 0 : if (!AP::ahrs().get_velocity_NED(vel_NED)) {
426 0 : return;
427 : }
428 0 : _vel_desired = vel_NED.xy() + _pid_vel.get_error();
429 :
430 0 : _ekf_xy_reset_count = reset_count;
431 : }
432 : }
|
