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

APMRover2: prevent unless calculus when stoping

Browse Source
This commit is contained in:
Pierre Kancir 2017-05-03 16:06:38 +02:00 committed by Grant Morphett
parent 670e7b7914
commit ed26c103f9
1 changed files with 6 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
APMrover2/APMrover2.cpp
View File

@ -569,11 +569,12 @@ void Rover::update_navigation()
case RTL: case RTL:
// no loitering around the wp with the rover, goes direct to the wp position // no loitering around the wp with the rover, goes direct to the wp position
calc_lateral_acceleration();
calc_nav_steer();
if (verify_RTL()) { if (verify_RTL()) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get()); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get());
set_mode(HOLD); set_mode(HOLD);
} else {
calc_lateral_acceleration();
calc_nav_steer();
} }
break; break;
@ -585,13 +586,14 @@ void Rover::update_navigation()
case Guided_WP: case Guided_WP:
// no loitering around the wp with the rover, goes direct to the wp position // no loitering around the wp with the rover, goes direct to the wp position
calc_lateral_acceleration();
calc_nav_steer();
if (rtl_complete || verify_RTL()) { if (rtl_complete || verify_RTL()) {
// we have reached destination so stop where we are // we have reached destination so stop where we are
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get()); SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get());
SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0); SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
lateral_acceleration = 0; lateral_acceleration = 0;
} else {
calc_lateral_acceleration();
calc_nav_steer();
} }
break; break;