Line data Source code
1 : #include "Sub.h"
2 :
3 : /*
4 : * control_auto.cpp
5 : * Contains the mission, waypoint navigation and NAV_CMD item implementation
6 : *
7 : * While in the auto flight mode, navigation or do/now commands can be run.
8 : * Code in this file implements the navigation commands
9 : */
10 9 : bool ModeAuto::init(bool ignore_checks) {
11 9 : if (!sub.position_ok() || !sub.mission.present()) {
12 0 : return false;
13 : }
14 :
15 9 : sub.auto_mode = Auto_Loiter;
16 :
17 : // stop ROI from carrying over from previous runs of the mission
18 : // To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check
19 9 : if (sub.auto_yaw_mode == AUTO_YAW_ROI) {
20 0 : set_auto_yaw_mode(AUTO_YAW_HOLD);
21 : }
22 :
23 : // initialise waypoint controller
24 9 : sub.wp_nav.wp_and_spline_init_m();
25 :
26 : // clear guided limits
27 9 : guided_limit_clear();
28 :
29 : // start/resume the mission (based on MIS_RESTART parameter)
30 9 : sub.mission.start_or_resume();
31 9 : return true;
32 : }
33 :
34 : // auto_run - runs the appropriate auto controller
35 : // according to the current auto_mode
36 543247 : void ModeAuto::run()
37 : {
38 543247 : sub.mission.update();
39 :
40 : // call the correct auto controller
41 543247 : switch (sub.auto_mode) {
42 :
43 398835 : case Auto_WP:
44 : case Auto_CircleMoveToEdge:
45 398835 : auto_wp_run();
46 398835 : break;
47 :
48 127276 : case Auto_Circle:
49 127276 : auto_circle_run();
50 127276 : break;
51 :
52 0 : case Auto_NavGuided:
53 : #if NAV_GUIDED
54 0 : auto_nav_guided_run();
55 : #endif
56 0 : break;
57 :
58 9474 : case Auto_Loiter:
59 9474 : auto_loiter_run();
60 9474 : break;
61 :
62 7662 : case Auto_TerrainRecover:
63 7662 : auto_terrain_recover_run();
64 7662 : break;
65 : }
66 543247 : }
67 :
68 : // auto_wp_start - initialises waypoint controller to implement flying to a particular destination
69 0 : void ModeAuto::auto_wp_start(const Vector3f& destination_neu_cm)
70 : {
71 0 : sub.auto_mode = Auto_WP;
72 :
73 : // initialise wpnav (no need to check return status because terrain data is not used)
74 0 : sub.wp_nav.set_wp_destination_NEU_cm(destination_neu_cm, false);
75 :
76 : // initialise yaw
77 : // To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI
78 0 : if (sub.auto_yaw_mode != AUTO_YAW_ROI) {
79 0 : set_auto_yaw_mode(get_default_auto_yaw_mode(false));
80 : }
81 0 : }
82 :
83 : // auto_wp_start - initialises waypoint controller to implement flying to a particular destination
84 39 : void ModeAuto::auto_wp_start(const Location& dest_loc)
85 : {
86 39 : sub.auto_mode = Auto_WP;
87 :
88 : // send target to waypoint controller
89 39 : if (!sub.wp_nav.set_wp_destination_loc(dest_loc)) {
90 : // failure to set destination can only be because of missing terrain data
91 0 : gcs().send_text(MAV_SEVERITY_WARNING, "Terrain data (rangefinder) not available");
92 0 : sub.failsafe_terrain_on_event();
93 0 : return;
94 : }
95 :
96 : // initialise yaw
97 : // To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI
98 39 : if (sub.auto_yaw_mode != AUTO_YAW_ROI) {
99 39 : set_auto_yaw_mode(get_default_auto_yaw_mode(false));
100 : }
101 : }
102 :
103 : // auto_wp_run - runs the auto waypoint controller
104 : // called by auto_run at 100hz or more
105 398835 : void ModeAuto::auto_wp_run()
106 : {
107 : // if not armed set throttle to zero and exit immediately
108 398835 : if (!motors.armed()) {
109 : // To-Do: reset waypoint origin to current location because vehicle is probably on the ground so we don't want it lurching left or right on take-off
110 : // (of course it would be better if people just used take-off)
111 : // call attitude controller
112 : // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
113 18 : motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
114 18 : attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt);
115 18 : attitude_control->relax_attitude_controllers();
116 18 : sub.wp_nav.wp_and_spline_init_m(); // Reset xy target
117 18 : return;
118 : }
119 :
120 : // process pilot's yaw input
121 398817 : float target_yaw_rate = 0;
122 398817 : if (!sub.failsafe.pilot_input) {
123 : // get pilot's desired yaw rate
124 398817 : target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
125 398817 : if (!is_zero(target_yaw_rate)) {
126 0 : set_auto_yaw_mode(AUTO_YAW_HOLD);
127 : }
128 : }
129 :
130 : // set motors to full range
131 398817 : motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
132 :
133 : // run waypoint controller
134 : // TODO logic for terrain tracking target going below fence limit
135 : // TODO implement waypoint radius individually for each waypoint based on cmd.p2
136 : // TODO fix auto yaw heading to switch to something appropriate when mission complete and switches to loiter
137 398817 : sub.failsafe_terrain_set_status(sub.wp_nav.update_wpnav());
138 :
139 : ///////////////////////
140 : // update xy outputs //
141 :
142 : float lateral_out, forward_out;
143 398817 : sub.translate_wpnav_rp(lateral_out, forward_out);
144 :
145 : // Send to forward/lateral outputs
146 398817 : motors.set_lateral(lateral_out);
147 398817 : motors.set_forward(forward_out);
148 :
149 : // WP_Nav has set the vertical position control targets
150 : // run the vertical position controller and set output throttle
151 398817 : position_control->D_update_controller();
152 :
153 : ////////////////////////////
154 : // update attitude output //
155 :
156 : // get pilot desired lean angles
157 : float target_roll, target_pitch;
158 398817 : sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control->lean_angle_max_cd());
159 :
160 : // call attitude controller
161 398817 : if (sub.auto_yaw_mode == AUTO_YAW_HOLD) {
162 : // roll & pitch & yaw rate from pilot
163 0 : attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(target_roll, target_pitch, target_yaw_rate);
164 : } else {
165 : // roll, pitch from pilot, yaw heading from auto_heading()
166 398817 : attitude_control->input_euler_angle_roll_pitch_yaw_cd(target_roll, target_pitch, get_auto_heading(), true);
167 : }
168 : }
169 :
170 : // auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location
171 : // we assume the caller has set the circle's circle with sub.circle_nav.set_center()
172 : // we assume the caller has performed all required GPS_ok checks
173 2 : void ModeAuto::auto_circle_movetoedge_start(const Location &circle_center, float radius_m, bool ccw_turn)
174 : {
175 : // set circle center
176 2 : sub.circle_nav.set_center(circle_center);
177 :
178 : // set circle radius
179 2 : if (!is_zero(radius_m)) {
180 2 : sub.circle_nav.set_radius_cm(radius_m * 100.0f);
181 : }
182 :
183 : // set circle direction by using rate
184 2 : float current_rate = sub.circle_nav.get_rate_degs();
185 2 : current_rate = ccw_turn ? -fabsf(current_rate) : fabsf(current_rate);
186 2 : sub.circle_nav.set_rate_degs(current_rate);
187 :
188 : // check our distance from edge of circle
189 2 : Vector3f circle_edge_neu_cm;
190 : float dist_to_edge;
191 2 : sub.circle_nav.get_closest_point_on_circle_NEU_cm(circle_edge_neu_cm, dist_to_edge);
192 :
193 : // if more than 3m then fly to edge
194 2 : if (dist_to_edge > 300.0f) {
195 : // set the state to move to the edge of the circle
196 2 : sub.auto_mode = Auto_CircleMoveToEdge;
197 :
198 : // convert circle_edge_neu_cm to Location
199 2 : Location circle_edge(circle_edge_neu_cm, Location::AltFrame::ABOVE_ORIGIN);
200 :
201 : // convert altitude to same as command
202 2 : circle_edge.copy_alt_from(circle_center);
203 :
204 : // initialise wpnav to move to edge of circle
205 2 : if (!sub.wp_nav.set_wp_destination_loc(circle_edge)) {
206 : // failure to set destination can only be because of missing terrain data
207 0 : sub.failsafe_terrain_on_event();
208 : }
209 :
210 : // if we are outside the circle, point at the edge, otherwise hold yaw
211 2 : float dist_to_center = get_horizontal_distance((position_control->get_pos_estimate_NED_m().xy() * 100.0f).tofloat(), sub.circle_nav.get_center_NEU_cm().xy().tofloat());
212 2 : if (dist_to_center > sub.circle_nav.get_radius_cm() && dist_to_center > 500) {
213 2 : set_auto_yaw_mode(get_default_auto_yaw_mode(false));
214 : } else {
215 : // vehicle is within circle so hold yaw to avoid spinning as we move to edge of circle
216 0 : set_auto_yaw_mode(AUTO_YAW_HOLD);
217 : }
218 : } else {
219 0 : auto_circle_start();
220 : }
221 2 : }
222 :
223 : // auto_circle_start - initialises controller to fly a circle in AUTO flight mode
224 : // assumes that circle_nav object has already been initialised with circle center and radius
225 2 : void ModeAuto::auto_circle_start()
226 : {
227 2 : sub.auto_mode = Auto_Circle;
228 :
229 : // initialise circle controller
230 2 : sub.circle_nav.init_NEU_cm(sub.circle_nav.get_center_NEU_cm(), sub.circle_nav.center_is_terrain_alt(), sub.circle_nav.get_rate_degs());
231 2 : }
232 :
233 : // auto_circle_run - circle in AUTO flight mode
234 : // called by auto_run at 100hz or more
235 127276 : void ModeAuto::auto_circle_run()
236 : {
237 : // call circle controller
238 127276 : sub.failsafe_terrain_set_status(sub.circle_nav.update_cms());
239 :
240 : float lateral_out, forward_out;
241 127276 : sub.translate_circle_nav_rp(lateral_out, forward_out);
242 :
243 : // Send to forward/lateral outputs
244 127276 : motors.set_lateral(lateral_out);
245 127276 : motors.set_forward(forward_out);
246 :
247 : // WP_Nav has set the vertical position control targets
248 : // run the vertical position controller and set output throttle
249 127276 : position_control->D_update_controller();
250 :
251 : // roll & pitch from waypoint controller, yaw rate from pilot
252 127276 : attitude_control->input_euler_angle_roll_pitch_yaw_cd(channel_roll->get_control_in(), channel_pitch->get_control_in(), sub.circle_nav.get_yaw_cd(), true);
253 127276 : }
254 :
255 : #if NAV_GUIDED
256 : // auto_nav_guided_start - hand over control to external navigation controller in AUTO mode
257 0 : void ModeAuto::auto_nav_guided_start()
258 : {
259 0 : sub.mode_guided.init(true);
260 0 : sub.auto_mode = Auto_NavGuided;
261 : // initialise guided start time and position as reference for limit checking
262 0 : sub.mode_auto.guided_limit_init_time_and_pos();
263 0 : }
264 :
265 : // auto_nav_guided_run - allows control by external navigation controller
266 : // called by auto_run at 100hz or more
267 0 : void ModeAuto::auto_nav_guided_run()
268 : {
269 : // call regular guided flight mode run function
270 0 : sub.mode_guided.run();
271 0 : }
272 : #endif // NAV_GUIDED
273 :
274 : // auto_loiter_start - initialises loitering in auto mode
275 : // returns success/failure because this can be called by exit_mission
276 6 : bool ModeAuto::auto_loiter_start()
277 : {
278 : // return failure if GPS is bad
279 6 : if (!sub.position_ok()) {
280 0 : return false;
281 : }
282 6 : sub.auto_mode = Auto_Loiter;
283 :
284 : // calculate stopping point
285 6 : Vector3f stopping_point_neu_cm;
286 6 : sub.wp_nav.get_wp_stopping_point_NEU_cm(stopping_point_neu_cm);
287 :
288 : // initialise waypoint controller target to stopping point
289 6 : sub.wp_nav.set_wp_destination_NEU_cm(stopping_point_neu_cm);
290 :
291 : // hold yaw at current heading
292 6 : set_auto_yaw_mode(AUTO_YAW_HOLD);
293 :
294 6 : return true;
295 : }
296 :
297 : // auto_loiter_run - loiter in AUTO flight mode
298 : // called by auto_run at 100hz or more
299 9474 : void ModeAuto::auto_loiter_run()
300 : {
301 : // if not armed set throttle to zero and exit immediately
302 9474 : if (!motors.armed()) {
303 9474 : motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
304 : // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
305 9474 : attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt);
306 9474 : attitude_control->relax_attitude_controllers();
307 :
308 9474 : sub.wp_nav.wp_and_spline_init_m(); // Reset xy target
309 9474 : return;
310 : }
311 :
312 : // accept pilot input of yaw
313 0 : float target_yaw_rate = 0;
314 0 : if (!sub.failsafe.pilot_input) {
315 0 : target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
316 : }
317 :
318 : // set motors to full range
319 0 : motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
320 :
321 : // run waypoint and z-axis position controller
322 0 : sub.failsafe_terrain_set_status(sub.wp_nav.update_wpnav());
323 :
324 : ///////////////////////
325 : // update xy outputs //
326 : float lateral_out, forward_out;
327 0 : sub.translate_wpnav_rp(lateral_out, forward_out);
328 :
329 : // Send to forward/lateral outputs
330 0 : motors.set_lateral(lateral_out);
331 0 : motors.set_forward(forward_out);
332 :
333 : // WP_Nav has set the vertical position control targets
334 : // run the vertical position controller and set output throttle
335 0 : position_control->D_update_controller();
336 :
337 : // get pilot desired lean angles
338 : float target_roll, target_pitch;
339 0 : sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control->lean_angle_max_cd());
340 :
341 : // roll & pitch & yaw rate from pilot
342 0 : attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(target_roll, target_pitch, target_yaw_rate);
343 : }
344 :
345 :
346 : // set_auto_yaw_look_at_heading - sets the yaw look at heading for auto mode
347 12 : void ModeAuto::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle)
348 : {
349 : // get current yaw
350 12 : int32_t curr_yaw_target = attitude_control->get_att_target_euler_cd().z;
351 :
352 : // get final angle, 1 = Relative, 0 = Absolute
353 12 : if (relative_angle == 0) {
354 : // absolute angle
355 10 : sub.yaw_look_at_heading = wrap_360_cd(angle_deg * 100);
356 : } else {
357 : // relative angle
358 2 : if (direction < 0) {
359 0 : angle_deg = -angle_deg;
360 : }
361 2 : sub.yaw_look_at_heading = wrap_360_cd((angle_deg * 100 + curr_yaw_target));
362 : }
363 :
364 : // get turn speed
365 12 : if (is_zero(turn_rate_dps)) {
366 : // default to regular auto slew rate
367 0 : sub.yaw_look_at_heading_slew = AUTO_YAW_SLEW_RATE;
368 : } else {
369 12 : sub.yaw_look_at_heading_slew = MIN(turn_rate_dps, AUTO_YAW_SLEW_RATE); // deg / sec
370 : }
371 :
372 : // set yaw mode
373 12 : set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING);
374 :
375 : // TO-DO: restore support for clockwise and counter clockwise rotation held in cmd.content.yaw.direction. 1 = clockwise, -1 = counterclockwise
376 12 : }
377 :
378 :
379 : // sets the desired yaw rate
380 0 : void ModeAuto::set_yaw_rate(float turn_rate_dps)
381 : {
382 : // set sub to desired yaw rate
383 0 : sub.yaw_look_at_heading_slew = MIN(turn_rate_dps, AUTO_YAW_SLEW_RATE); // deg / sec
384 :
385 : // set yaw mode
386 0 : set_auto_yaw_mode(AUTO_YAW_RATE);
387 0 : }
388 :
389 : // set_auto_yaw_roi - sets the yaw to look at roi for auto mode
390 0 : void ModeAuto::set_auto_yaw_roi(const Location &roi_location)
391 : {
392 : // if location is zero lat, lon and altitude turn off ROI
393 0 : if (!roi_location.initialised()) {
394 : // set auto yaw mode back to default assuming the active command is a waypoint command. A more sophisticated method is required to ensure we return to the proper yaw control for the active command
395 0 : set_auto_yaw_mode(get_default_auto_yaw_mode(false));
396 : #if HAL_MOUNT_ENABLED
397 : // switch off the camera tracking if enabled
398 0 : sub.camera_mount.clear_roi_target();
399 : #endif // HAL_MOUNT_ENABLED
400 : } else {
401 : #if HAL_MOUNT_ENABLED
402 : // check if mount type requires us to rotate the sub
403 0 : if (!sub.camera_mount.has_pan_control()) {
404 0 : if (roi_location.get_vector_from_origin_NEU_cm(sub.roi_WP_neu_cm)) {
405 0 : set_auto_yaw_mode(AUTO_YAW_ROI);
406 : }
407 : }
408 : // send the command to the camera mount
409 0 : sub.camera_mount.set_roi_target(roi_location);
410 :
411 : // TO-DO: expand handling of the do_nav_roi to support all modes of the MAVLink. Currently we only handle mode 4 (see below)
412 : // 0: do nothing
413 : // 1: point at next waypoint
414 : // 2: point at a waypoint taken from WP# parameter (2nd parameter?)
415 : // 3: point at a location given by alt, lon, lat parameters
416 : // 4: point at a target given a target id (can't be implemented)
417 : #else
418 : // if we have no camera mount aim the sub at the location
419 : if (roi_location.get_vector_from_origin_NEU_cm(sub.roi_WP_neu_cm)) {
420 : set_auto_yaw_mode(AUTO_YAW_ROI);
421 : }
422 : #endif // HAL_MOUNT_ENABLED
423 : }
424 0 : }
425 :
426 : // Return true if it is possible to recover from a rangefinder failure
427 2 : bool ModeAuto::auto_terrain_recover_start()
428 : {
429 : #if AP_RANGEFINDER_ENABLED
430 : // Check rangefinder status to see if recovery is possible
431 2 : switch (sub.rangefinder.status_orient(ROTATION_PITCH_270)) {
432 :
433 2 : case RangeFinder::Status::OutOfRangeLow:
434 : case RangeFinder::Status::OutOfRangeHigh:
435 :
436 : // RangeFinder::Good if just one valid sample was obtained recently, but ::rangefinder_state.alt_healthy
437 : // requires several consecutive valid readings for wpnav to accept rangefinder data
438 : case RangeFinder::Status::Good:
439 2 : sub.auto_mode = Auto_TerrainRecover;
440 2 : break;
441 :
442 0 : case RangeFinder::Status::NotConnected:
443 : case RangeFinder::Status::NoData:
444 : case RangeFinder::Status::PoweredDown:
445 0 : return false;
446 : }
447 :
448 : // Initialize recovery timeout time
449 2 : sub.fs_terrain_recover_start_ms = AP_HAL::millis();
450 :
451 : // Stop mission
452 2 : sub.mission.stop();
453 :
454 : // Reset xy target
455 2 : sub.loiter_nav.clear_pilot_desired_acceleration();
456 2 : sub.loiter_nav.init_target();
457 :
458 : // Reset z axis controller
459 2 : position_control->D_relax_controller(motors.get_throttle_hover());
460 :
461 : // initialize vertical maximum speeds and acceleration
462 : // All limits must be positive
463 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());
464 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());
465 :
466 2 : gcs().send_text(MAV_SEVERITY_WARNING, "Attempting auto failsafe recovery");
467 2 : return true;
468 : #else
469 : return false;
470 : #endif
471 : }
472 :
473 : // Attempt recovery from terrain failsafe
474 : // If recovery is successful resume mission
475 : // If recovery fails revert to failsafe action
476 7662 : void ModeAuto::auto_terrain_recover_run()
477 : {
478 7662 : float target_climb_rate = 0;
479 :
480 : // if not armed set throttle to zero and exit immediately
481 7662 : if (!motors.armed()) {
482 973 : motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
483 973 : attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt);
484 973 : attitude_control->relax_attitude_controllers();
485 :
486 973 : sub.loiter_nav.init_target(); // Reset xy target
487 973 : position_control->D_relax_controller(motors.get_throttle_hover()); // Reset z axis controller
488 974 : return;
489 : }
490 :
491 : #if AP_RANGEFINDER_ENABLED
492 : static uint32_t rangefinder_recovery_ms = 0;
493 6689 : switch (sub.rangefinder.status_orient(ROTATION_PITCH_270)) {
494 :
495 0 : case RangeFinder::Status::OutOfRangeLow:
496 0 : target_climb_rate = sub.wp_nav.get_default_speed_up_cms();
497 0 : rangefinder_recovery_ms = 0;
498 0 : break;
499 :
500 6047 : case RangeFinder::Status::OutOfRangeHigh:
501 6047 : target_climb_rate = -sub.wp_nav.get_default_speed_down_cms();
502 6047 : rangefinder_recovery_ms = 0;
503 6047 : break;
504 :
505 642 : case RangeFinder::Status::Good: // exit on success (recovered rangefinder data)
506 :
507 642 : target_climb_rate = 0; // Attempt to hold current depth
508 :
509 642 : if (sub.rangefinder_state.alt_healthy) {
510 :
511 : // Start timer as soon as rangefinder is healthy
512 602 : if (rangefinder_recovery_ms == 0) {
513 1 : rangefinder_recovery_ms = AP_HAL::millis();
514 1 : position_control->D_relax_controller(motors.get_throttle_hover()); // Reset alt hold targets
515 : }
516 :
517 : // 1.5 seconds of healthy rangefinder means we can resume mission with terrain enabled
518 602 : if (AP_HAL::millis() > rangefinder_recovery_ms + 1500) {
519 1 : gcs().send_text(MAV_SEVERITY_INFO, "Terrain failsafe recovery successful!");
520 1 : sub.failsafe_terrain_set_status(true); // Reset failsafe timers
521 1 : sub.failsafe.terrain = false; // Clear flag
522 1 : sub.auto_mode = Auto_Loiter; // Switch back to loiter for next iteration
523 1 : sub.mission.resume(); // Resume mission
524 1 : rangefinder_recovery_ms = 0; // Reset for subsequent recoveries
525 : }
526 :
527 : }
528 642 : break;
529 :
530 0 : case RangeFinder::Status::NotConnected:
531 : case RangeFinder::Status::NoData:
532 : case RangeFinder::Status::PoweredDown:
533 : // Terrain failsafe recovery has failed, terrain data is not available
534 : // and rangefinder is not connected, or has stopped responding
535 0 : gcs().send_text(MAV_SEVERITY_CRITICAL, "Terrain failsafe recovery failure: No Rangefinder!");
536 0 : sub.failsafe_terrain_act();
537 0 : rangefinder_recovery_ms = 0;
538 0 : return;
539 : }
540 : #else
541 : gcs().send_text(MAV_SEVERITY_CRITICAL, "Terrain failsafe recovery failure: No Rangefinder!");
542 : sub.failsafe_terrain_act();
543 : return;
544 : #endif
545 :
546 : // exit on failure (timeout)
547 6689 : if (AP_HAL::millis() > sub.fs_terrain_recover_start_ms + FS_TERRAIN_RECOVER_TIMEOUT_MS) {
548 : // Recovery has failed, revert to failsafe action
549 1 : gcs().send_text(MAV_SEVERITY_CRITICAL, "Terrain failsafe recovery timeout!");
550 1 : sub.failsafe_terrain_act();
551 1 : return;
552 : }
553 :
554 : // run loiter controller
555 6688 : sub.loiter_nav.update();
556 :
557 : ///////////////////////
558 : // update xy targets //
559 : float lateral_out, forward_out;
560 6688 : sub.translate_wpnav_rp(lateral_out, forward_out);
561 :
562 : // Send to forward/lateral outputs
563 6688 : motors.set_lateral(lateral_out);
564 6688 : motors.set_forward(forward_out);
565 :
566 : /////////////////////
567 : // update z target //
568 6688 : position_control->D_set_pos_target_from_climb_rate_cms(target_climb_rate);
569 6688 : position_control->D_update_controller();
570 :
571 : ////////////////////////////
572 : // update angular targets //
573 6688 : float target_roll = 0;
574 6688 : float target_pitch = 0;
575 :
576 : // convert pilot input to lean angles
577 : // To-Do: convert sub.get_pilot_desired_lean_angles to return angles as floats
578 6688 : sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control->lean_angle_max_cd());
579 :
580 6688 : float target_yaw_rate = 0;
581 :
582 : // call attitude controller
583 6688 : attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(target_roll, target_pitch, target_yaw_rate);
584 : }
|
