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 <AP_AHRS/AP_AHRS.h>
17 : #include <AP_Math/AP_Math.h>
18 : #include <AP_HAL/AP_HAL.h>
19 : #include "AR_WPNav.h"
20 : #include <GCS_MAVLink/GCS.h>
21 : #include <AP_InternalError/AP_InternalError.h>
22 :
23 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
24 : #include <stdio.h>
25 : #endif
26 :
27 : extern const AP_HAL::HAL& hal;
28 :
29 : #define AR_WPNAV_TIMEOUT_MS 100
30 : #define AR_WPNAV_SPEED_DEFAULT 2.0f
31 : #define AR_WPNAV_SPEED_MIN 0.05f // minimum speed between waypoints in m/s
32 : #define AR_WPNAV_SPEED_UPDATE_MIN_MS 500 // max speed cannot be updated more than once in this many milliseconds
33 : #define AR_WPNAV_RADIUS_DEFAULT 2.0f
34 : #define AR_WPNAV_OVERSPEED_RATIO_MAX 5.0f // if _overspeed_enabled the vehicle may travel as quickly as 5x WP_SPEED
35 : #define AR_WPNAV_SNAP_MAX 15.0f // scurve snap (change in jerk) in m/s/s/s/s
36 : #define AR_WPNAV_ACCEL_MAX 20.0 // acceleration used when user has specified no acceleration limit
37 :
38 : const AP_Param::GroupInfo AR_WPNav::var_info[] = {
39 :
40 : // @Param: SPEED
41 : // @DisplayName: Waypoint speed default
42 : // @Description: Waypoint speed default
43 : // @Units: m/s
44 : // @Range: 0 100
45 : // @Increment: 0.1
46 : // @User: Standard
47 : AP_GROUPINFO("SPEED", 1, AR_WPNav, _speed_max, AR_WPNAV_SPEED_DEFAULT),
48 :
49 : // @Param: RADIUS
50 : // @DisplayName: Waypoint radius
51 : // @Description: The distance in meters from a waypoint when we consider the waypoint has been reached. This determines when the vehicle will turn toward the next waypoint.
52 : // @Units: m
53 : // @Range: 0 100
54 : // @Increment: 0.1
55 : // @User: Standard
56 : AP_GROUPINFO("RADIUS", 2, AR_WPNav, _radius, AR_WPNAV_RADIUS_DEFAULT),
57 :
58 : // 3 was OVERSHOOT
59 :
60 : // 4 was PIVOT_ANGLE
61 : // 5 was PIVOT_RATE
62 : // 6 was SPEED_MIN
63 : // 7 was PIVOT_DELAY
64 :
65 : // @Group: PIVOT_
66 : // @Path: AR_PivotTurn.cpp
67 : AP_SUBGROUPINFO(_pivot, "PIVOT_", 8, AR_WPNav, AR_PivotTurn),
68 :
69 : // @Param: ACCEL
70 : // @DisplayName: Waypoint acceleration
71 : // @Description: Waypoint acceleration. If zero then ATC_ACCEL_MAX is used
72 : // @Units: m/s/s
73 : // @Range: 0 100
74 : // @Increment: 0.1
75 : // @User: Standard
76 : AP_GROUPINFO("ACCEL", 9, AR_WPNav, _accel_max, 0),
77 :
78 : // @Param: JERK
79 : // @DisplayName: Waypoint jerk
80 : // @Description: Waypoint jerk (change in acceleration). If zero then jerk is same as acceleration
81 : // @Units: m/s/s/s
82 : // @Range: 0 100
83 : // @Increment: 0.1
84 : // @User: Standard
85 : AP_GROUPINFO("JERK", 10, AR_WPNav, _jerk_max, 0),
86 :
87 : AP_GROUPEND
88 : };
89 :
90 285 : AR_WPNav::AR_WPNav(AR_AttitudeControl& atc, AR_PosControl &pos_control) :
91 285 : _pivot(atc),
92 285 : _atc(atc),
93 570 : _pos_control(pos_control)
94 : {
95 285 : AP_Param::setup_object_defaults(this, var_info);
96 285 : }
97 :
98 : // initialise waypoint controller. speed_max should be set to the maximum speed in m/s (or left at zero to use the default speed)
99 127 : void AR_WPNav::init(float speed_max)
100 : {
101 : // determine max speed, acceleration and jerk
102 127 : if (is_positive(speed_max)) {
103 5 : _base_speed_max = speed_max;
104 : } else {
105 122 : _base_speed_max = _speed_max;
106 : }
107 127 : _base_speed_max = MAX(AR_WPNAV_SPEED_MIN, _base_speed_max);
108 127 : float atc_accel_max = MIN(_atc.get_accel_max(), _atc.get_decel_max());
109 127 : if (!is_positive(atc_accel_max)) {
110 : // accel_max of zero means no limit so use maximum acceleration
111 0 : atc_accel_max = AR_WPNAV_ACCEL_MAX;
112 : }
113 127 : const float accel_max = is_positive(_accel_max) ? MIN(_accel_max, atc_accel_max) : atc_accel_max;
114 127 : const float jerk_max = is_positive(_jerk_max) ? _jerk_max : accel_max;
115 :
116 : // initialise position controller
117 127 : _pos_control.set_limits(_base_speed_max, accel_max, _atc.get_turn_lat_accel_max(), jerk_max);
118 :
119 127 : _scurve_prev_leg.init();
120 127 : _scurve_this_leg.init();
121 127 : _scurve_next_leg.init();
122 127 : _track_scalar_dt = 1.0f;
123 :
124 : // init some flags
125 127 : _reached_destination = false;
126 127 : _fast_waypoint = false;
127 :
128 : // ensure pivot turns are deactivated
129 127 : _pivot.deactivate();
130 127 : _pivot_at_next_wp = false;
131 :
132 : // initialise origin and destination to stopping point
133 127 : _orig_and_dest_valid = false;
134 127 : set_origin_and_destination_to_stopping_point();
135 :
136 : // initialise nudge speed to zero
137 127 : set_nudge_speed_max(0);
138 127 : }
139 :
140 : // update navigation
141 170371 : void AR_WPNav::update(float dt)
142 : {
143 : // exit immediately if no current location, origin or destination
144 170371 : Location current_loc;
145 : float speed;
146 170371 : if (!hal.util->get_soft_armed() || !_orig_and_dest_valid || !AP::ahrs().get_location(current_loc) || !_atc.get_forward_speed(speed)) {
147 0 : _desired_speed_limited = _atc.get_desired_speed_accel_limited(0.0f, dt);
148 0 : _desired_lat_accel = 0.0f;
149 0 : _desired_turn_rate_rads = 0.0f;
150 0 : _cross_track_error = 0;
151 0 : return;
152 : }
153 :
154 : // if no recent calls initialise desired_speed_limited to current speed
155 170371 : if (!is_active()) {
156 81 : _desired_speed_limited = speed;
157 : }
158 170371 : _last_update_ms = AP_HAL::millis();
159 :
160 170371 : update_distance_and_bearing_to_destination();
161 :
162 : // handle change in max speed
163 170371 : update_speed_max();
164 :
165 : // advance target along path unless vehicle is pivoting
166 170371 : if (!_pivot.active()) {
167 170371 : switch (_nav_control_type) {
168 130879 : case NavControllerType::NAV_SCURVE:
169 130879 : advance_wp_target_along_track(current_loc, dt);
170 130879 : break;
171 39492 : case NavControllerType::NAV_PSC_INPUT_SHAPING:
172 39492 : update_psc_input_shaping(dt);
173 39492 : break;
174 : }
175 : }
176 :
177 : // update_steering_and_speed
178 170371 : update_steering_and_speed(current_loc, dt);
179 : }
180 :
181 : // set maximum speed in m/s. returns true on success
182 : // this should not be called at more than 3hz or else SCurve path planning may not advance properly
183 17 : bool AR_WPNav::set_speed_max(float speed_max)
184 : {
185 : // range check target speed
186 17 : if (speed_max < AR_WPNAV_SPEED_MIN) {
187 0 : return false;
188 : }
189 :
190 17 : _base_speed_max = speed_max;
191 17 : return true;
192 : }
193 :
194 : // set speed nudge in m/s. this will have no effect unless nudge_speed_max > speed_max
195 : // nudge_speed_max should always be positive regardless of whether the vehicle is travelling forward or reversing
196 180988 : void AR_WPNav::set_nudge_speed_max(float nudge_speed_max)
197 : {
198 180988 : _nudge_speed_max = nudge_speed_max;
199 180988 : }
200 :
201 : // set desired location and (optionally) next_destination
202 : // next_destination should be provided if known to allow smooth cornering
203 200 : bool AR_WPNav::set_desired_location(const Location& destination, Location next_destination)
204 : {
205 : // re-initialise if inactive, previous destination has been interrupted or different controller was used
206 200 : if (!is_active() || !_reached_destination || (_nav_control_type != NavControllerType::NAV_SCURVE)) {
207 109 : if (!set_origin_and_destination_to_stopping_point()) {
208 0 : return false;
209 : }
210 : // clear scurves
211 109 : _scurve_prev_leg.init();
212 109 : _scurve_this_leg.init();
213 109 : _scurve_next_leg.init();
214 : }
215 :
216 : // shift this leg to previous leg
217 200 : _scurve_prev_leg = _scurve_this_leg;
218 :
219 : // initialise some variables
220 200 : _origin = _destination;
221 200 : _destination = destination;
222 200 : _orig_and_dest_valid = true;
223 200 : _reached_destination = false;
224 :
225 200 : update_distance_and_bearing_to_destination();
226 :
227 : // check if vehicle should pivot if vehicle stopped at previous waypoint
228 : // or journey to previous waypoint was interrupted or navigation has just started
229 200 : if (!_fast_waypoint) {
230 108 : _pivot.deactivate();
231 108 : _pivot.check_activation((_reversed ? wrap_360_cd(oa_wp_bearing_cd() + 18000) : oa_wp_bearing_cd()) * 0.01, _pivot_at_next_wp);
232 : }
233 :
234 : // convert origin and destination to offset from EKF origin
235 200 : Vector2f origin_NE;
236 200 : Vector2f destination_NE;
237 400 : if (!_origin.get_vector_xy_from_origin_NE_cm(origin_NE) ||
238 200 : !_destination.get_vector_xy_from_origin_NE_cm(destination_NE)) {
239 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
240 0 : return false;
241 : }
242 200 : origin_NE *= 0.01f;
243 200 : destination_NE *= 0.01f;
244 :
245 : // calculate track to destination
246 200 : if (_fast_waypoint && !_scurve_next_leg.finished()) {
247 : // skip recalculating this leg by simply shifting next leg
248 89 : _scurve_this_leg = _scurve_next_leg;
249 : } else {
250 111 : _scurve_this_leg.calculate_track(Vector3p{origin_NE.x, origin_NE.y, 0.0f}, // origin
251 222 : Vector3p{destination_NE.x, destination_NE.y, 0.0f}, // destination
252 : 0.0, // arc length is zero for straight track
253 111 : _pos_control.get_speed_max(),
254 111 : _pos_control.get_speed_max(), // speed up (not used)
255 111 : _pos_control.get_speed_max(), // speed down (not used)
256 111 : _pos_control.get_accel_max(), // forward back acceleration
257 111 : _pos_control.get_accel_max(), // vertical accel (not used)
258 111 : _pos_control.get_accel_max(), // corner acceleration
259 : AR_WPNAV_SNAP_MAX, // snap
260 111 : _pos_control.get_jerk_max());
261 : }
262 :
263 : // handle next destination
264 200 : _scurve_next_leg.init();
265 200 : _fast_waypoint = false;
266 200 : _pivot_at_next_wp = false;
267 200 : if (next_destination.initialised()) {
268 : // check if vehicle should pivot at next waypoint
269 112 : const float next_wp_yaw_change = get_corner_angle(_origin, destination, next_destination);
270 112 : _pivot_at_next_wp = _pivot.would_activate(next_wp_yaw_change);
271 112 : if (!_pivot_at_next_wp) {
272 : // convert next_destination to offset from EKF origin
273 112 : Vector2f next_destination_NE;
274 112 : if (!next_destination.get_vector_xy_from_origin_NE_cm(next_destination_NE)) {
275 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
276 0 : return false;
277 : }
278 112 : next_destination_NE *= 0.01f;
279 112 : _scurve_next_leg.calculate_track(Vector3p{destination_NE.x, destination_NE.y, 0.0f},
280 112 : Vector3p{next_destination_NE.x, next_destination_NE.y, 0.0f},
281 : 0.0, // arc length is zero for straight track
282 112 : _pos_control.get_speed_max(),
283 112 : _pos_control.get_speed_max(), // speed up (not used)
284 112 : _pos_control.get_speed_max(), // speed down (not used)
285 112 : _pos_control.get_accel_max(), // forward back acceleration
286 112 : _pos_control.get_accel_max(), // vertical accel (not used)
287 112 : _pos_control.get_accel_max(), // corner accel
288 : AR_WPNAV_SNAP_MAX, // snap
289 112 : _pos_control.get_jerk_max());
290 :
291 : // next destination provided so fast waypoint
292 112 : _fast_waypoint = true;
293 : }
294 : }
295 :
296 : // scurves used for navigation to destination
297 200 : _nav_control_type = NavControllerType::NAV_SCURVE;
298 :
299 200 : update_distance_and_bearing_to_destination();
300 :
301 200 : return true;
302 : }
303 :
304 : // set desired location to a reasonable stopping point, return true on success
305 3 : bool AR_WPNav::set_desired_location_to_stopping_location()
306 : {
307 3 : Location stopping_loc;
308 3 : if (!get_stopping_location(stopping_loc)) {
309 0 : return false;
310 : }
311 3 : return set_desired_location(stopping_loc);
312 : }
313 :
314 : // set desired location as offset from the EKF origin, return true on success
315 2 : bool AR_WPNav::set_desired_location_NED(const Vector3f& destination)
316 : {
317 : // initialise destination to ekf origin
318 2 : Location destination_ned;
319 2 : if (!AP::ahrs().get_origin(destination_ned)) {
320 0 : return false;
321 : }
322 :
323 : // apply offset
324 2 : destination_ned.offset(destination.x, destination.y);
325 2 : return set_desired_location(destination_ned);
326 : }
327 :
328 5 : bool AR_WPNav::set_desired_location_NED(const Vector3f &destination, const Vector3f &next_destination)
329 : {
330 : // initialise destination to ekf origin
331 5 : Location dest_loc, next_dest_loc;
332 5 : if (!AP::ahrs().get_origin(dest_loc)) {
333 0 : return false;
334 : }
335 5 : next_dest_loc = dest_loc;
336 :
337 : // apply offsets
338 5 : dest_loc.offset(destination.x, destination.y);
339 5 : next_dest_loc.offset(next_destination.x, next_destination.y);
340 5 : return set_desired_location(dest_loc, next_dest_loc);
341 : }
342 :
343 : // set desired location but expect the destination to be updated again in the near future
344 : // position controller input shaping will be used for navigation instead of scurves
345 : // Note: object avoidance is not supported if this method is used
346 565 : bool AR_WPNav::set_desired_location_expect_fast_update(const Location &destination)
347 : {
348 : // initialise if not active
349 565 : if (!is_active() || (_nav_control_type != NavControllerType::NAV_PSC_INPUT_SHAPING)) {
350 29 : if (!set_origin_and_destination_to_stopping_point()) {
351 0 : return false;
352 : }
353 : }
354 :
355 : // initialise some variables
356 565 : _origin = _destination;
357 565 : _destination = destination;
358 565 : _orig_and_dest_valid = true;
359 565 : _reached_destination = false;
360 :
361 565 : update_distance_and_bearing_to_destination();
362 :
363 : // check if vehicle should pivot
364 565 : _pivot.check_activation((_reversed ? wrap_360_cd(oa_wp_bearing_cd() + 18000) : oa_wp_bearing_cd()) * 0.01);
365 :
366 : // position controller input shaping used for navigation to destination
367 565 : _nav_control_type = NavControllerType::NAV_PSC_INPUT_SHAPING;
368 565 : return true;
369 : }
370 :
371 : // calculate vehicle stopping point using current location, velocity and maximum acceleration
372 316 : bool AR_WPNav::get_stopping_location(Location& stopping_loc)
373 : {
374 316 : Location current_loc;
375 316 : if (!AP::ahrs().get_location(current_loc)) {
376 28 : return false;
377 : }
378 :
379 : // get current velocity vector and speed
380 288 : const Vector2f velocity = AP::ahrs().groundspeed_vector();
381 288 : const float speed = velocity.length();
382 :
383 : // avoid divide by zero
384 288 : if (!is_positive(speed)) {
385 15 : stopping_loc = current_loc;
386 15 : return true;
387 : }
388 :
389 : // get stopping distance in meters
390 273 : const float stopping_dist = _atc.get_stopping_distance(speed);
391 :
392 : // calculate stopping position from current location in meters
393 273 : const Vector2f stopping_offset = velocity.normalized() * stopping_dist;
394 273 : stopping_loc = current_loc;
395 273 : stopping_loc.offset(stopping_offset.x, stopping_offset.y);
396 :
397 273 : return true;
398 : }
399 :
400 : // true if update has been called recently
401 171136 : bool AR_WPNav::is_active() const
402 : {
403 171136 : return ((AP_HAL::millis() - _last_update_ms) < AR_WPNAV_TIMEOUT_MS);
404 : }
405 :
406 : // move target location along track from origin to destination using SCurves navigation
407 130879 : void AR_WPNav::advance_wp_target_along_track(const Location ¤t_loc, float dt)
408 : {
409 : // exit immediately if no current location, destination or disarmed
410 130879 : Vector2f curr_pos_NE;
411 130879 : Vector3f curr_vel_NED;
412 130879 : if (!AP::ahrs().get_relative_position_NE_origin_float(curr_pos_NE) || !AP::ahrs().get_velocity_NED(curr_vel_NED)) {
413 0 : return;
414 : }
415 :
416 : // exit immediately if we can't convert waypoint origin to offset from ekf origin
417 130879 : Vector2f origin_NE;
418 130879 : if (!_origin.get_vector_xy_from_origin_NE_cm(origin_NE)) {
419 0 : return;
420 : }
421 : // convert from cm to meters
422 130879 : origin_NE *= 0.01f;
423 :
424 : // use _track_scalar_dt to slow down S-Curve time to prevent target moving too far in front of vehicle
425 130879 : Vector2f curr_target_vel = _pos_control.get_desired_velocity();
426 130879 : float track_scaler_dt = 1.0f;
427 130879 : if (is_positive(curr_target_vel.length())) {
428 130659 : Vector2f track_direction = curr_target_vel.normalized();
429 130659 : const float track_error = _pos_control.get_pos_error().tofloat().dot(track_direction);
430 130659 : float track_velocity = curr_vel_NED.xy().dot(track_direction);
431 : // set time scaler to be consistent with the achievable vehicle speed with a 5% buffer for short term variation.
432 130659 : const float time_scaler_dt_max = _overspeed_enabled ? AR_WPNAV_OVERSPEED_RATIO_MAX : 1.0f;
433 130659 : track_scaler_dt = constrain_float(0.05f + (track_velocity - _pos_control.get_pos_p().kP() * track_error) / curr_target_vel.length(), 0.0f, time_scaler_dt_max);
434 : }
435 : // change s-curve time speed with a time constant of maximum acceleration / maximum jerk
436 130879 : float track_scaler_tc = 1.0f;
437 130879 : if (is_positive(_pos_control.get_jerk_max())) {
438 130879 : track_scaler_tc = _pos_control.get_accel_max() / _pos_control.get_jerk_max();
439 : }
440 130879 : _track_scalar_dt += (track_scaler_dt - _track_scalar_dt) * (dt / track_scaler_tc);
441 :
442 : // target position, velocity and acceleration from straight line or spline calculators
443 130879 : Vector3p target_pos_3d{origin_NE.x, origin_NE.y, 0.0f};
444 130879 : Vector3f target_vel, target_accel;
445 :
446 : // update target position, velocity and acceleration
447 130879 : const float wp_radius = MAX(_radius, _turn_radius);
448 130879 : bool s_finished = _scurve_this_leg.advance_target_along_track(_scurve_prev_leg, _scurve_next_leg, wp_radius, _pos_control.get_lat_accel_max(), _fast_waypoint, _track_scalar_dt * dt, target_pos_3d, target_vel, target_accel);
449 :
450 : // pass new target to the position controller
451 130879 : init_pos_control_if_necessary();
452 130879 : Vector2p target_pos_ptype{target_pos_3d.x, target_pos_3d.y};
453 130879 : _pos_control.set_pos_vel_accel_target(target_pos_ptype, target_vel.xy(), target_accel.xy());
454 :
455 : // check if we've reached the waypoint
456 130879 : if (!_reached_destination && s_finished) {
457 : // "fast" waypoints are complete once the intermediate point reaches the destination
458 138 : if (_fast_waypoint) {
459 104 : _reached_destination = true;
460 : } else {
461 : // regular waypoints also require the vehicle to be within the waypoint radius or past the "finish line"
462 34 : const bool near_wp = current_loc.get_distance(_destination) <= _radius;
463 34 : const bool past_wp = current_loc.past_interval_finish_line(_origin, _destination);
464 34 : _reached_destination = near_wp || past_wp;
465 : }
466 : }
467 : }
468 :
469 : // update psc input shaping navigation controller
470 39492 : void AR_WPNav::update_psc_input_shaping(float dt)
471 : {
472 : // convert destination location to offset from EKF origin (in meters)
473 39492 : Vector2f pos_target_cm;
474 39492 : if (!_destination.get_vector_xy_from_origin_NE_cm(pos_target_cm)) {
475 0 : return;
476 : }
477 :
478 : // initialise position controller if not called recently
479 39492 : init_pos_control_if_necessary();
480 :
481 : // convert to meters and update target
482 39492 : const Vector2p pos_target = pos_target_cm.topostype() * 0.01;
483 39492 : _pos_control.input_pos_target(pos_target, dt);
484 :
485 : // update reached_destination
486 39492 : if (!_reached_destination) {
487 : // calculate position difference between destination and position controller input shaped target
488 39492 : Vector2p pos_target_diff = pos_target - _pos_control.get_pos_target();
489 : // vehicle has reached destination when the target is within 1cm of the destination and vehicle is within waypoint radius
490 39492 : _reached_destination = (pos_target_diff.length_squared() < sq(0.01)) && (_pos_control.get_pos_error().length_squared() < sq(_radius));
491 : }
492 : }
493 :
494 : // update straight-line distance and bearing from vehicle's current position to destination
495 171336 : void AR_WPNav::update_distance_and_bearing_to_destination()
496 : {
497 : // if no current location leave distance unchanged
498 171336 : Location current_loc;
499 171336 : if (!_orig_and_dest_valid || !AP::ahrs().get_location(current_loc)) {
500 0 : _distance_to_destination = 0.0f;
501 0 : _wp_bearing_cd = 0.0f;
502 0 : return;
503 : }
504 171336 : _distance_to_destination = current_loc.get_distance(_destination);
505 171336 : _wp_bearing_cd = current_loc.get_bearing_to(_destination);
506 : }
507 :
508 : // calculate steering and speed to drive along line from origin to destination waypoint
509 170371 : void AR_WPNav::update_steering_and_speed(const Location ¤t_loc, float dt)
510 : {
511 170371 : _cross_track_error = calc_crosstrack_error(current_loc);
512 :
513 : // update position controller
514 170371 : _pos_control.set_reversed(_reversed);
515 170371 : _pos_control.update(dt);
516 :
517 : // handle pivot turns
518 170371 : if (_pivot.active()) {
519 : // decelerate to zero
520 0 : _desired_speed_limited = _atc.get_desired_speed_accel_limited(0.0f, dt);
521 0 : _desired_heading_cd = _reversed ? wrap_360_cd(oa_wp_bearing_cd() + 18000) : oa_wp_bearing_cd();
522 0 : _desired_turn_rate_rads = is_zero(_desired_speed_limited) ? _pivot.get_turn_rate_rads(_desired_heading_cd * 0.01, dt) : 0;
523 0 : _desired_lat_accel = 0.0f;
524 : } else {
525 170371 : _desired_speed_limited = _pos_control.get_desired_speed();
526 170371 : _desired_turn_rate_rads = _pos_control.get_desired_turn_rate_rads();
527 170371 : _desired_lat_accel = _pos_control.get_desired_lat_accel();
528 : }
529 170371 : }
530 :
531 : // settor to allow vehicle code to provide turn related param values to this library (should be updated regularly)
532 12526 : void AR_WPNav::set_turn_params(float turn_radius, bool pivot_possible)
533 : {
534 12526 : _turn_radius = pivot_possible ? 0.0 : turn_radius;
535 12526 : _pivot.enable(pivot_possible);
536 12526 : }
537 :
538 : // calculate the crosstrack error
539 : // value is negative when the vehicle is on the path's left side
540 170371 : float AR_WPNav::calc_crosstrack_error(const Location& current_loc) const
541 : {
542 170371 : if (!_orig_and_dest_valid) {
543 0 : return 0.0f;
544 : }
545 :
546 : // get object avoidance adjusted origin and destination
547 170371 : const Location &orig = get_oa_origin();
548 170371 : const Location &dest = get_oa_destination();
549 :
550 : // calculate the NE position of destination relative to origin
551 170371 : Vector2f dest_from_origin = orig.get_distance_NE(dest);
552 :
553 : // return distance to destination if length of track is very small
554 170371 : if (dest_from_origin.length() < 1.0e-6f) {
555 2608 : return current_loc.get_distance_NE(dest).length();
556 : }
557 :
558 : // convert to a vector indicating direction only
559 167763 : dest_from_origin.normalize();
560 :
561 : // calculate the NE position of the vehicle relative to origin
562 167763 : const Vector2f veh_from_origin = orig.get_distance_NE(current_loc);
563 :
564 : // calculate distance to target track, for reporting
565 167763 : return veh_from_origin % dest_from_origin;
566 : }
567 :
568 : // calculate yaw change at next waypoint in degrees
569 : // returns zero if the angle cannot be calculated because some points are on top of others
570 112 : float AR_WPNav::get_corner_angle(const Location& loc1, const Location& loc2, const Location& loc3) const
571 : {
572 : // sanity check
573 112 : if (!loc1.initialised() || !loc2.initialised() || !loc3.initialised()) {
574 0 : return 0;
575 : }
576 112 : const float loc1_to_loc2_deg = loc1.get_bearing_to(loc2) * 0.01;
577 112 : const float loc2_to_loc3_deg = loc2.get_bearing_to(loc3) * 0.01;
578 112 : const float diff_yaw_deg = wrap_180(loc2_to_loc3_deg - loc1_to_loc2_deg);
579 112 : return diff_yaw_deg;
580 : }
581 :
582 : // helper function to initialise position controller if it hasn't been called recently
583 : // this should be called before updating the position controller with new targets but after the EKF has a good position estimate
584 170371 : void AR_WPNav::init_pos_control_if_necessary()
585 : {
586 : // initialise position controller if not called recently
587 170371 : if (!_pos_control.is_active()) {
588 80 : if (!_pos_control.init()) {
589 : // this should never fail because we should always have a valid position estimate at this point
590 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
591 0 : return;
592 : }
593 : }
594 : }
595 :
596 : // set origin and destination to stopping point
597 265 : bool AR_WPNav::set_origin_and_destination_to_stopping_point()
598 : {
599 : // initialise origin and destination to stopping point
600 265 : Location stopping_loc;
601 265 : if (!get_stopping_location(stopping_loc)) {
602 9 : return false;
603 : }
604 256 : _origin = _destination = stopping_loc;
605 256 : _orig_and_dest_valid = true;
606 256 : return true;
607 : }
608 :
609 : // check for changes in _base_speed_max or _nudge_speed_max
610 : // updates position controller limits and recalculate scurve path if required
611 170371 : void AR_WPNav::update_speed_max()
612 : {
613 170371 : const float speed_max = MAX(_base_speed_max, _nudge_speed_max);
614 :
615 : // ignore calls that do not change the speed
616 170371 : if (is_equal(speed_max, _pos_control.get_speed_max())) {
617 170355 : return;
618 : }
619 :
620 : // protect against rapid updates
621 16 : const uint32_t now_ms = AP_HAL::millis();
622 16 : if (now_ms - _last_speed_update_ms < AR_WPNAV_SPEED_UPDATE_MIN_MS) {
623 0 : return;
624 : }
625 16 : _last_speed_update_ms = now_ms;
626 :
627 : // update position controller max speed
628 16 : _pos_control.set_limits(speed_max, _pos_control.get_accel_max(), _pos_control.get_lat_accel_max(), _pos_control.get_jerk_max());
629 :
630 : // change track speed
631 16 : _scurve_this_leg.set_speed_max(_pos_control.get_speed_max(), _pos_control.get_speed_max(), _pos_control.get_speed_max());
632 16 : _scurve_next_leg.set_speed_max(_pos_control.get_speed_max(), _pos_control.get_speed_max(), _pos_control.get_speed_max());
633 : }
|
