From cdb532a594a0c5040b6bc295e804a769cc9cd8bc Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 11 Apr 2013 18:24:20 +0900 Subject: [PATCH] Copter: bug fix for loiter target --- ArduCopter/navigation.pde | 5 +++-- libraries/AP_InertialNav/AP_InertialNav.h | 3 +++ 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 897c70a018..8c5a27624a 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -389,8 +389,9 @@ get_loiter_pos_lat_lon(int32_t target_lat_from_home, int32_t target_lon_from_hom int16_t linear_distance; // the distace we swap between linear and sqrt. // calculate distance error - dist_error_lat = target_lat_from_home - inertial_nav.get_latitude_diff(); - dist_error_lon = target_lon_from_home - inertial_nav.get_longitude_diff(); + Vector3f curr = inertial_nav.get_position(); + dist_error_lat = target_lat_from_home - curr.x; + dist_error_lon = target_lon_from_home - curr.y; linear_distance = MAX_LOITER_POS_ACCEL/(2*g.pi_loiter_lat.kP()*g.pi_loiter_lat.kP()); diff --git a/libraries/AP_InertialNav/AP_InertialNav.h b/libraries/AP_InertialNav/AP_InertialNav.h index ad2f06d0dc..05d6b87935 100644 --- a/libraries/AP_InertialNav/AP_InertialNav.h +++ b/libraries/AP_InertialNav/AP_InertialNav.h @@ -61,6 +61,9 @@ public: // correct_with_gps - modifies accelerometer offsets using gps. dt is time since last gps update void correct_with_gps(int32_t lon, int32_t lat, float dt); + // get_position - returns current position from home in cm + Vector3f get_position() { return _position_base + _position_correction; } + // get latitude & longitude positions int32_t get_latitude(); int32_t get_longitude();