aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: bug fix to rally point alt

Browse Source 
rally point library uses absolute altitudes, we were passing in relative
altitudes which caused the vehicle to climb before heading to the rally
point
This commit is contained in:
Randy Mackay 2014-08-14 21:09:54 +09:00
parent 8925c61ae4
commit 0a88281a76
1 changed files with 9 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
ArduCopter/control_rtl.pde
View File

@ -86,8 +86,10 @@ static void rtl_climb_start()
wp_nav.get_wp_stopping_point_xy(destination);
#if AC_RALLY == ENABLED
// rally_point.alt will be max of RTL_ALT and the height of the nearest rally point (if there is one)
Location rally_point = rally.calc_best_rally_or_home_location(current_loc, get_RTL_alt());
// rally_point.alt will be the altitude of the nearest rally point or the RTL_ALT. uses absolute altitudes
Location rally_point = rally.calc_best_rally_or_home_location(current_loc, get_RTL_alt()+ahrs.get_home().alt);
rally_point.alt -= ahrs.get_home().alt; // convert to altitude above home
rally_point.alt = max(rally_point.alt, current_loc.alt); // ensure we do not descend before reaching home
destination.z = rally_point.alt;
#else
destination.z = get_RTL_alt();
@ -109,7 +111,11 @@ static void rtl_return_start()
// set target to above home/rally point
#if AC_RALLY == ENABLED
Vector3f destination = pv_location_to_vector(rally.calc_best_rally_or_home_location(current_loc, get_RTL_alt()));
// rally_point will be the nearest rally point or home. uses absolute altitudes
Location rally_point = rally.calc_best_rally_or_home_location(current_loc, get_RTL_alt()+ahrs.get_home().alt);
rally_point.alt -= ahrs.get_home().alt; // convert to altitude above home
rally_point.alt = max(rally_point.alt, current_loc.alt); // ensure we do not descend before reaching home
Vector3f destination = pv_location_to_vector(rally_point);
#else
Vector3f destination = Vector3f(0,0,get_RTL_alt());
#endif