Line data Source code
1 : #include "Rover.h"
2 :
3 35 : bool ModeGuided::_enter()
4 : {
5 : // initialise submode to stop or loiter
6 35 : if (rover.is_boat()) {
7 1 : if (!start_loiter()) {
8 0 : start_stop();
9 : }
10 : } else {
11 34 : start_stop();
12 : }
13 :
14 : // initialise waypoint navigation library
15 35 : g2.wp_nav.init();
16 :
17 35 : send_notification = false;
18 :
19 35 : return true;
20 : }
21 :
22 66819 : void ModeGuided::update()
23 : {
24 66819 : switch (_guided_mode) {
25 32852 : case SubMode::WP:
26 : {
27 : // check if we've reached the destination
28 32852 : if (!g2.wp_nav.reached_destination()) {
29 : // update navigation controller
30 32848 : navigate_to_waypoint();
31 : } else {
32 : // send notification
33 4 : if (send_notification) {
34 4 : send_notification = false;
35 4 : rover.gcs().send_mission_item_reached_message(0);
36 : }
37 :
38 : // we have reached the destination so stay here
39 4 : if (rover.is_boat()) {
40 4 : if (!start_loiter()) {
41 0 : stop_vehicle();
42 : }
43 : } else {
44 0 : stop_vehicle();
45 : }
46 : // update distance to destination
47 4 : _distance_to_destination = rover.current_loc.get_distance(g2.wp_nav.get_destination());
48 : }
49 32852 : break;
50 : }
51 :
52 8726 : case SubMode::HeadingAndSpeed:
53 : {
54 : // stop vehicle if target not updated within GUID_TIMEOUT seconds
55 8726 : if (have_attitude_target && (millis() - _des_att_time_ms) > get_timeout_ms()) {
56 1 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "target not received last %.1f secs, stopping", get_timeout_ms()/1000.0f);
57 1 : have_attitude_target = false;
58 : }
59 8726 : if (have_attitude_target) {
60 : // run steering and throttle controllers
61 8675 : calc_steering_to_heading(_desired_yaw_cd);
62 8675 : calc_throttle(calc_speed_nudge(_desired_speed, is_negative(_desired_speed)), true);
63 : } else {
64 : // we have reached the destination so stay here
65 51 : if (rover.is_boat()) {
66 0 : if (!start_loiter()) {
67 0 : stop_vehicle();
68 : }
69 : } else {
70 51 : stop_vehicle();
71 : }
72 : }
73 8726 : break;
74 : }
75 :
76 479 : case SubMode::TurnRateAndSpeed:
77 : {
78 : // stop vehicle if target not updated within GUID_TIMEOUT seconds
79 479 : if (have_attitude_target && (millis() - _des_att_time_ms) > get_timeout_ms()) {
80 1 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "target not received last %.1f secs, stopping", get_timeout_ms()/1000.0f);
81 1 : have_attitude_target = false;
82 : }
83 479 : if (have_attitude_target) {
84 : // run steering and throttle controllers
85 399 : float steering_out = attitude_control.get_steering_out_rate(radians(_desired_yaw_rate_cds * 0.01f),
86 399 : g2.motors.limit.steer_left,
87 399 : g2.motors.limit.steer_right,
88 : rover.G_Dt);
89 399 : set_steering(steering_out * 4500.0f);
90 399 : calc_throttle(calc_speed_nudge(_desired_speed, is_negative(_desired_speed)), true);
91 : } else {
92 : // we have reached the destination so stay here
93 80 : if (rover.is_boat()) {
94 0 : if (!start_loiter()) {
95 0 : stop_vehicle();
96 : }
97 : } else {
98 80 : stop_vehicle();
99 : }
100 : }
101 479 : break;
102 : }
103 :
104 1416 : case SubMode::Loiter:
105 : {
106 1416 : rover.mode_loiter.update();
107 1416 : break;
108 : }
109 :
110 1003 : case SubMode::SteeringAndThrottle:
111 : {
112 : // handle timeout
113 1003 : if (_have_strthr && (AP_HAL::millis() - _strthr_time_ms) > get_timeout_ms()) {
114 0 : _have_strthr = false;
115 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "target not received last %.1f secs, stopping", get_timeout_ms()/1000.0f);
116 : }
117 1003 : if (_have_strthr) {
118 : // pass latest steering and throttle directly to motors library
119 1003 : g2.motors.set_steering(_strthr_steering * 4500.0f, false);
120 1003 : g2.motors.set_throttle(_strthr_throttle * 100.0f);
121 : } else {
122 : // loiter or stop vehicle
123 0 : if (rover.is_boat()) {
124 0 : if (!start_loiter()) {
125 0 : stop_vehicle();
126 : }
127 : } else {
128 0 : stop_vehicle();
129 : }
130 : }
131 1003 : break;
132 : }
133 :
134 22343 : case SubMode::Stop:
135 22343 : stop_vehicle();
136 22343 : break;
137 :
138 0 : default:
139 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
140 0 : break;
141 : }
142 66819 : }
143 :
144 : // return heading (in degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)
145 28053 : float ModeGuided::wp_bearing() const
146 : {
147 28053 : switch (_guided_mode) {
148 13804 : case SubMode::WP:
149 13804 : return g2.wp_nav.wp_bearing_cd() * 0.01f;
150 3859 : case SubMode::HeadingAndSpeed:
151 : case SubMode::TurnRateAndSpeed:
152 3859 : return 0.0f;
153 596 : case SubMode::Loiter:
154 596 : return rover.mode_loiter.wp_bearing();
155 9794 : case SubMode::SteeringAndThrottle:
156 : case SubMode::Stop:
157 9794 : return 0.0f;
158 : }
159 :
160 : // we should never reach here but just in case, return 0
161 0 : return 0.0f;
162 : }
163 :
164 26713 : float ModeGuided::nav_bearing() const
165 : {
166 26713 : switch (_guided_mode) {
167 13139 : case SubMode::WP:
168 13139 : return g2.wp_nav.nav_bearing_cd() * 0.01f;
169 3674 : case SubMode::HeadingAndSpeed:
170 : case SubMode::TurnRateAndSpeed:
171 3674 : return 0.0f;
172 568 : case SubMode::Loiter:
173 568 : return rover.mode_loiter.nav_bearing();
174 9332 : case SubMode::SteeringAndThrottle:
175 : case SubMode::Stop:
176 9332 : return 0.0f;
177 : }
178 :
179 : // we should never reach here but just in case, return 0
180 0 : return 0.0f;
181 : }
182 :
183 28053 : float ModeGuided::crosstrack_error_m() const
184 : {
185 28053 : switch (_guided_mode) {
186 13804 : case SubMode::WP:
187 13804 : return g2.wp_nav.crosstrack_error_m();
188 3859 : case SubMode::HeadingAndSpeed:
189 : case SubMode::TurnRateAndSpeed:
190 3859 : return 0.0f;
191 596 : case SubMode::Loiter:
192 596 : return rover.mode_loiter.crosstrack_error_m();
193 9794 : case SubMode::SteeringAndThrottle:
194 : case SubMode::Stop:
195 9794 : return 0.0f;
196 : }
197 :
198 : // we should never reach here but just in case, return 0
199 0 : return 0.0f;
200 : }
201 :
202 13368 : float ModeGuided::get_desired_lat_accel() const
203 : {
204 13368 : switch (_guided_mode) {
205 6572 : case SubMode::WP:
206 6572 : return g2.wp_nav.get_lat_accel();
207 1841 : case SubMode::HeadingAndSpeed:
208 : case SubMode::TurnRateAndSpeed:
209 1841 : return 0.0f;
210 285 : case SubMode::Loiter:
211 285 : return rover.mode_loiter.get_desired_lat_accel();
212 4670 : case SubMode::SteeringAndThrottle:
213 : case SubMode::Stop:
214 4670 : return 0.0f;
215 : }
216 :
217 : // we should never reach here but just in case, return 0
218 0 : return 0.0f;
219 : }
220 :
221 : // return straight-line distance (in meters) to destination
222 28053 : float ModeGuided::get_distance_to_destination() const
223 : {
224 28053 : switch (_guided_mode) {
225 13804 : case SubMode::WP:
226 13804 : return _distance_to_destination;
227 3859 : case SubMode::HeadingAndSpeed:
228 : case SubMode::TurnRateAndSpeed:
229 3859 : return 0.0f;
230 596 : case SubMode::Loiter:
231 596 : return rover.mode_loiter.get_distance_to_destination();
232 9794 : case SubMode::SteeringAndThrottle:
233 : case SubMode::Stop:
234 9794 : return 0.0f;
235 : }
236 :
237 : // we should never reach here but just in case, return 0
238 0 : return 0.0f;
239 : }
240 :
241 : // return true if vehicle has reached or even passed destination
242 0 : bool ModeGuided::reached_destination() const
243 : {
244 0 : switch (_guided_mode) {
245 0 : case SubMode::WP:
246 0 : return g2.wp_nav.reached_destination();
247 0 : case SubMode::HeadingAndSpeed:
248 : case SubMode::TurnRateAndSpeed:
249 : case SubMode::Loiter:
250 : case SubMode::SteeringAndThrottle:
251 : case SubMode::Stop:
252 0 : return true;
253 : }
254 :
255 : // we should never reach here but just in case, return true is the safer option
256 0 : return true;
257 : }
258 :
259 : // set desired speed in m/s
260 8 : bool ModeGuided::set_desired_speed(float speed_ms)
261 : {
262 8 : switch (_guided_mode) {
263 8 : case SubMode::WP:
264 8 : return g2.wp_nav.set_speed_max(speed_ms);
265 0 : case SubMode::HeadingAndSpeed:
266 : case SubMode::TurnRateAndSpeed:
267 : // speed is set from mavlink message
268 0 : return false;
269 0 : case SubMode::Loiter:
270 0 : return rover.mode_loiter.set_desired_speed(speed_ms);
271 0 : case SubMode::SteeringAndThrottle:
272 : case SubMode::Stop:
273 : // no speed control
274 0 : return false;
275 : }
276 0 : return false;
277 : }
278 :
279 : // get desired location
280 13346 : bool ModeGuided::get_desired_location(Location& destination) const
281 : {
282 13346 : switch (_guided_mode) {
283 6568 : case SubMode::WP:
284 6568 : if (g2.wp_nav.is_destination_valid()) {
285 6568 : destination = g2.wp_nav.get_oa_destination();
286 6568 : return true;
287 : }
288 0 : return false;
289 1833 : case SubMode::HeadingAndSpeed:
290 : case SubMode::TurnRateAndSpeed:
291 : // not supported in these submodes
292 1833 : return false;
293 283 : case SubMode::Loiter:
294 : // get destination from loiter
295 283 : return rover.mode_loiter.get_desired_location(destination);
296 4662 : case SubMode::SteeringAndThrottle:
297 : case SubMode::Stop:
298 : // no desired location in this submode
299 4662 : break;
300 : }
301 :
302 : // should never get here but just in case
303 4662 : return false;
304 : }
305 :
306 : // set desired location
307 367 : bool ModeGuided::set_desired_location(const Location &destination, Location next_destination)
308 : {
309 : #if AP_FENCE_ENABLED
310 : // reject destination outside the fence
311 367 : if (!rover.fence.check_location_within_fence(destination)) {
312 1 : LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
313 1 : return false;
314 : }
315 : #endif
316 :
317 366 : if (use_scurves_for_navigation()) {
318 : // use scurves for navigation
319 0 : if (!g2.wp_nav.set_desired_location(destination, next_destination)) {
320 0 : return false;
321 : }
322 : } else {
323 : // use position controller input shaping for navigation
324 : // this does not support object avoidance but does allow faster updates of the target
325 366 : if (!g2.wp_nav.set_desired_location_expect_fast_update(destination)) {
326 0 : return false;
327 : }
328 : }
329 :
330 : // handle guided specific initialisation and logging
331 366 : _guided_mode = SubMode::WP;
332 366 : send_notification = true;
333 : #if HAL_LOGGING_ENABLED
334 366 : rover.Log_Write_GuidedTarget((uint8_t)_guided_mode, Vector3f(destination.lat, destination.lng, 0), Vector3f(g2.wp_nav.get_speed_max(), 0.0f, 0.0f));
335 : #endif
336 366 : return true;
337 : }
338 :
339 : // set desired attitude
340 1702 : void ModeGuided::set_desired_heading_and_speed(float yaw_angle_cd, float target_speed)
341 : {
342 : // initialisation and logging
343 1702 : _guided_mode = SubMode::HeadingAndSpeed;
344 1702 : _des_att_time_ms = AP_HAL::millis();
345 :
346 : // record targets
347 1702 : _desired_yaw_cd = yaw_angle_cd;
348 1702 : _desired_speed = target_speed;
349 1702 : have_attitude_target = true;
350 :
351 : #if HAL_LOGGING_ENABLED
352 : // log new target
353 1702 : rover.Log_Write_GuidedTarget((uint8_t)_guided_mode, Vector3f(_desired_yaw_cd, 0.0f, 0.0f), Vector3f(_desired_speed, 0.0f, 0.0f));
354 : #endif
355 1702 : }
356 :
357 0 : void ModeGuided::set_desired_heading_delta_and_speed(float yaw_delta_cd, float target_speed)
358 : {
359 : // handle initialisation
360 0 : if (_guided_mode != SubMode::HeadingAndSpeed) {
361 0 : _guided_mode = SubMode::HeadingAndSpeed;
362 0 : _desired_yaw_cd = ahrs.yaw_sensor;
363 : }
364 0 : set_desired_heading_and_speed(wrap_180_cd(_desired_yaw_cd + yaw_delta_cd), target_speed);
365 0 : }
366 :
367 : // set desired velocity
368 51 : void ModeGuided::set_desired_turn_rate_and_speed(float turn_rate_cds, float target_speed)
369 : {
370 : // handle initialisation
371 51 : _guided_mode = SubMode::TurnRateAndSpeed;
372 51 : _des_att_time_ms = AP_HAL::millis();
373 :
374 : // record targets
375 51 : _desired_yaw_rate_cds = turn_rate_cds;
376 51 : _desired_speed = target_speed;
377 51 : have_attitude_target = true;
378 :
379 : #if HAL_LOGGING_ENABLED
380 : // log new target
381 51 : rover.Log_Write_GuidedTarget((uint8_t)_guided_mode, Vector3f(_desired_yaw_rate_cds, 0.0f, 0.0f), Vector3f(_desired_speed, 0.0f, 0.0f));
382 : #endif
383 51 : }
384 :
385 : // set steering and throttle (both in the range -1 to +1)
386 20 : void ModeGuided::set_steering_and_throttle(float steering, float throttle)
387 : {
388 20 : _guided_mode = SubMode::SteeringAndThrottle;
389 20 : _strthr_time_ms = AP_HAL::millis();
390 20 : _strthr_steering = constrain_float(steering, -1.0f, 1.0f);
391 20 : _strthr_throttle = constrain_float(throttle, -1.0f, 1.0f);
392 20 : _have_strthr = true;
393 20 : }
394 :
395 5 : bool ModeGuided::start_loiter()
396 : {
397 5 : if (rover.mode_loiter.enter()) {
398 5 : _guided_mode = SubMode::Loiter;
399 5 : return true;
400 : }
401 0 : return false;
402 : }
403 :
404 :
405 : // start stopping vehicle as quickly as possible
406 34 : void ModeGuided::start_stop()
407 : {
408 34 : _guided_mode = SubMode::Stop;
409 34 : }
410 :
411 : // set guided timeout and movement limits
412 1 : void ModeGuided::limit_set(uint32_t timeout_ms, float horiz_max)
413 : {
414 1 : limit.timeout_ms = timeout_ms;
415 1 : limit.horiz_max = horiz_max;
416 1 : }
417 :
418 : // clear/turn off guided limits
419 40 : void ModeGuided::limit_clear()
420 : {
421 40 : limit.timeout_ms = 0;
422 40 : limit.horiz_max = 0.0f;
423 40 : }
424 :
425 : // initialise guided start time and location as reference for limit checking
426 : // only called from AUTO mode's start_guided method
427 1 : void ModeGuided::limit_init_time_and_location()
428 : {
429 1 : limit.start_time_ms = AP_HAL::millis();
430 1 : limit.start_loc = rover.current_loc;
431 1 : }
432 :
433 : // returns true if guided mode has breached a limit
434 500 : bool ModeGuided::limit_breached() const
435 : {
436 : // check if we have passed the timeout
437 500 : if ((limit.timeout_ms > 0) && (millis() - limit.start_time_ms >= limit.timeout_ms)) {
438 1 : return true;
439 : }
440 :
441 : // check if we have gone beyond horizontal limit
442 499 : if (is_positive(limit.horiz_max)) {
443 0 : return (rover.current_loc.get_distance(limit.start_loc) > limit.horiz_max);
444 : }
445 :
446 : // if we got this far we must be within limits
447 499 : return false;
448 : }
449 :
450 : // returns true if GUID_OPTIONS bit set to use scurve navigation instead of position controller input shaping
451 : // scurves provide path planning and object avoidance but cannot handle fast updates to the destination (for fast updates use position controller input shaping)
452 366 : bool ModeGuided::use_scurves_for_navigation() const
453 : {
454 366 : return ((g2.guided_options.get() & uint32_t(Options::SCurvesUsedForNavigation)) != 0);
455 : }
456 :
457 : // return guided mode timeout in milliseconds. Only used for velocity, throttle, heading or turn rate control
458 10081 : uint32_t ModeGuided::get_timeout_ms() const
459 : {
460 10081 : return MAX(g2.guided_timeout, 0.1) * 1000;
461 : }
|
