Line data Source code
1 : #include "Copter.h"
2 :
3 : #if MODE_GUIDED_ENABLED
4 :
5 : /*
6 : * Init and run calls for guided flight mode
7 : */
8 :
9 : static Vector3p guided_pos_target_ned_m; // position target (used by posvel controller only)
10 : static bool guided_is_terrain_alt; // true if guided_pos_target_ned_m.z should be offset by the terrain altitude
11 : static Vector3f guided_vel_target_ned_ms; // velocity target (used by pos_vel_accel controller and vel_accel controller)
12 : static Vector3f guided_accel_target_ned_mss; // acceleration target (used by pos_vel_accel controller vel_accel controller and accel controller)
13 : static uint32_t update_time_ms; // system time of last target update to pos_vel_accel, vel_accel or accel controller
14 :
15 : struct {
16 : uint32_t update_time_ms;
17 : Quaternion attitude_quat;
18 : Vector3f ang_vel_body;
19 : float climb_rate_ms; // climb rate in ms. Used if use_thrust is false
20 : float thrust_norm; // thrust from -1 to 1. Used if use_thrust is true
21 : bool use_thrust;
22 : } static guided_angle_state;
23 :
24 : struct Guided_Limit {
25 : uint32_t timeout_ms; // timeout (in seconds) from the time that guided is invoked
26 : float alt_min_m; // lower altitude limit in m above home (0 = no limit)
27 : float alt_max_m; // upper altitude limit in m above home (0 = no limit)
28 : float horiz_max_m; // horizontal position limit in m from where guided mode was initiated (0 = no limit)
29 : uint32_t start_time_ms; // system time in milliseconds that control was handed to the external computer
30 : Vector3p start_pos_ned_m; // start position as an offset from home in m. used for checking horiz_max limit
31 : } static guided_limit;
32 :
33 : bool ModeGuided::takeoff_complete; // true once takeoff has completed (used to trigger retracting of landing gear)
34 :
35 : // init - initialise guided controller
36 163 : bool ModeGuided::init(bool ignore_checks)
37 : {
38 : // start in velaccel control mode
39 163 : velaccel_control_start();
40 163 : guided_vel_target_ned_ms.zero();
41 163 : guided_accel_target_ned_mss.zero();
42 163 : send_notification = false;
43 :
44 : // clear pause state when entering guided mode
45 163 : _paused = false;
46 :
47 163 : return true;
48 : }
49 :
50 : // hold_position - bring vehicle to a stop and hold current position
51 : // using velocity/acceleration control with zero velocity and acceleration targets
52 105 : void ModeGuided::hold_position()
53 : {
54 : // check we are in velocity and acceleration control mode
55 105 : if (guided_mode != SubMode::VelAccel) {
56 1 : velaccel_control_start();
57 : }
58 105 : guided_vel_target_ned_ms.zero();
59 105 : guided_accel_target_ned_mss.zero();
60 105 : }
61 :
62 : // run - runs the guided controller
63 : // should be called at 100hz or more
64 4966750 : void ModeGuided::run()
65 : {
66 : // run pause control if the vehicle is paused
67 4966750 : if (_paused) {
68 9676 : pause_control_run();
69 9676 : return;
70 : }
71 :
72 : // call the correct auto controller
73 4957074 : switch (guided_mode) {
74 :
75 2465464 : case SubMode::TakeOff:
76 : // run takeoff controller
77 2465464 : takeoff_run();
78 2465464 : break;
79 :
80 0 : case SubMode::WP:
81 : // run waypoint controller
82 0 : wp_control_run();
83 0 : if (send_notification && wp_nav->reached_wp_destination()) {
84 0 : send_notification = false;
85 0 : gcs().send_mission_item_reached_message(0);
86 : }
87 0 : break;
88 :
89 752774 : case SubMode::Pos:
90 : // run position controller
91 752774 : pos_control_run();
92 752774 : break;
93 :
94 2475 : case SubMode::Accel:
95 2475 : accel_control_run();
96 2475 : break;
97 :
98 1607509 : case SubMode::VelAccel:
99 1607509 : velaccel_control_run();
100 1607509 : break;
101 :
102 29526 : case SubMode::PosVelAccel:
103 29526 : posvelaccel_control_run();
104 29526 : break;
105 :
106 99326 : case SubMode::Angle:
107 99326 : angle_control_run();
108 99326 : break;
109 : }
110 : }
111 :
112 : // returns true if the Guided-mode-option is set (see GUID_OPTIONS)
113 7547747 : bool ModeGuided::option_is_enabled(Option option) const
114 : {
115 7547747 : return (copter.g2.guided_options.get() & (uint32_t)option) != 0;
116 : }
117 :
118 145 : bool ModeGuided::allows_arming(AP_Arming::Method method) const
119 : {
120 : // always allow arming from the ground station or scripting
121 145 : if (AP_Arming::method_is_GCS(method) || method == AP_Arming::Method::SCRIPTING) {
122 145 : return true;
123 : }
124 :
125 : // optionally allow arming from the transmitter
126 0 : return option_is_enabled(Option::AllowArmingFromTX);
127 : };
128 :
129 : #if WEATHERVANE_ENABLED
130 3336953 : bool ModeGuided::allows_weathervaning() const
131 : {
132 3336953 : return option_is_enabled(Option::AllowWeatherVaning);
133 : }
134 : #endif
135 :
136 : // determine EKF reset handling method based on Guide submode
137 4984275 : bool ModeGuided::move_vehicle_on_ekf_reset() const
138 : {
139 : // call the correct auto controller
140 4984275 : switch (guided_mode) {
141 4192777 : case SubMode::TakeOff:
142 : case SubMode::Accel:
143 : case SubMode::VelAccel:
144 : case SubMode::Angle:
145 : // these submodes have no absolute position target so we reset the position target
146 4192777 : return false;
147 791498 : case SubMode::WP:
148 : case SubMode::Pos:
149 : case SubMode::PosVelAccel:
150 : // these submodes have absolute position targets so we smoothly slew the target upon an ekf reset
151 791498 : return true;
152 : }
153 :
154 : // should never reach here but just in case
155 0 : return true;
156 : }
157 :
158 : // initialises position controller to implement take-off
159 : // takeoff_alt_m is interpreted as alt-above-home (in m) or alt-above-terrain if a rangefinder is available
160 146 : bool ModeGuided::do_user_takeoff_start_m(float takeoff_alt_m)
161 : {
162 : // calculate target altitude and frame (either alt-above-ekf-origin or alt-above-terrain)
163 : float alt_target_m;
164 146 : bool alt_target_terrain = false;
165 : #if AP_RANGEFINDER_ENABLED
166 155 : if (wp_nav->rangefinder_used_and_healthy() &&
167 155 : wp_nav->get_terrain_source() == AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER &&
168 9 : takeoff_alt_m < copter.rangefinder.max_distance_orient(ROTATION_PITCH_270)) {
169 : // can't takeoff downwards
170 6 : if (takeoff_alt_m <= copter.rangefinder_state.alt_m) {
171 0 : return false;
172 : }
173 : // provide target altitude as alt-above-terrain
174 6 : alt_target_m = takeoff_alt_m;
175 6 : alt_target_terrain = true;
176 : } else
177 : #endif
178 : {
179 : // interpret altitude as alt-above-home
180 140 : Location target_loc = copter.current_loc;
181 140 : target_loc.set_alt_m(takeoff_alt_m, Location::AltFrame::ABOVE_HOME);
182 :
183 : // provide target altitude as alt-above-ekf-origin
184 140 : if (!target_loc.get_alt_m(Location::AltFrame::ABOVE_ORIGIN, alt_target_m)) {
185 : // this should never happen but we reject the command just in case
186 0 : return false;
187 : }
188 : }
189 :
190 146 : guided_mode = SubMode::TakeOff;
191 :
192 : // initialise yaw
193 146 : auto_yaw.set_mode(AutoYaw::Mode::HOLD);
194 :
195 : // clear i term when we're taking off
196 146 : pos_control->D_init_controller();
197 :
198 : // initialise alt for WP_NAVALT_MIN and set completion alt
199 146 : auto_takeoff.start_m(alt_target_m, alt_target_terrain);
200 :
201 : // record takeoff has not completed
202 146 : takeoff_complete = false;
203 :
204 146 : return true;
205 : }
206 :
207 : // initialise guided mode's waypoint navigation controller
208 0 : void ModeGuided::wp_control_start()
209 : {
210 : // set to position control mode
211 0 : guided_mode = SubMode::WP;
212 :
213 : // initialise waypoint and spline controller
214 0 : wp_nav->wp_and_spline_init_m();
215 :
216 : // initialise wpnav to stopping point
217 0 : Vector3p stopping_point_ned_m;
218 0 : wp_nav->get_wp_stopping_point_NED_m(stopping_point_ned_m);
219 0 : if (!wp_nav->set_wp_destination_NED_m(stopping_point_ned_m, false)) {
220 : // this should never happen because terrain data is not used
221 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
222 : }
223 :
224 : // initialise yaw
225 0 : auto_yaw.set_mode_to_default(false);
226 0 : }
227 :
228 : // run guided mode's waypoint navigation controller
229 0 : void ModeGuided::wp_control_run()
230 : {
231 : // if not armed set throttle to zero and exit immediately
232 0 : if (is_disarmed_or_landed()) {
233 : // do not spool down tradheli when on the ground with motor interlock enabled
234 0 : make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock());
235 0 : return;
236 : }
237 :
238 : // set motors to full range
239 0 : motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
240 :
241 : // run waypoint controller
242 0 : copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
243 :
244 : // call z-axis position controller (wpnav should have already updated it's alt target)
245 0 : pos_control->D_update_controller();
246 :
247 : // call attitude controller with auto yaw
248 0 : attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
249 : }
250 :
251 : // initialise position controller
252 222 : void ModeGuided::pva_control_start()
253 : {
254 : // initialise horizontal speed, acceleration
255 222 : pos_control->NE_set_max_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss());
256 222 : pos_control->NE_set_correction_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss());
257 :
258 : // initialize vertical speeds and acceleration
259 222 : pos_control->D_set_max_speed_accel_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss());
260 222 : pos_control->D_set_correction_speed_accel_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss());
261 :
262 : // initialise velocity controller
263 222 : pos_control->D_init_controller();
264 222 : pos_control->NE_init_controller();
265 :
266 : // initialise yaw
267 222 : auto_yaw.set_mode_to_default(false);
268 :
269 : // initialise terrain alt
270 222 : guided_is_terrain_alt = false;
271 222 : }
272 :
273 : // initialise guided mode's position controller
274 50 : void ModeGuided::pos_control_start()
275 : {
276 : // set to position control mode
277 50 : guided_mode = SubMode::Pos;
278 :
279 : // initialise position controller
280 50 : pva_control_start();
281 50 : }
282 :
283 : // initialise guided mode's acceleration controller
284 1 : void ModeGuided::accel_control_start()
285 : {
286 : // set guided_mode to acceleration controller
287 1 : guided_mode = SubMode::Accel;
288 :
289 : // initialise position controller
290 1 : pva_control_start();
291 1 : }
292 :
293 : // initialise guided mode's velocity and acceleration controller
294 170 : void ModeGuided::velaccel_control_start()
295 : {
296 : // set guided_mode to velocity and acceleration controller
297 170 : guided_mode = SubMode::VelAccel;
298 :
299 : // initialise position controller
300 170 : pva_control_start();
301 170 : }
302 :
303 : // initialise guided mode's position, velocity and acceleration controller
304 1 : void ModeGuided::posvelaccel_control_start()
305 : {
306 : // set guided_mode to position, velocity and acceleration controller
307 1 : guided_mode = SubMode::PosVelAccel;
308 :
309 : // initialise position controller
310 1 : pva_control_start();
311 1 : }
312 :
313 726624 : bool ModeGuided::is_taking_off() const
314 : {
315 726624 : return guided_mode == SubMode::TakeOff && !takeoff_complete;
316 : }
317 :
318 6 : bool ModeGuided::set_speed_NE_ms(float speed_ne_ms)
319 : {
320 : // initialise horizontal speed, acceleration
321 6 : pos_control->NE_set_max_speed_accel_m(speed_ne_ms, wp_nav->get_wp_acceleration_mss());
322 6 : pos_control->NE_set_correction_speed_accel_m(speed_ne_ms, wp_nav->get_wp_acceleration_mss());
323 6 : return true;
324 : }
325 :
326 0 : bool ModeGuided::set_speed_up_ms(float speed_up_ms)
327 : {
328 : // initialize vertical speeds and acceleration
329 0 : pos_control->D_set_max_speed_accel_m(wp_nav->get_default_speed_down_ms(), speed_up_ms, wp_nav->get_accel_D_mss());
330 0 : pos_control->D_set_correction_speed_accel_m(wp_nav->get_default_speed_down_ms(), speed_up_ms, wp_nav->get_accel_D_mss());
331 0 : return true;
332 : }
333 :
334 0 : bool ModeGuided::set_speed_down_ms(float speed_down_ms)
335 : {
336 : // initialize vertical speeds and acceleration
337 0 : pos_control->D_set_max_speed_accel_m(speed_down_ms, wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss());
338 0 : pos_control->D_set_correction_speed_accel_m(speed_down_ms, wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss());
339 0 : return true;
340 : }
341 :
342 : // initialise guided mode's angle controller
343 9 : void ModeGuided::angle_control_start()
344 : {
345 : // set guided_mode to velocity controller
346 9 : guided_mode = SubMode::Angle;
347 :
348 : // set vertical speed and acceleration limits
349 9 : pos_control->D_set_max_speed_accel_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss());
350 9 : pos_control->D_set_correction_speed_accel_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss());
351 :
352 : // initialise the vertical position controller
353 9 : if (!pos_control->D_is_active()) {
354 0 : pos_control->D_init_controller();
355 : }
356 :
357 : // initialise targets
358 9 : guided_angle_state.update_time_ms = millis();
359 9 : guided_angle_state.attitude_quat.from_euler(Vector3f{0.0, 0.0, attitude_control->get_att_target_euler_rad().z});
360 9 : guided_angle_state.ang_vel_body.zero();
361 9 : guided_angle_state.climb_rate_ms = 0.0f;
362 9 : }
363 :
364 : // set_pos_ned_m - sets guided mode's target pos_ned_m
365 : // Returns true if the fence is enabled and guided waypoint is within the fence
366 : // else return false if the waypoint is outside the fence
367 53 : bool ModeGuided::set_pos_NED_m(const Vector3p& pos_ned_m, bool use_yaw, float yaw_rad, bool use_yaw_rate, float yaw_rate_rads, bool relative_yaw, bool is_terrain_alt)
368 : {
369 : #if AP_FENCE_ENABLED
370 : // reject destination if outside the fence
371 53 : const Location dest_loc = Location::from_ekf_offset_NED_m(pos_ned_m, is_terrain_alt ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
372 53 : if (!copter.fence.check_location_within_fence(dest_loc)) {
373 0 : LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
374 : // failure is propagated to GCS with NAK
375 0 : return false;
376 : }
377 : #endif
378 :
379 : // if configured to use wpnav for position control
380 53 : if (use_wpnav_for_position_control()) {
381 : // ensure we are in position control mode
382 0 : if (guided_mode != SubMode::WP) {
383 0 : wp_control_start();
384 : }
385 :
386 : // set yaw state
387 0 : set_yaw_state_rad(use_yaw, yaw_rad, use_yaw_rate, yaw_rate_rads, relative_yaw);
388 :
389 : // no need to check return status because terrain data is not used
390 0 : wp_nav->set_wp_destination_NED_m(pos_ned_m, is_terrain_alt);
391 :
392 : #if HAL_LOGGING_ENABLED
393 : // log target
394 0 : copter.Log_Write_Guided_Position_Target(guided_mode, pos_ned_m, is_terrain_alt, Vector3f(), Vector3f());
395 : #endif
396 0 : send_notification = true;
397 0 : return true;
398 : }
399 :
400 : // if configured to use position controller for position control
401 : // ensure we are in position control mode
402 53 : if (guided_mode != SubMode::Pos) {
403 27 : pos_control_start();
404 : }
405 :
406 : // initialise terrain following if needed
407 53 : if (is_terrain_alt) {
408 : // get current alt above terrain
409 : float terrain_d_m;
410 0 : if (!wp_nav->get_terrain_D_m(terrain_d_m)) {
411 : // if we don't have terrain altitude then stop
412 0 : hold_position();
413 0 : return false;
414 : }
415 : // convert origin to alt-above-terrain if necessary
416 0 : if (!guided_is_terrain_alt) {
417 : // new destination is alt-above-terrain, previous destination was alt-above-ekf-origin
418 0 : pos_control->init_pos_terrain_D_m(terrain_d_m);
419 : }
420 : } else {
421 53 : pos_control->init_pos_terrain_D_m(0.0);
422 : }
423 :
424 : // set yaw state
425 53 : set_yaw_state_rad(use_yaw, yaw_rad, use_yaw_rate, yaw_rate_rads, relative_yaw);
426 :
427 : // set position target and zero velocity and acceleration
428 53 : guided_pos_target_ned_m = pos_ned_m;
429 53 : guided_is_terrain_alt = is_terrain_alt;
430 53 : guided_vel_target_ned_ms.zero();
431 53 : guided_accel_target_ned_mss.zero();
432 53 : update_time_ms = millis();
433 :
434 : #if HAL_LOGGING_ENABLED
435 : // log target
436 53 : copter.Log_Write_Guided_Position_Target(guided_mode, guided_pos_target_ned_m, guided_is_terrain_alt, guided_vel_target_ned_ms, guided_accel_target_ned_mss);
437 : #endif
438 :
439 53 : send_notification = true;
440 :
441 53 : return true;
442 : }
443 :
444 62098 : bool ModeGuided::get_wp(Location& destination) const
445 : {
446 62098 : switch (guided_mode) {
447 0 : case SubMode::WP:
448 0 : return wp_nav->get_oa_wp_destination(destination);
449 9500 : case SubMode::Pos:
450 9500 : destination = Location::from_ekf_offset_NED_m(guided_pos_target_ned_m, guided_is_terrain_alt ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
451 9500 : return true;
452 52598 : case SubMode::Angle:
453 : case SubMode::TakeOff:
454 : case SubMode::Accel:
455 : case SubMode::VelAccel:
456 : case SubMode::PosVelAccel:
457 52598 : break;
458 : }
459 :
460 52598 : return false;
461 : }
462 :
463 : // sets guided mode's target from a Location object
464 : // returns false if destination could not be set (probably caused by missing terrain data)
465 : // or if the fence is enabled and guided waypoint is outside the fence
466 422 : bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float yaw_rad, bool use_yaw_rate, float yaw_rate_rads, bool relative_yaw)
467 : {
468 : #if AP_FENCE_ENABLED
469 : // reject destination outside the fence.
470 : // Note: there is a danger that a target specified as a terrain altitude might not be checked if the conversion to alt-above-home fails
471 422 : if (!copter.fence.check_location_within_fence(dest_loc)) {
472 1 : LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
473 : // failure is propagated to GCS with NAK
474 1 : return false;
475 : }
476 : #endif
477 :
478 : // if using wpnav for position control
479 421 : if (use_wpnav_for_position_control()) {
480 0 : if (guided_mode != SubMode::WP) {
481 0 : wp_control_start();
482 : }
483 :
484 0 : if (!wp_nav->set_wp_destination_loc(dest_loc)) {
485 : // failure to set destination can only be because of missing terrain data
486 0 : LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION);
487 : // failure is propagated to GCS with NAK
488 0 : return false;
489 : }
490 :
491 : // set yaw state
492 0 : set_yaw_state_rad(use_yaw, yaw_rad, use_yaw_rate, yaw_rate_rads, relative_yaw);
493 :
494 : #if HAL_LOGGING_ENABLED
495 : // log target
496 0 : copter.Log_Write_Guided_Position_Target(guided_mode, Vector3p(dest_loc.lat, dest_loc.lng, dest_loc.alt), (dest_loc.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN), Vector3f(), Vector3f());
497 : #endif
498 :
499 0 : send_notification = true;
500 0 : return true;
501 : }
502 :
503 : // set position target and zero velocity and acceleration
504 421 : Vector3p pos_target_ned_m;
505 : bool is_terrain_alt;
506 421 : if (!wp_nav->get_vector_NED_m(dest_loc, pos_target_ned_m, is_terrain_alt)) {
507 8 : return false;
508 : }
509 :
510 : // if configured to use position controller for position control
511 : // ensure we are in position control mode
512 413 : if (guided_mode != SubMode::Pos) {
513 23 : pos_control_start();
514 : }
515 :
516 : // set yaw state
517 413 : set_yaw_state_rad(use_yaw, yaw_rad, use_yaw_rate, yaw_rate_rads, relative_yaw);
518 :
519 : // initialise terrain following if needed
520 413 : if (is_terrain_alt) {
521 : // get current alt above terrain
522 : float terrain_d_m;
523 92 : if (!wp_nav->get_terrain_D_m(terrain_d_m)) {
524 : // if we don't have terrain altitude then stop
525 0 : hold_position();
526 0 : return false;
527 : }
528 : // convert origin to alt-above-terrain if necessary
529 92 : if (!guided_is_terrain_alt) {
530 : // new destination is alt-above-terrain, previous destination was alt-above-ekf-origin
531 1 : pos_control->init_pos_terrain_D_m(terrain_d_m);
532 : }
533 : } else {
534 321 : pos_control->init_pos_terrain_D_m(0.0);
535 : }
536 :
537 413 : guided_pos_target_ned_m = pos_target_ned_m;
538 413 : guided_is_terrain_alt = is_terrain_alt;
539 413 : guided_vel_target_ned_ms.zero();
540 413 : guided_accel_target_ned_mss.zero();
541 413 : update_time_ms = millis();
542 :
543 : // log target
544 : #if HAL_LOGGING_ENABLED
545 413 : copter.Log_Write_Guided_Position_Target(guided_mode, Vector3p(dest_loc.lat, dest_loc.lng, dest_loc.alt), guided_is_terrain_alt, guided_vel_target_ned_ms, guided_accel_target_ned_mss);
546 : #endif
547 :
548 413 : send_notification = true;
549 :
550 413 : return true;
551 : }
552 :
553 : // set_vel_accel_NED_m - sets guided mode's target velocity and acceleration
554 1 : void ModeGuided::set_accel_NED_mss(const Vector3f& accel_ned_mss, bool use_yaw, float yaw_rad, bool use_yaw_rate, float yaw_rate_rads, bool relative_yaw, bool log_request)
555 : {
556 : // check we are in acceleration control mode
557 1 : if (guided_mode != SubMode::Accel) {
558 1 : accel_control_start();
559 : }
560 :
561 : // set yaw state
562 1 : set_yaw_state_rad(use_yaw, yaw_rad, use_yaw_rate, yaw_rate_rads, relative_yaw);
563 :
564 : // set velocity and acceleration targets and zero position
565 1 : guided_pos_target_ned_m.zero();
566 1 : guided_is_terrain_alt = false;
567 1 : guided_vel_target_ned_ms.zero();
568 1 : guided_accel_target_ned_mss = accel_ned_mss;
569 1 : update_time_ms = millis();
570 :
571 : #if HAL_LOGGING_ENABLED
572 : // log target
573 1 : if (log_request) {
574 1 : copter.Log_Write_Guided_Position_Target(guided_mode, guided_pos_target_ned_m, guided_is_terrain_alt, guided_vel_target_ned_ms, guided_accel_target_ned_mss);
575 : }
576 : #endif
577 1 : }
578 :
579 : // set_vel_NED_ms - sets guided mode's target velocity
580 0 : void ModeGuided::set_vel_NED_ms(const Vector3f& vel_ned_ms, bool use_yaw, float yaw_rad, bool use_yaw_rate, float yaw_rate_rads, bool relative_yaw, bool log_request)
581 : {
582 0 : set_vel_accel_NED_m(vel_ned_ms, Vector3f(), use_yaw, yaw_rad, use_yaw_rate, yaw_rate_rads, relative_yaw, log_request);
583 0 : }
584 :
585 : // set_vel_accel_NED_m - sets guided mode's target velocity and acceleration
586 2860 : void ModeGuided::set_vel_accel_NED_m(const Vector3f& vel_ned_ms, const Vector3f& accel_ned_mss, bool use_yaw, float yaw_rad, bool use_yaw_rate, float yaw_rate_rads, bool relative_yaw, bool log_request)
587 : {
588 : // check we are in velocity and acceleration control mode
589 2860 : if (guided_mode != SubMode::VelAccel) {
590 6 : velaccel_control_start();
591 : }
592 :
593 : // set yaw state
594 2860 : set_yaw_state_rad(use_yaw, yaw_rad, use_yaw_rate, yaw_rate_rads, relative_yaw);
595 :
596 : // set velocity and acceleration targets and zero position
597 2860 : guided_pos_target_ned_m.zero();
598 2860 : guided_is_terrain_alt = false;
599 2860 : guided_vel_target_ned_ms = vel_ned_ms;
600 2860 : guided_accel_target_ned_mss = accel_ned_mss;
601 2860 : update_time_ms = millis();
602 :
603 : #if HAL_LOGGING_ENABLED
604 : // log target
605 2860 : if (log_request) {
606 2860 : copter.Log_Write_Guided_Position_Target(guided_mode, guided_pos_target_ned_m, guided_is_terrain_alt, guided_vel_target_ned_ms, guided_accel_target_ned_mss);
607 : }
608 : #endif
609 2860 : }
610 :
611 : // set guided mode position and velocity target
612 0 : bool ModeGuided::set_pos_vel_NED_m(const Vector3p& pos_ned_m, const Vector3f& vel_ned_ms, bool use_yaw, float yaw_rad, bool use_yaw_rate, float yaw_rate_rads, bool relative_yaw)
613 : {
614 0 : return set_pos_vel_accel_NED_m(pos_ned_m, vel_ned_ms, Vector3f(), use_yaw, yaw_rad, use_yaw_rate, yaw_rate_rads, relative_yaw);
615 : }
616 :
617 : // set_pos_vel_accel_NED_m - set guided mode position, velocity and acceleration target
618 1 : bool ModeGuided::set_pos_vel_accel_NED_m(const Vector3p& pos_ned_m, const Vector3f& vel_ned_ms, const Vector3f& accel_ned_mss, bool use_yaw, float yaw_rad, bool use_yaw_rate, float yaw_rate_rads, bool relative_yaw)
619 : {
620 : #if AP_FENCE_ENABLED
621 : // reject destination if outside the fence
622 1 : const Location dest_loc = Location::from_ekf_offset_NED_m(pos_ned_m, Location::AltFrame::ABOVE_ORIGIN);
623 1 : if (!copter.fence.check_location_within_fence(dest_loc)) {
624 0 : LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
625 : // failure is propagated to GCS with NAK
626 0 : return false;
627 : }
628 : #endif
629 :
630 : // check we are in position, velocity and acceleration control mode
631 1 : if (guided_mode != SubMode::PosVelAccel) {
632 1 : posvelaccel_control_start();
633 : }
634 :
635 : // set yaw state
636 1 : set_yaw_state_rad(use_yaw, yaw_rad, use_yaw_rate, yaw_rate_rads, relative_yaw);
637 :
638 1 : update_time_ms = millis();
639 1 : guided_pos_target_ned_m = pos_ned_m;
640 1 : guided_is_terrain_alt = false;
641 1 : guided_vel_target_ned_ms = vel_ned_ms;
642 1 : guided_accel_target_ned_mss = accel_ned_mss;
643 :
644 : #if HAL_LOGGING_ENABLED
645 : // log target
646 1 : copter.Log_Write_Guided_Position_Target(guided_mode, guided_pos_target_ned_m, guided_is_terrain_alt, guided_vel_target_ned_ms, guided_accel_target_ned_mss);
647 : #endif
648 1 : return true;
649 : }
650 :
651 : // returns true if GUIDED_OPTIONS param suggests SET_ATTITUDE_TARGET's "thrust" field should be interpreted as thrust instead of climb rate
652 61 : bool ModeGuided::set_attitude_target_provides_thrust() const
653 : {
654 61 : return option_is_enabled(Option::SetAttitudeTarget_ThrustAsThrust);
655 : }
656 :
657 : // returns true if GUIDED_OPTIONS param specifies position should be controlled (when velocity and/or acceleration control is active)
658 314451 : bool ModeGuided::stabilizing_pos_NE() const
659 : {
660 314451 : return !option_is_enabled(Option::DoNotStabilizePositionXY);
661 : }
662 :
663 : // returns true if GUIDED_OPTIONS param specifies velocity should be controlled (when acceleration control is active)
664 567375 : bool ModeGuided::stabilizing_vel_NE() const
665 : {
666 567375 : return !option_is_enabled(Option::DoNotStabilizeVelocityXY);
667 : }
668 :
669 : // returns true if GUIDED_OPTIONS param specifies waypoint navigation should be used for position control (allow path planning to be used but updates must be slower)
670 474 : bool ModeGuided::use_wpnav_for_position_control() const
671 : {
672 474 : return option_is_enabled(Option::WPNavUsedForPosControl);
673 : }
674 :
675 : // Sets guided's angular target submode: Using a rotation quaternion, angular velocity, and climbrate or thrust (depends on user option)
676 : // attitude_quat: IF zero: ang_vel_body (body frame angular velocity) must be provided even if all zeroes
677 : // IF non-zero: attitude_control is performed using both the attitude quaternion and body frame angular velocity
678 : // ang_vel_body: body frame angular velocity (rad/s)
679 : // climb_rate_ms_or_thrust: represents either the climb_rate (m/s) or thrust scaled from [0, 1], unitless
680 : // use_thrust: IF true: climb_rate_ms_or_thrust represents thrust
681 : // IF false: climb_rate_ms_or_thrust represents climb_rate (m/s)
682 306 : void ModeGuided::set_angle(const Quaternion &attitude_quat, const Vector3f &ang_vel_body, float climb_rate_ms_or_thrust, bool use_thrust)
683 : {
684 : // check we are in velocity control mode
685 306 : if (guided_mode != SubMode::Angle) {
686 7 : angle_control_start();
687 299 : } else if (!use_thrust && guided_angle_state.use_thrust) {
688 : // Already angle control but changing from thrust to climb rate
689 0 : pos_control->D_init_controller();
690 : }
691 :
692 306 : guided_angle_state.attitude_quat = attitude_quat;
693 306 : guided_angle_state.ang_vel_body = ang_vel_body;
694 :
695 306 : guided_angle_state.use_thrust = use_thrust;
696 306 : if (use_thrust) {
697 246 : guided_angle_state.thrust_norm = climb_rate_ms_or_thrust;
698 246 : guided_angle_state.climb_rate_ms = 0.0f;
699 : } else {
700 60 : guided_angle_state.thrust_norm = 0.0f;
701 60 : guided_angle_state.climb_rate_ms = climb_rate_ms_or_thrust;
702 : }
703 :
704 306 : guided_angle_state.update_time_ms = millis();
705 :
706 : // convert quaternion to euler angles
707 : float roll_rad, pitch_rad, yaw_rad;
708 306 : attitude_quat.to_euler(roll_rad, pitch_rad, yaw_rad);
709 :
710 : #if HAL_LOGGING_ENABLED
711 : // log target
712 306 : copter.Log_Write_Guided_Attitude_Target(guided_mode, roll_rad, pitch_rad, yaw_rad, ang_vel_body, guided_angle_state.thrust_norm, guided_angle_state.climb_rate_ms);
713 : #endif
714 306 : }
715 :
716 : // takeoff_run - takeoff in guided mode
717 : // called by guided_run at 100hz or more
718 2465464 : void ModeGuided::takeoff_run()
719 : {
720 2465464 : auto_takeoff.run();
721 2465464 : if (auto_takeoff.complete && !takeoff_complete) {
722 67 : takeoff_complete = true;
723 : #if AP_FENCE_ENABLED
724 67 : copter.fence.auto_enable_fence_after_takeoff();
725 : #endif
726 : #if AP_LANDINGGEAR_ENABLED
727 : // optionally retract landing gear
728 67 : copter.landinggear.retract_after_takeoff();
729 : #endif
730 : }
731 2465464 : }
732 :
733 : // pos_control_run - runs the guided position controller
734 : // called from guided_run
735 752774 : void ModeGuided::pos_control_run()
736 : {
737 : // if not armed set throttle to zero and exit immediately
738 752774 : if (is_disarmed_or_landed()) {
739 : // do not spool down tradheli when on the ground with motor interlock enabled
740 13209 : make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock());
741 13209 : return;
742 : }
743 :
744 : // calculate terrain adjustments
745 739565 : float terrain_d_m = 0.0f;
746 739565 : if (guided_is_terrain_alt && !wp_nav->get_terrain_D_m(terrain_d_m)) {
747 : // failure to set destination can only be because of missing terrain data
748 0 : copter.failsafe_terrain_on_event();
749 0 : return;
750 : }
751 :
752 : // set motors to full range
753 739565 : motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
754 :
755 : // send position and velocity targets to position controller
756 739565 : guided_accel_target_ned_mss.zero();
757 739565 : guided_vel_target_ned_ms.zero();
758 :
759 : // stop rotating if no updates received within timeout_ms
760 739565 : if (millis() - update_time_ms > get_timeout_ms()) {
761 528250 : if ((auto_yaw.mode() == AutoYaw::Mode::RATE) || (auto_yaw.mode() == AutoYaw::Mode::ANGLE_RATE)) {
762 12 : auto_yaw.set_mode(AutoYaw::Mode::HOLD);
763 : }
764 : }
765 :
766 739565 : float terrain_margin_m = 0.0; // Vertical buffer size in m
767 739565 : if (guided_is_terrain_alt) {
768 56715 : terrain_margin_m = MIN(copter.wp_nav->get_terrain_margin_m(), 0.5 * fabsF(guided_pos_target_ned_m.z));
769 : }
770 739565 : pos_control->input_pos_NED_m(guided_pos_target_ned_m, terrain_d_m, terrain_margin_m);
771 :
772 : // run position controllers
773 739565 : pos_control->NE_update_controller();
774 739565 : pos_control->D_update_controller();
775 :
776 : // call attitude controller with auto yaw
777 739565 : attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
778 : }
779 :
780 : // velaccel_control_run - runs the guided velocity controller
781 : // called from guided_run
782 2475 : void ModeGuided::accel_control_run()
783 : {
784 : // if not armed set throttle to zero and exit immediately
785 2475 : if (is_disarmed_or_landed()) {
786 : // do not spool down tradheli when on the ground with motor interlock enabled
787 0 : make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock());
788 0 : return;
789 : }
790 :
791 : // set motors to full range
792 2475 : motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
793 :
794 : // set velocity to zero and stop rotating if no updates received for 3 seconds
795 2475 : uint32_t tnow = millis();
796 2475 : if (tnow - update_time_ms > get_timeout_ms()) {
797 0 : guided_vel_target_ned_ms.zero();
798 0 : guided_accel_target_ned_mss.zero();
799 0 : if ((auto_yaw.mode() == AutoYaw::Mode::RATE) || (auto_yaw.mode() == AutoYaw::Mode::ANGLE_RATE)) {
800 0 : auto_yaw.set_mode(AutoYaw::Mode::HOLD);
801 : }
802 0 : pos_control->input_vel_accel_NE_m(guided_vel_target_ned_ms.xy(), guided_accel_target_ned_mss.xy(), false);
803 0 : pos_control->input_vel_accel_D_m(guided_vel_target_ned_ms.z, guided_accel_target_ned_mss.z, false);
804 : } else {
805 : // update position controller with new target
806 2475 : pos_control->input_accel_NE_m(guided_accel_target_ned_mss.xy());
807 2475 : if (!stabilizing_vel_NE()) {
808 : // set position and velocity errors to zero
809 0 : pos_control->NE_stop_vel_stabilisation();
810 2475 : } else if (!stabilizing_pos_NE()) {
811 : // set position errors to zero
812 0 : pos_control->NE_stop_pos_stabilisation();
813 : }
814 2475 : pos_control->input_accel_D_m(guided_accel_target_ned_mss.z);
815 : }
816 :
817 : // call velocity controller which includes z axis controller
818 2475 : pos_control->NE_update_controller();
819 2475 : pos_control->D_update_controller();
820 :
821 : // call attitude controller with auto yaw
822 2475 : attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
823 : }
824 :
825 : // velaccel_control_run - runs the guided velocity and acceleration controller
826 : // called from guided_run
827 1607509 : void ModeGuided::velaccel_control_run()
828 : {
829 : // if not armed set throttle to zero and exit immediately
830 1607509 : if (is_disarmed_or_landed()) {
831 : // do not spool down tradheli when on the ground with motor interlock enabled
832 1354585 : make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock());
833 1354585 : return;
834 : }
835 :
836 : // set motors to full range
837 252924 : motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
838 :
839 : // set velocity to zero and stop rotating if no updates received for 3 seconds
840 252924 : uint32_t tnow = millis();
841 252924 : if (tnow - update_time_ms > get_timeout_ms()) {
842 17707 : guided_vel_target_ned_ms.zero();
843 17707 : guided_accel_target_ned_mss.zero();
844 17707 : if ((auto_yaw.mode() == AutoYaw::Mode::RATE) || (auto_yaw.mode() == AutoYaw::Mode::ANGLE_RATE)) {
845 0 : auto_yaw.set_mode(AutoYaw::Mode::HOLD);
846 : }
847 : }
848 :
849 252924 : bool do_avoid = false;
850 : #if AP_AVOIDANCE_ENABLED
851 : // limit the velocity for obstacle/fence avoidance
852 252924 : copter.avoid.adjust_velocity_NED_m(guided_vel_target_ned_ms, pos_control->NE_get_pos_p().kP(), pos_control->NE_get_max_accel_mss(), pos_control->D_get_pos_p().kP(), pos_control->D_get_max_accel_mss(), G_Dt);
853 252924 : do_avoid = copter.avoid.limits_active();
854 : #endif
855 :
856 : // update position controller with new target
857 :
858 252924 : if (!stabilizing_vel_NE() && !do_avoid) {
859 : // set the current commanded xy vel to the desired vel
860 0 : guided_vel_target_ned_ms.xy() = pos_control->get_vel_desired_NED_ms().xy();
861 : }
862 252924 : pos_control->input_vel_accel_NE_m(guided_vel_target_ned_ms.xy(), guided_accel_target_ned_mss.xy(), false);
863 252924 : if (!stabilizing_vel_NE() && !do_avoid) {
864 : // set position and velocity errors to zero
865 0 : pos_control->NE_stop_vel_stabilisation();
866 252924 : } else if (!stabilizing_pos_NE() && !do_avoid) {
867 : // set position errors to zero
868 0 : pos_control->NE_stop_pos_stabilisation();
869 : }
870 252924 : pos_control->input_vel_accel_D_m(guided_vel_target_ned_ms.z, guided_accel_target_ned_mss.z, false);
871 :
872 : // call velocity controller which includes z axis controller
873 252924 : pos_control->NE_update_controller();
874 252924 : pos_control->D_update_controller();
875 :
876 : // call attitude controller with auto yaw
877 252924 : attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
878 : }
879 :
880 : // pause_control_run - runs the guided mode pause controller
881 : // called from guided_run
882 9676 : void ModeGuided::pause_control_run()
883 : {
884 : // if not armed set throttle to zero and exit immediately
885 9676 : if (is_disarmed_or_landed()) {
886 : // do not spool down tradheli when on the ground with motor interlock enabled
887 0 : make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock());
888 0 : return;
889 : }
890 :
891 : // set motors to full range
892 9676 : motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
893 :
894 : // set the horizontal velocity and acceleration targets to zero
895 9676 : Vector2f vel_xy_zero, accel_xy_zero;
896 9676 : pos_control->input_vel_accel_NE_m(vel_xy_zero, accel_xy_zero, false);
897 :
898 : // set the vertical velocity and acceleration targets to zero
899 9676 : float vel_d_zero = 0.0;
900 9676 : pos_control->input_vel_accel_D_m(vel_d_zero, 0.0, false);
901 :
902 : // call velocity controller which includes z axis controller
903 9676 : pos_control->NE_update_controller();
904 9676 : pos_control->D_update_controller();
905 :
906 : // call attitude controller
907 9676 : attitude_control->input_thrust_vector_rate_heading_rads(pos_control->get_thrust_vector(), 0.0);
908 : }
909 :
910 : // posvelaccel_control_run - runs the guided position, velocity and acceleration controller
911 : // called from guided_run
912 29526 : void ModeGuided::posvelaccel_control_run()
913 : {
914 : // if not armed set throttle to zero and exit immediately
915 29526 : if (is_disarmed_or_landed()) {
916 : // do not spool down tradheli when on the ground with motor interlock enabled
917 0 : make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock());
918 0 : return;
919 : }
920 :
921 : // set motors to full range
922 29526 : motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
923 :
924 : // set velocity to zero and stop rotating if no updates received for 3 seconds
925 29526 : uint32_t tnow = millis();
926 29526 : if (tnow - update_time_ms > get_timeout_ms()) {
927 0 : guided_vel_target_ned_ms.zero();
928 0 : guided_accel_target_ned_mss.zero();
929 0 : if ((auto_yaw.mode() == AutoYaw::Mode::RATE) || (auto_yaw.mode() == AutoYaw::Mode::ANGLE_RATE)) {
930 0 : auto_yaw.set_mode(AutoYaw::Mode::HOLD);
931 : }
932 : }
933 :
934 : // send position and velocity targets to position controller
935 29526 : if (!stabilizing_vel_NE()) {
936 : // set the current commanded xy pos to the target pos and xy vel to the desired vel
937 0 : guided_pos_target_ned_m.xy() = pos_control->get_pos_desired_NED_m().xy();
938 0 : guided_vel_target_ned_ms.xy() = pos_control->get_vel_desired_NED_ms().xy();
939 29526 : } else if (!stabilizing_pos_NE()) {
940 : // set the current commanded xy pos to the target pos
941 0 : guided_pos_target_ned_m.xy() = pos_control->get_pos_desired_NED_m().xy();
942 : }
943 29526 : pos_control->input_pos_vel_accel_NE_m(guided_pos_target_ned_m.xy(), guided_vel_target_ned_ms.xy(), guided_accel_target_ned_mss.xy(), false);
944 29526 : if (!stabilizing_vel_NE()) {
945 : // set position and velocity errors to zero
946 0 : pos_control->NE_stop_vel_stabilisation();
947 29526 : } else if (!stabilizing_pos_NE()) {
948 : // set position errors to zero
949 0 : pos_control->NE_stop_pos_stabilisation();
950 : }
951 :
952 : // guided_pos_target z-axis should never be a terrain altitude
953 29526 : if (guided_is_terrain_alt) {
954 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
955 : }
956 :
957 29526 : float pz = guided_pos_target_ned_m.z;
958 29526 : pos_control->input_pos_vel_accel_D_m(pz, guided_vel_target_ned_ms.z, guided_accel_target_ned_mss.z, false);
959 29526 : guided_pos_target_ned_m.z = pz;
960 :
961 : // run position controllers
962 29526 : pos_control->NE_update_controller();
963 29526 : pos_control->D_update_controller();
964 :
965 : // call attitude controller with auto yaw
966 29526 : attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading());
967 : }
968 :
969 : // angle_control_run - runs the guided angle controller
970 : // called from guided_run
971 100766 : void ModeGuided::angle_control_run()
972 : {
973 100766 : float climb_rate_ms = 0.0f;
974 100766 : if (!guided_angle_state.use_thrust) {
975 : // constrain climb rate
976 99566 : climb_rate_ms = constrain_float(guided_angle_state.climb_rate_ms, -wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms());
977 :
978 : // get avoidance adjusted climb rate
979 99566 : climb_rate_ms = get_avoidance_adjusted_climbrate_ms(climb_rate_ms);
980 : }
981 :
982 : // check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds
983 100766 : uint32_t tnow = millis();
984 100766 : if (tnow - guided_angle_state.update_time_ms > get_timeout_ms()) {
985 65555 : guided_angle_state.attitude_quat.from_euler(Vector3f{0.0, 0.0, attitude_control->get_att_target_euler_rad().z});
986 65555 : guided_angle_state.ang_vel_body.zero();
987 65555 : climb_rate_ms = 0.0f;
988 65555 : if (guided_angle_state.use_thrust) {
989 : // initialise vertical velocity controller
990 1 : pos_control->D_init_controller();
991 1 : guided_angle_state.use_thrust = false;
992 : }
993 : }
994 :
995 : // interpret positive climb rate or thrust as triggering take-off
996 100766 : const bool positive_thrust_or_climbrate = is_positive(guided_angle_state.use_thrust ? guided_angle_state.thrust_norm : climb_rate_ms);
997 100766 : if (motors->armed() && positive_thrust_or_climbrate) {
998 18493 : copter.set_auto_armed(true);
999 : }
1000 :
1001 : // if not armed set throttle to zero and exit immediately
1002 100766 : if (!motors->armed() || !copter.ap.auto_armed || (copter.ap.land_complete && !positive_thrust_or_climbrate)) {
1003 : // do not spool down tradheli when on the ground with motor interlock enabled
1004 1440 : make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock());
1005 1440 : return;
1006 : }
1007 :
1008 : // TODO: use get_alt_hold_state_D_ms
1009 : // landed with positive desired climb rate or thrust, takeoff
1010 99326 : if (copter.ap.land_complete && positive_thrust_or_climbrate) {
1011 813 : zero_throttle_and_relax_ac();
1012 813 : motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
1013 813 : if (motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
1014 1 : set_land_complete(false);
1015 1 : pos_control->D_init_controller();
1016 : }
1017 813 : return;
1018 : }
1019 :
1020 : // set motors to full range
1021 98513 : motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
1022 :
1023 : // call attitude controller
1024 98513 : if (guided_angle_state.attitude_quat.is_zero()) {
1025 0 : attitude_control->input_rate_bf_roll_pitch_yaw_rads(guided_angle_state.ang_vel_body.x, guided_angle_state.ang_vel_body.y, guided_angle_state.ang_vel_body.z);
1026 : } else {
1027 98513 : attitude_control->input_quaternion(guided_angle_state.attitude_quat, guided_angle_state.ang_vel_body);
1028 : }
1029 :
1030 : // call position controller
1031 98513 : if (guided_angle_state.use_thrust) {
1032 386 : attitude_control->set_throttle_out(guided_angle_state.thrust_norm, true, copter.g.throttle_filt);
1033 : } else {
1034 98127 : pos_control->D_set_pos_target_from_climb_rate_ms(climb_rate_ms);
1035 98127 : pos_control->D_update_controller();
1036 : }
1037 : }
1038 :
1039 : // helper function to set yaw state and targets
1040 3328 : void ModeGuided::set_yaw_state_rad(bool use_yaw, float yaw_rad, bool use_yaw_rate, float yaw_rate_rads, bool relative_angle)
1041 : {
1042 3328 : if (use_yaw && relative_angle) {
1043 0 : auto_yaw.set_fixed_yaw_rad(yaw_rad, 0.0f, 0, relative_angle);
1044 3328 : } else if (use_yaw && use_yaw_rate) {
1045 0 : auto_yaw.set_yaw_angle_and_rate_rad(yaw_rad, yaw_rate_rads);
1046 3328 : } else if (use_yaw && !use_yaw_rate) {
1047 906 : auto_yaw.set_yaw_angle_and_rate_rad(yaw_rad, 0.0f);
1048 2422 : } else if (use_yaw_rate) {
1049 1162 : auto_yaw.set_rate_rad(yaw_rate_rads);
1050 : } else {
1051 1260 : auto_yaw.set_mode_to_default(false);
1052 : }
1053 3328 : }
1054 :
1055 : // returns true if pilot's yaw input should be used to adjust vehicle's heading
1056 3328433 : bool ModeGuided::use_pilot_yaw(void) const
1057 : {
1058 3328433 : return !option_is_enabled(Option::IgnorePilotYaw);
1059 : }
1060 :
1061 : // Guided Limit code
1062 :
1063 : // limit_clear - clear/turn off guided limits
1064 98 : void ModeGuided::limit_clear()
1065 : {
1066 98 : guided_limit.timeout_ms = 0;
1067 98 : guided_limit.alt_min_m = 0.0f;
1068 98 : guided_limit.alt_max_m = 0.0f;
1069 98 : guided_limit.horiz_max_m = 0.0f;
1070 98 : }
1071 :
1072 : // limit_set - set guided timeout and movement limits
1073 0 : void ModeGuided::limit_set(uint32_t timeout_ms, float alt_min_m, float alt_max_m, float horiz_max_m)
1074 : {
1075 0 : guided_limit.timeout_ms = timeout_ms;
1076 0 : guided_limit.alt_min_m = alt_min_m;
1077 0 : guided_limit.alt_max_m = alt_max_m;
1078 0 : guided_limit.horiz_max_m = horiz_max_m;
1079 0 : }
1080 :
1081 : // limit_init_time_and_pos - initialise guided start time and position as reference for limit checking
1082 : // only called from AUTO mode's auto_nav_guided_start function
1083 0 : void ModeGuided::limit_init_time_and_pos()
1084 : {
1085 : // initialise start time
1086 0 : guided_limit.start_time_ms = AP_HAL::millis();
1087 :
1088 : // initialise start position from current position
1089 0 : guided_limit.start_pos_ned_m = pos_control->get_pos_estimate_NED_m();
1090 0 : }
1091 :
1092 : // limit_check - returns true if guided mode has breached a limit
1093 : // used when guided is invoked from the NAV_GUIDED_ENABLE mission command
1094 0 : bool ModeGuided::limit_check()
1095 : {
1096 : // check if we have passed the timeout
1097 0 : if ((guided_limit.timeout_ms > 0) && (millis() - guided_limit.start_time_ms >= guided_limit.timeout_ms)) {
1098 0 : return true;
1099 : }
1100 :
1101 : // get current location
1102 0 : const Vector3p& curr_pos_ned_m = pos_control->get_pos_estimate_NED_m();
1103 :
1104 : // check if we have gone below min alt
1105 0 : if (!is_zero(guided_limit.alt_min_m) && (-curr_pos_ned_m.z < guided_limit.alt_min_m)) {
1106 0 : return true;
1107 : }
1108 :
1109 : // check if we have gone above max alt
1110 0 : if (!is_zero(guided_limit.alt_max_m) && (-curr_pos_ned_m.z > guided_limit.alt_max_m)) {
1111 0 : return true;
1112 : }
1113 :
1114 : // check if we have gone beyond horizontal limit
1115 0 : if (guided_limit.horiz_max_m > 0.0f) {
1116 0 : const float horiz_offset_m = get_horizontal_distance(guided_limit.start_pos_ned_m.xy(), curr_pos_ned_m.xy());
1117 0 : if (horiz_offset_m > guided_limit.horiz_max_m) {
1118 0 : return true;
1119 : }
1120 : }
1121 :
1122 : // if we got this far we must be within limits
1123 0 : return false;
1124 : }
1125 :
1126 1901 : const Vector3p &ModeGuided::get_target_pos_NED_m() const
1127 : {
1128 1901 : return guided_pos_target_ned_m;
1129 : }
1130 :
1131 1352 : const Vector3f& ModeGuided::get_target_vel_NED_ms() const
1132 : {
1133 1352 : return guided_vel_target_ned_ms;
1134 : }
1135 :
1136 1419 : const Vector3f& ModeGuided::get_target_accel_NED_mss() const
1137 : {
1138 1419 : return guided_accel_target_ned_mss;
1139 : }
1140 :
1141 74529 : float ModeGuided::wp_distance_m() const
1142 : {
1143 74529 : switch(guided_mode) {
1144 0 : case SubMode::WP:
1145 0 : return wp_nav->get_wp_distance_to_destination_m();
1146 11398 : case SubMode::Pos:
1147 11398 : return get_horizontal_distance(pos_control->get_pos_estimate_NED_m().xy().tofloat(), guided_pos_target_ned_m.xy().tofloat());
1148 470 : case SubMode::PosVelAccel:
1149 470 : return pos_control->get_pos_error_NE_m();
1150 62661 : default:
1151 62661 : return 0.0f;
1152 : }
1153 : }
1154 :
1155 74529 : float ModeGuided::wp_bearing_deg() const
1156 : {
1157 74529 : switch(guided_mode) {
1158 0 : case SubMode::WP:
1159 0 : return degrees(wp_nav->get_wp_bearing_to_destination_rad());
1160 11398 : case SubMode::Pos:
1161 11398 : return degrees(get_bearing_rad(pos_control->get_pos_estimate_NED_m().xy().tofloat(), guided_pos_target_ned_m.xy().tofloat()));
1162 470 : case SubMode::PosVelAccel:
1163 470 : return degrees(pos_control->get_bearing_to_target_rad());
1164 62661 : case SubMode::TakeOff:
1165 : case SubMode::Accel:
1166 : case SubMode::VelAccel:
1167 : case SubMode::Angle:
1168 : // these do not have bearings
1169 62661 : return 0;
1170 : }
1171 : // compiler guarantees we don't get here
1172 0 : return 0.0;
1173 : }
1174 :
1175 74772 : float ModeGuided::crosstrack_error_m() const
1176 : {
1177 74772 : switch (guided_mode) {
1178 0 : case SubMode::WP:
1179 0 : return wp_nav->crosstrack_error_m();
1180 73260 : case SubMode::Pos:
1181 : case SubMode::TakeOff:
1182 : case SubMode::Accel:
1183 : case SubMode::VelAccel:
1184 : case SubMode::PosVelAccel:
1185 73260 : return pos_control->crosstrack_error_m();
1186 1512 : case SubMode::Angle:
1187 : // no track to have a crosstrack to
1188 1512 : return 0;
1189 : }
1190 : // compiler guarantees we don't get here
1191 0 : return 0;
1192 : }
1193 :
1194 : // return guided mode timeout in milliseconds. Only used for velocity, acceleration, angle control, and angular rates
1195 1125256 : uint32_t ModeGuided::get_timeout_ms() const
1196 : {
1197 1125256 : return MAX(copter.g2.guided_timeout, 0.1) * 1000;
1198 : }
1199 :
1200 : // pause guide mode
1201 5 : bool ModeGuided::pause()
1202 : {
1203 5 : _paused = true;
1204 5 : return true;
1205 : }
1206 :
1207 : // resume guided mode
1208 5 : bool ModeGuided::resume()
1209 : {
1210 5 : _paused = false;
1211 5 : return true;
1212 : }
1213 :
1214 : #endif
|
