Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Sub: guided pos hold init loses redundant set-auto-yaw-mode 

also use wpnav's 3d stopping point method
This commit is contained in:
Randy Mackay 2020-11-25 10:29:56 +09:00
parent 4cd1c3cfde
commit a8f4ab3d43
1 changed files with 2 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduSub/control_guided.cpp
View File

@ -39,8 +39,7 @@ bool Sub::guided_init(bool ignore_checks)
if (!position_ok() && !ignore_checks) { if (!position_ok() && !ignore_checks) {
return false; return false;
} }
// initialise yaw
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
// start in position control mode // start in position control mode
guided_pos_control_start(); guided_pos_control_start();
return true; return true;
@ -59,8 +58,7 @@ void Sub::guided_pos_control_start()
// To-Do: set to current location if disarmed? // To-Do: set to current location if disarmed?
// To-Do: set to stopping point altitude? // To-Do: set to stopping point altitude?
Vector3f stopping_point; Vector3f stopping_point;
stopping_point.z = inertial_nav.get_altitude(); wp_nav.get_wp_stopping_point(stopping_point);
wp_nav.get_wp_stopping_point_xy(stopping_point);
// no need to check return status because terrain data is not used // no need to check return status because terrain data is not used
wp_nav.set_wp_destination(stopping_point, false); wp_nav.set_wp_destination(stopping_point, false);