Line data Source code
1 : #include "Sub.h"
2 :
3 : /*
4 : * Init and run calls for guided flight mode
5 : */
6 :
7 : #define GUIDED_VEL_TIMEOUT_MS 3000 // velocity controllers time out after 3 seconds with no new updates
8 : #define GUIDED_ATTITUDE_TIMEOUT_MS 1000 // attitude controller times out after 1 second with no new updates
9 :
10 : // targets for Guided_PosVelAccel mode
11 : static Vector3p posvelaccel_pos_target_neu_cm;
12 : static Vector3f posvelaccel_vel_target_neu_cms;
13 : static Vector3f posvelaccel_accel_target_neu_cmss;
14 : static uint32_t update_time_ms;
15 :
16 : struct {
17 : uint32_t update_time_ms;
18 : float roll_cd;
19 : float pitch_cd;
20 : float yaw_cd;
21 : float climb_rate_cms;
22 : } static guided_angle_state = {0,0.0f, 0.0f, 0.0f, 0.0f};
23 :
24 : struct Guided_Limit {
25 : uint32_t timeout_ms; // timeout (in seconds) from the time that guided is invoked
26 : float alt_min_cm; // lower altitude limit in cm above home (0 = no limit)
27 : float alt_max_cm; // upper altitude limit in cm above home (0 = no limit)
28 : float horiz_max_cm; // horizontal position limit in cm 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 : Vector3f start_pos_neu_cm; // start position as a distance from home in cm. used for checking horiz_max limit
31 : } guided_limit;
32 :
33 : // guided_init - initialise guided controller
34 6 : bool ModeGuided::init(bool ignore_checks)
35 : {
36 6 : if (!sub.position_ok() && !ignore_checks) {
37 0 : return false;
38 : }
39 :
40 : // start in position control mode
41 6 : guided_pos_control_start();
42 6 : return true;
43 : }
44 :
45 : // get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter
46 : // set rtl parameter to true if this is during an RTL
47 47 : autopilot_yaw_mode ModeGuided::get_default_auto_yaw_mode(bool rtl) const
48 : {
49 47 : switch (g.wp_yaw_behavior) {
50 :
51 0 : case WP_YAW_BEHAVIOR_NONE:
52 0 : return AUTO_YAW_HOLD;
53 : break;
54 :
55 0 : case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL:
56 0 : if (rtl) {
57 0 : return AUTO_YAW_HOLD;
58 : } else {
59 0 : return AUTO_YAW_LOOK_AT_NEXT_WP;
60 : }
61 : break;
62 :
63 0 : case WP_YAW_BEHAVIOR_LOOK_AHEAD:
64 0 : return AUTO_YAW_LOOK_AHEAD;
65 : break;
66 :
67 47 : case WP_YAW_BEHAVIOR_CORRECT_XTRACK:
68 47 : return AUTO_YAW_CORRECT_XTRACK;
69 : break;
70 :
71 0 : case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP:
72 : default:
73 0 : return AUTO_YAW_LOOK_AT_NEXT_WP;
74 : break;
75 : }
76 : }
77 :
78 :
79 : // initialise guided mode's position controller
80 6 : void ModeGuided::guided_pos_control_start()
81 : {
82 : // set to position control mode
83 6 : sub.guided_mode = Guided_WP;
84 :
85 : // initialise waypoint controller
86 6 : sub.wp_nav.wp_and_spline_init_m();
87 :
88 : // initialise wpnav to stopping point at current altitude
89 : // To-Do: set to current location if disarmed?
90 : // To-Do: set to stopping point altitude?
91 6 : Vector3f stopping_point_neu_cm;
92 6 : sub.wp_nav.get_wp_stopping_point_NEU_cm(stopping_point_neu_cm);
93 :
94 : // no need to check return status because terrain data is not used
95 6 : sub.wp_nav.set_wp_destination_NEU_cm(stopping_point_neu_cm, false);
96 :
97 : // initialise yaw
98 6 : sub.yaw_rate_only = false;
99 6 : set_auto_yaw_mode(get_default_auto_yaw_mode(false));
100 6 : }
101 :
102 : // initialise guided mode's velocity controller
103 0 : void ModeGuided::guided_vel_control_start()
104 : {
105 : // set guided_mode to velocity controller
106 0 : sub.guided_mode = Guided_Velocity;
107 :
108 : // initialize vertical maximum speeds and acceleration
109 : // All limits must be positive
110 0 : position_control->D_set_max_speed_accel_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
111 0 : position_control->D_set_correction_speed_accel_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
112 :
113 : // initialise velocity controller
114 0 : position_control->D_init_controller();
115 0 : position_control->NE_init_controller();
116 :
117 : // pilot always controls yaw
118 0 : sub.yaw_rate_only = false;
119 0 : set_auto_yaw_mode(AUTO_YAW_HOLD);
120 0 : }
121 :
122 : // initialise guided mode's posvelaccel controller
123 2 : void ModeGuided::guided_posvelaccel_control_start()
124 : {
125 : // set guided_mode to posvelaccel controller
126 2 : sub.guided_mode = Guided_PosVelAccel;
127 :
128 : // initialise horizontal speed, acceleration
129 2 : position_control->NE_set_max_speed_accel_cm(sub.wp_nav.get_default_speed_NE_cms(), sub.wp_nav.get_wp_acceleration_cmss());
130 2 : position_control->NE_set_correction_speed_accel_cm(sub.wp_nav.get_default_speed_NE_cms(), sub.wp_nav.get_wp_acceleration_cmss());
131 :
132 : // set vertical speed and acceleration
133 : // All limits must be positive
134 2 : position_control->D_set_max_speed_accel_cm(sub.wp_nav.get_default_speed_down_cms(), sub.wp_nav.get_default_speed_up_cms(), sub.wp_nav.get_accel_D_cmss());
135 2 : position_control->D_set_correction_speed_accel_cm(sub.wp_nav.get_default_speed_down_cms(), sub.wp_nav.get_default_speed_up_cms(), sub.wp_nav.get_accel_D_cmss());
136 :
137 : // initialise velocity controller
138 2 : position_control->D_init_controller();
139 2 : position_control->NE_init_controller();
140 :
141 : // pilot always controls yaw
142 2 : sub.yaw_rate_only = false;
143 2 : set_auto_yaw_mode(AUTO_YAW_HOLD);
144 2 : }
145 :
146 : // initialise guided mode's angle controller
147 0 : void ModeGuided::guided_angle_control_start()
148 : {
149 : // set guided_mode to velocity controller
150 0 : sub.guided_mode = Guided_Angle;
151 :
152 : // set vertical speed and acceleration
153 : // All limits must be positive
154 0 : position_control->D_set_max_speed_accel_cm(sub.wp_nav.get_default_speed_down_cms(), sub.wp_nav.get_default_speed_up_cms(), sub.wp_nav.get_accel_D_cmss());
155 0 : position_control->D_set_correction_speed_accel_cm(sub.wp_nav.get_default_speed_down_cms(), sub.wp_nav.get_default_speed_up_cms(), sub.wp_nav.get_accel_D_cmss());
156 :
157 : // initialise velocity controller
158 0 : position_control->D_init_controller();
159 :
160 : // initialise targets
161 0 : guided_angle_state.update_time_ms = AP_HAL::millis();
162 0 : guided_angle_state.roll_cd = ahrs.roll_sensor;
163 0 : guided_angle_state.pitch_cd = ahrs.pitch_sensor;
164 0 : guided_angle_state.yaw_cd = ahrs.yaw_sensor;
165 0 : guided_angle_state.climb_rate_cms = 0.0f;
166 :
167 : // pilot always controls yaw
168 0 : sub.yaw_rate_only = false;
169 0 : set_auto_yaw_mode(AUTO_YAW_HOLD);
170 0 : }
171 :
172 : // sets guided mode's target from a Location object
173 : // returns false if destination could not be set (probably caused by missing terrain data)
174 : // or if the fence is enabled and guided waypoint is outside the fence
175 5 : bool ModeGuided::guided_set_destination(const Location& dest_loc)
176 : {
177 : #if AP_FENCE_ENABLED
178 : // reject destination outside the fence.
179 : // 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
180 5 : if (!sub.fence.check_location_within_fence(dest_loc)) {
181 0 : LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
182 : // failure is propagated to GCS with NAK
183 0 : return false;
184 : }
185 : #endif
186 :
187 : // ensure we are in position control mode
188 5 : if (sub.guided_mode != Guided_WP) {
189 0 : guided_pos_control_start();
190 : }
191 :
192 5 : if (!sub.wp_nav.set_wp_destination_loc(dest_loc)) {
193 : // failure to set destination can only be because of missing terrain data
194 0 : LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION);
195 : // failure is propagated to GCS with NAK
196 0 : return false;
197 : }
198 :
199 : #if HAL_LOGGING_ENABLED
200 : // log target
201 5 : sub.Log_Write_GuidedTarget(sub.guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt), Vector3f(), Vector3f());
202 : #endif
203 :
204 5 : return true;
205 : }
206 :
207 : // guided_set_destination - sets guided mode's target destination and target heading
208 : // Returns true if the fence is enabled and guided waypoint is within the fence
209 : // else return false if the waypoint is outside the fence
210 0 : bool ModeGuided::guided_set_destination(const Vector3f& destination_neu_cm, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
211 : {
212 : #if AP_FENCE_ENABLED
213 : // reject destination if outside the fence
214 0 : const Location dest_loc(destination_neu_cm, Location::AltFrame::ABOVE_ORIGIN);
215 0 : if (!sub.fence.check_location_within_fence(dest_loc)) {
216 0 : LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
217 : // failure is propagated to GCS with NAK
218 0 : return false;
219 : }
220 : #endif
221 :
222 : // ensure we are in position control mode
223 0 : if (sub.guided_mode != Guided_WP) {
224 0 : guided_pos_control_start();
225 : }
226 :
227 : // set yaw state
228 0 : guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
229 :
230 0 : update_time_ms = AP_HAL::millis();
231 :
232 : // no need to check return status because terrain data is not used
233 0 : sub.wp_nav.set_wp_destination_NEU_cm(destination_neu_cm, false);
234 :
235 : #if HAL_LOGGING_ENABLED
236 : // log target
237 0 : sub.Log_Write_GuidedTarget(sub.guided_mode, destination_neu_cm, Vector3f(), Vector3f());
238 : #endif
239 :
240 0 : return true;
241 : }
242 :
243 : // guided_set_velocity - sets guided mode's target velocity
244 0 : void ModeGuided::guided_set_velocity(const Vector3f& velocity_neu_cms)
245 : {
246 : // check we are in velocity control mode
247 0 : if (sub.guided_mode != Guided_Velocity) {
248 0 : guided_vel_control_start();
249 : }
250 :
251 0 : update_time_ms = AP_HAL::millis();
252 :
253 : // set position controller velocity target
254 0 : position_control->set_vel_desired_NEU_cms(velocity_neu_cms);
255 0 : }
256 :
257 : // guided_set_velocity - sets guided mode's target velocity
258 0 : void ModeGuided::guided_set_velocity(const Vector3f& velocity_neu_cms, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
259 : {
260 : // check we are in velocity control mode
261 0 : if (sub.guided_mode != Guided_Velocity) {
262 0 : guided_vel_control_start();
263 : }
264 :
265 : // set yaw state
266 0 : guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
267 :
268 0 : update_time_ms = AP_HAL::millis();
269 :
270 : // set position controller velocity target
271 0 : position_control->set_vel_desired_NEU_cms(velocity_neu_cms);
272 :
273 0 : }
274 :
275 : // expose the current Guided_PosVelAccel position target so the GCS layer can
276 : // report POSITION_TARGET_GLOBAL_INT in that submode.
277 1865 : bool ModeGuided::get_posvelaccel_target_NEU_cm(Vector3f &pos) const
278 : {
279 1865 : if (sub.guided_mode != Guided_PosVelAccel) {
280 0 : return false;
281 : }
282 1865 : pos = posvelaccel_pos_target_neu_cm.tofloat();
283 1865 : return true;
284 : }
285 :
286 : // set guided mode posvelaccel target
287 0 : bool ModeGuided::guided_set_posvelaccel(const Vector3f& destination_neu_cm, const Vector3f& velocity_neu_cms, const Vector3f& accel_neu_cmss)
288 : {
289 : #if AP_FENCE_ENABLED
290 : // reject destination if outside the fence
291 0 : const Location dest_loc(destination_neu_cm, Location::AltFrame::ABOVE_ORIGIN);
292 0 : if (!sub.fence.check_location_within_fence(dest_loc)) {
293 0 : LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
294 : // failure is propagated to GCS with NAK
295 0 : return false;
296 : }
297 : #endif
298 :
299 : // check we are in posvelaccel control mode
300 0 : if (sub.guided_mode != Guided_PosVelAccel) {
301 0 : guided_posvelaccel_control_start();
302 : }
303 :
304 0 : update_time_ms = AP_HAL::millis();
305 0 : posvelaccel_pos_target_neu_cm = destination_neu_cm.topostype();
306 0 : posvelaccel_vel_target_neu_cms = velocity_neu_cms;
307 0 : posvelaccel_accel_target_neu_cmss = accel_neu_cmss;
308 :
309 0 : position_control->input_pos_vel_accel_NE_cm(posvelaccel_pos_target_neu_cm.xy(), posvelaccel_vel_target_neu_cms.xy(), posvelaccel_accel_target_neu_cmss.xy());
310 0 : float dz = posvelaccel_pos_target_neu_cm.z;
311 0 : position_control->input_pos_vel_accel_U_cm(dz, posvelaccel_vel_target_neu_cms.z, posvelaccel_accel_target_neu_cmss.z);
312 0 : posvelaccel_pos_target_neu_cm.z = dz;
313 :
314 : #if HAL_LOGGING_ENABLED
315 : // log target
316 0 : sub.Log_Write_GuidedTarget(sub.guided_mode, destination_neu_cm, velocity_neu_cms, accel_neu_cmss);
317 : #endif
318 :
319 0 : return true;
320 : }
321 :
322 : // set guided mode posvelaccel target
323 1228 : bool ModeGuided::guided_set_posvelaccel(const Vector3f& destination_neu_cm, const Vector3f& velocity_neu_cms, const Vector3f& accel_neu_cmss, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
324 : {
325 : #if AP_FENCE_ENABLED
326 : // reject destination if outside the fence
327 1228 : const Location dest_loc(destination_neu_cm, Location::AltFrame::ABOVE_ORIGIN);
328 1228 : if (!sub.fence.check_location_within_fence(dest_loc)) {
329 0 : LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::DEST_OUTSIDE_FENCE);
330 : // failure is propagated to GCS with NAK
331 0 : return false;
332 : }
333 : #endif
334 :
335 : // check we are in posvelaccel control mode
336 1228 : if (sub.guided_mode != Guided_PosVelAccel) {
337 2 : guided_posvelaccel_control_start();
338 : }
339 :
340 : // set yaw state
341 1228 : guided_set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
342 :
343 1228 : update_time_ms = AP_HAL::millis();
344 :
345 1228 : posvelaccel_pos_target_neu_cm = destination_neu_cm.topostype();
346 1228 : posvelaccel_vel_target_neu_cms = velocity_neu_cms;
347 1228 : posvelaccel_accel_target_neu_cmss = accel_neu_cmss;
348 :
349 1228 : position_control->input_pos_vel_accel_NE_cm(posvelaccel_pos_target_neu_cm.xy(), posvelaccel_vel_target_neu_cms.xy(), posvelaccel_accel_target_neu_cmss.xy());
350 1228 : float dz = posvelaccel_pos_target_neu_cm.z;
351 1228 : position_control->input_pos_vel_accel_U_cm(dz, posvelaccel_vel_target_neu_cms.z, posvelaccel_accel_target_neu_cmss.z);
352 1228 : posvelaccel_pos_target_neu_cm.z = dz;
353 :
354 : #if HAL_LOGGING_ENABLED
355 : // log target
356 1228 : sub.Log_Write_GuidedTarget(sub.guided_mode, destination_neu_cm, velocity_neu_cms, accel_neu_cmss);
357 : #endif
358 :
359 1228 : return true;
360 : }
361 :
362 : // set guided mode angle target
363 0 : void ModeGuided::guided_set_angle(const Quaternion &q, float climb_rate_cms)
364 : {
365 : // check we are in angle control mode
366 0 : if (sub.guided_mode != Guided_Angle) {
367 0 : guided_angle_control_start();
368 : }
369 :
370 : // convert quaternion to euler angles
371 0 : q.to_euler(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd);
372 0 : guided_angle_state.roll_cd = degrees(guided_angle_state.roll_cd) * 100.0f;
373 0 : guided_angle_state.pitch_cd = degrees(guided_angle_state.pitch_cd) * 100.0f;
374 0 : guided_angle_state.yaw_cd = wrap_180_cd(degrees(guided_angle_state.yaw_cd) * 100.0f);
375 :
376 0 : guided_angle_state.climb_rate_cms = climb_rate_cms;
377 0 : guided_angle_state.update_time_ms = AP_HAL::millis();
378 0 : }
379 :
380 : // helper function to set yaw state and targets
381 1228 : void ModeGuided::guided_set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle)
382 : {
383 1228 : float current_yaw = wrap_2PI(AP::ahrs().get_yaw_rad());
384 : float euler_yaw_angle;
385 : float yaw_error;
386 :
387 1228 : euler_yaw_angle = wrap_2PI((yaw_cd * 0.01f));
388 1228 : yaw_error = wrap_PI(euler_yaw_angle - current_yaw);
389 :
390 1228 : int direction = 0;
391 1228 : if (yaw_error < 0){
392 0 : direction = -1;
393 : } else {
394 1228 : direction = 1;
395 : }
396 :
397 : /*
398 : case 1: target yaw only
399 : case 2: target yaw and yaw rate
400 : case 3: target yaw rate only
401 : case 4: hold current yaw
402 : */
403 1228 : if (use_yaw && !use_yaw_rate) {
404 0 : sub.yaw_rate_only = false;
405 0 : sub.mode_auto.set_auto_yaw_look_at_heading(yaw_cd * 0.01f, 0.0f, direction, relative_angle);
406 1228 : } else if (use_yaw && use_yaw_rate) {
407 0 : sub.yaw_rate_only = false;
408 0 : sub.mode_auto.set_auto_yaw_look_at_heading(yaw_cd * 0.01f, yaw_rate_cds * 0.01f, direction, relative_angle);
409 1228 : } else if (!use_yaw && use_yaw_rate) {
410 0 : sub.yaw_rate_only = true;
411 0 : sub.mode_auto.set_yaw_rate(yaw_rate_cds * 0.01f);
412 : } else{
413 1228 : sub.yaw_rate_only = false;
414 1228 : set_auto_yaw_mode(AUTO_YAW_HOLD);
415 : }
416 1228 : }
417 :
418 : // guided_run - runs the guided controller
419 : // should be called at 100hz or more
420 171173 : void ModeGuided::run()
421 : {
422 : // call the correct auto controller
423 171173 : switch (sub.guided_mode) {
424 :
425 96369 : case Guided_WP:
426 : // run position controller
427 96369 : guided_pos_control_run();
428 96369 : break;
429 :
430 0 : case Guided_Velocity:
431 : // run velocity controller
432 0 : guided_vel_control_run();
433 0 : break;
434 :
435 74804 : case Guided_PosVelAccel:
436 : // run position-velocity-acceleration controller
437 74804 : guided_posvelaccel_control_run();
438 74804 : break;
439 :
440 0 : case Guided_Angle:
441 : // run angle controller
442 0 : guided_angle_control_run();
443 0 : break;
444 : }
445 171173 : }
446 :
447 : // guided_pos_control_run - runs the guided position controller
448 : // called from guided_run
449 96369 : void ModeGuided::guided_pos_control_run()
450 : {
451 : // if motors not enabled set throttle to zero and exit immediately
452 96369 : if (!motors.armed()) {
453 4876 : motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
454 : // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
455 4876 : attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt);
456 4876 : attitude_control->relax_attitude_controllers();
457 4876 : sub.wp_nav.wp_and_spline_init_m();
458 4876 : return;
459 : }
460 :
461 : // process pilot's yaw input
462 91493 : float target_yaw_rate = 0;
463 91493 : if (!sub.failsafe.pilot_input) {
464 : // get pilot's desired yaw rate
465 91493 : target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
466 91493 : if (!is_zero(target_yaw_rate)) {
467 0 : set_auto_yaw_mode(AUTO_YAW_HOLD);
468 : } else{
469 91493 : if (sub.yaw_rate_only){
470 0 : set_auto_yaw_mode(AUTO_YAW_RATE);
471 : } else{
472 91493 : set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING);
473 : }
474 : }
475 : }
476 :
477 : // set motors to full range
478 91493 : motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
479 :
480 : // run waypoint controller
481 91493 : sub.failsafe_terrain_set_status(sub.wp_nav.update_wpnav());
482 :
483 : float lateral_out, forward_out;
484 91493 : sub.translate_wpnav_rp(lateral_out, forward_out);
485 :
486 : // Send to forward/lateral outputs
487 91493 : motors.set_lateral(lateral_out);
488 91493 : motors.set_forward(forward_out);
489 :
490 : // WP_Nav has set the vertical position control targets
491 : // run the vertical position controller and set output throttle
492 91493 : position_control->D_update_controller();
493 :
494 : // call attitude controller
495 91493 : if (sub.auto_yaw_mode == AUTO_YAW_HOLD) {
496 : // roll & pitch & yaw rate from pilot
497 0 : attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate);
498 91493 : } else if (sub.auto_yaw_mode == AUTO_YAW_LOOK_AT_HEADING) {
499 : // roll, pitch from pilot, yaw & yaw_rate from auto_control
500 91493 : target_yaw_rate = sub.yaw_look_at_heading_slew * 100.0;
501 91493 : attitude_control->input_euler_angle_roll_pitch_slew_yaw_cd(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), target_yaw_rate);
502 0 : } else if (sub.auto_yaw_mode == AUTO_YAW_RATE) {
503 : // roll, pitch from pilot, yaw_rate from auto_control
504 0 : target_yaw_rate = sub.yaw_look_at_heading_slew * 100.0;
505 0 : attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate);
506 : } else {
507 : // roll, pitch from pilot, yaw heading from auto_heading()
508 0 : attitude_control->input_euler_angle_roll_pitch_yaw_cd(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true);
509 : }
510 : }
511 :
512 : // guided_vel_control_run - runs the guided velocity controller
513 : // called from guided_run
514 0 : void ModeGuided::guided_vel_control_run()
515 : {
516 : // ifmotors not enabled set throttle to zero and exit immediately
517 0 : if (!motors.armed()) {
518 0 : motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
519 : // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
520 0 : attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt);
521 0 : attitude_control->relax_attitude_controllers();
522 : // initialise velocity controller
523 0 : position_control->D_init_controller();
524 0 : position_control->NE_init_controller();
525 0 : return;
526 : }
527 :
528 : // process pilot's yaw input
529 0 : float target_yaw_rate = 0;
530 0 : if (!sub.failsafe.pilot_input) {
531 : // get pilot's desired yaw rate
532 0 : target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
533 0 : if (!is_zero(target_yaw_rate)) {
534 0 : set_auto_yaw_mode(AUTO_YAW_HOLD);
535 : } else{
536 0 : if (sub.yaw_rate_only){
537 0 : set_auto_yaw_mode(AUTO_YAW_RATE);
538 : } else{
539 0 : set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING);
540 : }
541 : }
542 : }
543 :
544 : // set motors to full range
545 0 : motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
546 :
547 : // set velocity to zero if no updates received for 3 seconds
548 0 : uint32_t tnow = AP_HAL::millis();
549 0 : if (tnow - update_time_ms > GUIDED_VEL_TIMEOUT_MS && !position_control->get_vel_desired_NEU_cms().is_zero()) {
550 0 : position_control->set_vel_desired_NEU_cms(Vector3f(0,0,0));
551 : }
552 :
553 0 : position_control->NE_stop_pos_stabilisation();
554 : // call velocity controller which includes z axis controller
555 0 : position_control->NE_update_controller();
556 :
557 0 : position_control->D_set_pos_target_from_climb_rate_cms(position_control->get_vel_desired_NEU_cms().z);
558 0 : position_control->D_update_controller();
559 :
560 : float lateral_out, forward_out;
561 0 : sub.translate_pos_control_rp(lateral_out, forward_out);
562 :
563 : // Send to forward/lateral outputs
564 0 : motors.set_lateral(lateral_out);
565 0 : motors.set_forward(forward_out);
566 :
567 : // call attitude controller
568 0 : if (sub.auto_yaw_mode == AUTO_YAW_HOLD) {
569 : // roll & pitch & yaw rate from pilot
570 0 : attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate);
571 0 : } else if (sub.auto_yaw_mode == AUTO_YAW_LOOK_AT_HEADING) {
572 : // roll, pitch from pilot, yaw & yaw_rate from auto_control
573 0 : target_yaw_rate = sub.yaw_look_at_heading_slew * 100.0;
574 0 : attitude_control->input_euler_angle_roll_pitch_slew_yaw_cd(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), target_yaw_rate);
575 0 : } else if (sub.auto_yaw_mode == AUTO_YAW_RATE) {
576 : // roll, pitch from pilot, yaw_rate from auto_control
577 0 : target_yaw_rate = sub.yaw_look_at_heading_slew * 100.0;
578 0 : attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate);
579 : } else {
580 : // roll, pitch from pilot, yaw heading from auto_heading()
581 0 : attitude_control->input_euler_angle_roll_pitch_yaw_cd(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true);
582 : }
583 : }
584 :
585 : // guided_posvelaccel_control_run - runs the guided posvelaccel controller
586 : // called from guided_run
587 74804 : void ModeGuided::guided_posvelaccel_control_run()
588 : {
589 : // if motors not enabled set throttle to zero and exit immediately
590 74804 : if (!motors.armed()) {
591 2566 : motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
592 : // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
593 2566 : attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt);
594 2566 : attitude_control->relax_attitude_controllers();
595 : // initialise velocity controller
596 2566 : position_control->D_init_controller();
597 2566 : position_control->NE_init_controller();
598 2566 : return;
599 : }
600 :
601 : // process pilot's yaw input
602 72238 : float target_yaw_rate = 0;
603 :
604 72238 : if (!sub.failsafe.pilot_input) {
605 : // get pilot's desired yaw rate
606 72238 : target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
607 72238 : if (!is_zero(target_yaw_rate)) {
608 0 : set_auto_yaw_mode(AUTO_YAW_HOLD);
609 : } else{
610 72238 : if (sub.yaw_rate_only){
611 0 : set_auto_yaw_mode(AUTO_YAW_RATE);
612 : } else{
613 72238 : set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING);
614 : }
615 : }
616 : }
617 :
618 : // set motors to full range
619 72238 : motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
620 :
621 : // set velocity to zero if no updates received for 3 seconds
622 72238 : uint32_t tnow = AP_HAL::millis();
623 72238 : if (tnow - update_time_ms > GUIDED_VEL_TIMEOUT_MS && !posvelaccel_vel_target_neu_cms.is_zero()) {
624 0 : posvelaccel_vel_target_neu_cms.zero();
625 0 : posvelaccel_accel_target_neu_cmss.zero();
626 : }
627 :
628 : // send position and velocity targets to position controller
629 72238 : position_control->input_pos_vel_accel_NE_cm(posvelaccel_pos_target_neu_cm.xy(), posvelaccel_vel_target_neu_cms.xy(), posvelaccel_accel_target_neu_cmss.xy());
630 72238 : float pz = posvelaccel_pos_target_neu_cm.z;
631 72238 : position_control->input_pos_vel_accel_U_cm(pz, posvelaccel_vel_target_neu_cms.z, posvelaccel_accel_target_neu_cmss.z);
632 72238 : posvelaccel_pos_target_neu_cm.z = pz;
633 :
634 : // run position controller
635 72238 : position_control->NE_update_controller();
636 72238 : position_control->D_update_controller();
637 :
638 : float lateral_out, forward_out;
639 72238 : sub.translate_pos_control_rp(lateral_out, forward_out);
640 :
641 : // Send to forward/lateral outputs
642 72238 : motors.set_lateral(lateral_out);
643 72238 : motors.set_forward(forward_out);
644 :
645 : // call attitude controller
646 72238 : if (sub.auto_yaw_mode == AUTO_YAW_HOLD) {
647 : // roll & pitch & yaw rate from pilot
648 0 : attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate);
649 72238 : } else if (sub.auto_yaw_mode == AUTO_YAW_LOOK_AT_HEADING) {
650 : // roll, pitch from pilot, yaw & yaw_rate from auto_control
651 72238 : target_yaw_rate = sub.yaw_look_at_heading_slew * 100.0;
652 72238 : attitude_control->input_euler_angle_roll_pitch_slew_yaw_cd(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), target_yaw_rate);
653 0 : } else if (sub.auto_yaw_mode == AUTO_YAW_RATE) {
654 : // roll, pitch from pilot, and yaw_rate from auto_control
655 0 : target_yaw_rate = sub.yaw_look_at_heading_slew * 100.0;
656 0 : attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate);
657 : } else {
658 : // roll, pitch from pilot, yaw heading from auto_heading()
659 0 : attitude_control->input_euler_angle_roll_pitch_yaw_cd(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true);
660 : }
661 : }
662 :
663 : // guided_angle_control_run - runs the guided angle controller
664 : // called from guided_run
665 0 : void ModeGuided::guided_angle_control_run()
666 : {
667 : // if motors not enabled set throttle to zero and exit immediately
668 0 : if (!motors.armed()) {
669 0 : motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
670 : // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
671 0 : attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt);
672 0 : attitude_control->relax_attitude_controllers();
673 : // initialise velocity controller
674 0 : position_control->D_init_controller();
675 0 : return;
676 : }
677 :
678 : // constrain desired lean angles
679 0 : float roll_in = guided_angle_state.roll_cd;
680 0 : float pitch_in = guided_angle_state.pitch_cd;
681 0 : float total_in = norm(roll_in, pitch_in);
682 0 : float angle_max = MIN(attitude_control->get_althold_lean_angle_max_cd(), attitude_control->lean_angle_max_cd());
683 0 : if (total_in > angle_max) {
684 0 : float ratio = angle_max / total_in;
685 0 : roll_in *= ratio;
686 0 : pitch_in *= ratio;
687 : }
688 :
689 : // wrap yaw request
690 0 : float yaw_in = wrap_180_cd(guided_angle_state.yaw_cd);
691 :
692 : // constrain climb rate
693 0 : float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -sub.wp_nav.get_default_speed_down_cms(), sub.wp_nav.get_default_speed_up_cms());
694 :
695 : // check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds
696 0 : uint32_t tnow = AP_HAL::millis();
697 0 : if (tnow - guided_angle_state.update_time_ms > GUIDED_ATTITUDE_TIMEOUT_MS) {
698 0 : roll_in = 0.0f;
699 0 : pitch_in = 0.0f;
700 0 : climb_rate_cms = 0.0f;
701 : }
702 :
703 : // set motors to full range
704 0 : motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
705 :
706 : // call attitude controller
707 0 : attitude_control->input_euler_angle_roll_pitch_yaw_cd(roll_in, pitch_in, yaw_in, true);
708 :
709 : // call position controller
710 0 : position_control->D_set_pos_target_from_climb_rate_cms(climb_rate_cms);
711 0 : position_control->D_update_controller();
712 : }
713 :
714 : // Guided Limit code
715 :
716 : // guided_limit_clear - clear/turn off guided limits
717 9 : void ModeGuided::guided_limit_clear()
718 : {
719 9 : guided_limit.timeout_ms = 0;
720 9 : guided_limit.alt_min_cm = 0.0f;
721 9 : guided_limit.alt_max_cm = 0.0f;
722 9 : guided_limit.horiz_max_cm = 0.0f;
723 9 : }
724 :
725 :
726 : // set_auto_yaw_mode - sets the yaw mode for auto
727 165026 : void ModeGuided::set_auto_yaw_mode(autopilot_yaw_mode yaw_mode)
728 : {
729 : // return immediately if no change
730 165026 : if (sub.auto_yaw_mode == yaw_mode) {
731 162600 : return;
732 : }
733 2426 : sub.auto_yaw_mode = yaw_mode;
734 :
735 : // perform initialisation
736 2426 : switch (sub.auto_yaw_mode) {
737 :
738 1207 : case AUTO_YAW_HOLD:
739 : // pilot controls the heading
740 1207 : break;
741 :
742 0 : case AUTO_YAW_LOOK_AT_NEXT_WP:
743 : // wpnav will initialise heading when wpnav's set_destination method is called
744 0 : break;
745 :
746 0 : case AUTO_YAW_ROI:
747 : // point towards a location held in yaw_look_at_WP
748 0 : sub.yaw_look_at_WP_bearing = ahrs.yaw_sensor;
749 0 : break;
750 :
751 1206 : case AUTO_YAW_LOOK_AT_HEADING:
752 : // keep heading pointing in the direction held in yaw_look_at_heading
753 : // caller should set the yaw_look_at_heading
754 1206 : break;
755 :
756 0 : case AUTO_YAW_LOOK_AHEAD:
757 : // Commanded Yaw to automatically look ahead.
758 0 : sub.yaw_look_ahead_bearing = ahrs.yaw_sensor;
759 0 : break;
760 :
761 0 : case AUTO_YAW_RESETTOARMEDYAW:
762 : // initial_armed_bearing will be set during arming so no init required
763 0 : break;
764 :
765 0 : case AUTO_YAW_RATE:
766 : // set target yaw rate to yaw_look_at_heading_slew
767 0 : break;
768 : }
769 : }
770 :
771 : // get_auto_heading - returns target heading depending upon auto_yaw_mode
772 : // 100hz update rate
773 562548 : float ModeGuided::get_auto_heading()
774 : {
775 562548 : switch (sub.auto_yaw_mode) {
776 :
777 0 : case AUTO_YAW_ROI:
778 : // point towards a location held in roi_WP_neu_cm
779 0 : return sub.get_roi_yaw();
780 : break;
781 :
782 163731 : case AUTO_YAW_LOOK_AT_HEADING:
783 : // keep heading pointing in the direction held in yaw_look_at_heading with no pilot input allowed
784 163731 : return sub.yaw_look_at_heading;
785 : break;
786 :
787 0 : case AUTO_YAW_LOOK_AHEAD:
788 : // Commanded Yaw to automatically look ahead.
789 0 : return sub.get_look_ahead_yaw();
790 : break;
791 :
792 0 : case AUTO_YAW_RESETTOARMEDYAW:
793 : // changes yaw to be same as when quad was armed
794 0 : return sub.initial_armed_bearing;
795 : break;
796 :
797 398817 : case AUTO_YAW_CORRECT_XTRACK: {
798 : // TODO return current yaw if not in appropriate mode
799 : // Bearing of current track (centidegrees)
800 398817 : float track_bearing = get_bearing_cd(sub.wp_nav.get_wp_origin_NEU_cm().xy(), sub.wp_nav.get_wp_destination_NEU_cm().xy());
801 :
802 : // Bearing from current position towards intermediate position target (centidegrees)
803 398817 : const Vector2f target_vel_ne_cms = position_control->get_vel_target_NEU_cms().xy();
804 398817 : float angle_error = 0.0f;
805 398817 : if (target_vel_ne_cms.length() >= position_control->NE_get_max_speed_cms() * 0.1f) {
806 393198 : const float desired_angle_cd = degrees(target_vel_ne_cms.angle()) * 100.0f;
807 393198 : angle_error = wrap_180_cd(desired_angle_cd - track_bearing);
808 : }
809 398817 : float angle_limited = constrain_float(angle_error, -g.xtrack_angle_limit * 100.0f, g.xtrack_angle_limit * 100.0f);
810 398817 : return wrap_360_cd(track_bearing + angle_limited);
811 : }
812 : break;
813 :
814 0 : case AUTO_YAW_LOOK_AT_NEXT_WP:
815 : default:
816 : // point towards next waypoint.
817 : // we don't use wp_bearing because we don't want the vehicle to turn too much during flight
818 0 : return sub.wp_nav.get_yaw();
819 : break;
820 : }
821 : }
822 : // guided_limit_set - set guided timeout and movement limits
823 0 : void ModeGuided::guided_limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm)
824 : {
825 0 : guided_limit.timeout_ms = timeout_ms;
826 0 : guided_limit.alt_min_cm = alt_min_cm;
827 0 : guided_limit.alt_max_cm = alt_max_cm;
828 0 : guided_limit.horiz_max_cm = horiz_max_cm;
829 0 : }
830 :
831 : // guided_limit_init_time_and_pos - initialise guided start time and position as reference for limit checking
832 : // only called from AUTO mode's auto_nav_guided_start function
833 0 : void ModeGuided::guided_limit_init_time_and_pos()
834 : {
835 : // initialise start time
836 0 : guided_limit.start_time_ms = AP_HAL::millis();
837 :
838 : // initialise start position from current position
839 0 : Vector3f pos_cm = (position_control->get_pos_estimate_NED_m() * 100.0f).tofloat();
840 0 : pos_cm.z = -pos_cm.z;
841 0 : guided_limit.start_pos_neu_cm = pos_cm;
842 0 : }
843 :
844 : // guided_limit_check - returns true if guided mode has breached a limit
845 : // used when guided is invoked from the NAV_GUIDED_ENABLE mission command
846 0 : bool ModeGuided::guided_limit_check()
847 : {
848 : // check if we have passed the timeout
849 0 : if ((guided_limit.timeout_ms > 0) && (AP_HAL::millis() - guided_limit.start_time_ms >= guided_limit.timeout_ms)) {
850 0 : return true;
851 : }
852 :
853 : // get current location
854 0 : Vector3f curr_pos_neu_cm = (position_control->get_pos_estimate_NED_m() * 100.0f).tofloat();
855 0 : curr_pos_neu_cm.z = -curr_pos_neu_cm.z;
856 :
857 : // check if we have gone below min alt
858 0 : if (!is_zero(guided_limit.alt_min_cm) && (curr_pos_neu_cm.z < guided_limit.alt_min_cm)) {
859 0 : return true;
860 : }
861 :
862 : // check if we have gone above max alt
863 0 : if (!is_zero(guided_limit.alt_max_cm) && (curr_pos_neu_cm.z > guided_limit.alt_max_cm)) {
864 0 : return true;
865 : }
866 :
867 : // check if we have gone beyond horizontal limit
868 0 : if (guided_limit.horiz_max_cm > 0.0f) {
869 0 : const float horiz_move_cm = get_horizontal_distance(guided_limit.start_pos_neu_cm.xy(), curr_pos_neu_cm.xy());
870 0 : if (horiz_move_cm > guided_limit.horiz_max_cm) {
871 0 : return true;
872 : }
873 : }
874 :
875 : // if we got this far we must be within limits
876 0 : return false;
877 : }
|
