diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 0edd1eef40..691c33c238 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1211,6 +1211,11 @@ static void update_GPS(void) // start again if we lose 3d lock ground_start_count = 10; } + } else { + // update home position when not armed + if (!motors.armed()) { + update_home(); + } } //If we are not currently armed, and we're ready to diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index 2481a0b7d0..3dc10b5e71 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -26,5 +26,19 @@ static void init_home() scaleLongUp = 1.0f/scaleLongDown; } +// update_home - reset home to current location +// should be called only when vehicle is disarmed +static void update_home() +{ + // copter uses 0 home altitude + Location loc = gps.location(); + + // set ahrs object's home position + ahrs.set_home(loc); + + // update navigation scalers. used to offset the shrinking longitude as we go towards the poles + scaleLongDown = longitude_scale(loc); + scaleLongUp = 1.0f/scaleLongDown; +}