mirror of https://github.com/ArduPilot/ardupilot
small change to RTL to prevent negative effects from flying backwards.
git-svn-id: https://arducopter.googlecode.com/svn/trunk@2981 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
f096d8cb23
commit
b2a47c01a7
|
@ -146,6 +146,7 @@ static void calc_rate_nav(int speed)
|
||||||
|
|
||||||
// calc the cos of the error to tell how fast we are moving towards the target in cm
|
// calc the cos of the error to tell how fast we are moving towards the target in cm
|
||||||
int groundspeed = (float)g_gps->ground_speed * cos(radians((float)target_error/100));
|
int groundspeed = (float)g_gps->ground_speed * cos(radians((float)target_error/100));
|
||||||
|
groundspeed = max(groundspeed, 0);
|
||||||
|
|
||||||
// Reduce speed on RTL
|
// Reduce speed on RTL
|
||||||
if(control_mode == RTL){
|
if(control_mode == RTL){
|
||||||
|
|
Loading…
Reference in New Issue