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

Copter: don't assume home is at the origin

Browse Source
This commit is contained in:
Jonathan Challinger 2015-02-02 18:12:31 -08:00 committed by Randy Mackay
parent 9c2865f6a8
commit 2cb63dc792
2 changed files with 4 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduCopter/control_rtl.pde
View File

@ -117,7 +117,8 @@ static void rtl_return_start()
rally_point.alt = max(rally_point.alt, current_loc.alt); // ensure we do not descend before reaching 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); Vector3f destination = pv_location_to_vector(rally_point);
#else #else
Vector3f destination = Vector3f(0,0,get_RTL_alt()); Vector3f destination = pv_location_to_vector(ahrs.get_home());
destination.z = get_RTL_alt();
#endif #endif
wp_nav.set_wp_destination(destination); wp_nav.set_wp_destination(destination);

3
ArduCopter/navigation.pde
View File

@ -66,7 +66,8 @@ static void calc_home_distance_and_bearing()
// calculate home distance and bearing // calculate home distance and bearing
if (position_ok()) { if (position_ok()) {
home_distance = pythagorous2(curr.x, curr.y); home_distance = pythagorous2(curr.x, curr.y);
home_bearing = pv_get_bearing_cd(curr,Vector3f(0,0,0)); Vector3f home = pv_location_to_vector(ahrs.get_home());
home_bearing = pv_get_bearing_cd(curr,home);
// update super simple bearing (if required) because it relies on home_bearing // update super simple bearing (if required) because it relies on home_bearing
update_super_simple_bearing(false); update_super_simple_bearing(false);