Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: auto, guided, rtl call wpnav.init instead of set-desired-speed

Browse Source 
also smart-rtl
This commit is contained in:
Randy Mackay 2022-01-06 09:01:49 +09:00
parent 5d96804ef3
commit a5638f5699
4 changed files with 10 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Rover/mode_auto.cpp
View File

@ -11,8 +11,8 @@ bool ModeAuto::_enter()
return false; return false;
} }
// initialise waypoint speed // initialise waypoint navigation library
g2.wp_nav.set_desired_speed_to_default(); g2.wp_nav.init();
// other initialisation // other initialisation
auto_triggered = false; auto_triggered = false;

4
Rover/mode_guided.cpp
View File

@ -12,8 +12,8 @@ bool ModeGuided::_enter()
start_stop(); start_stop();
} }
// initialise waypoint speed // initialise waypoint navigation library
g2.wp_nav.set_desired_speed_to_default(); g2.wp_nav.init();
send_notification = false; send_notification = false;

10
Rover/mode_rtl.cpp
View File

@ -8,6 +8,9 @@ bool ModeRTL::_enter()
return false; return false;
} }
// initialise waypoint navigation library
g2.wp_nav.init(MAX(0, g2.rtl_speed));
// set target to the closest rally point or home // set target to the closest rally point or home
#if AP_RALLY == ENABLED #if AP_RALLY == ENABLED
if (!g2.wp_nav.set_desired_location(g2.rally.calc_best_rally_or_home_location(rover.current_loc, ahrs.get_home().alt))) { if (!g2.wp_nav.set_desired_location(g2.rally.calc_best_rally_or_home_location(rover.current_loc, ahrs.get_home().alt))) {
@ -20,13 +23,6 @@ bool ModeRTL::_enter()
} }
#endif #endif
// initialise waypoint speed
if (is_positive(g2.rtl_speed)) {
g2.wp_nav.set_desired_speed(g2.rtl_speed);
} else {
g2.wp_nav.set_desired_speed_to_default();
}
send_notification = true; send_notification = true;
_loitering = false; _loitering = false;
return true; return true;

10
Rover/mode_smart_rtl.cpp
View File

@ -14,18 +14,14 @@ bool ModeSmartRTL::_enter()
return false; return false;
} }
// initialise waypoint navigation library
g2.wp_nav.init(MAX(0, g2.rtl_speed));
// set desired location to reasonable stopping point // set desired location to reasonable stopping point
if (!g2.wp_nav.set_desired_location_to_stopping_location()) { if (!g2.wp_nav.set_desired_location_to_stopping_location()) {
return false; return false;
} }
// initialise waypoint speed
if (is_positive(g2.rtl_speed)) {
g2.wp_nav.set_desired_speed(g2.rtl_speed);
} else {
g2.wp_nav.set_desired_speed_to_default();
}
// init state // init state
smart_rtl_state = SmartRTL_WaitForPathCleanup; smart_rtl_state = SmartRTL_WaitForPathCleanup;
_loitering = false; _loitering = false;