Copter: when arming set home to current loc only if not locked

This commit is contained in:
Randy Mackay 2015-02-09 20:27:12 +09:00
parent 9e156d2f81
commit e7579198c9
1 changed files with 4 additions and 5 deletions

View File

@ -140,12 +140,11 @@ static bool init_arm_motors(bool arming_from_gcs)
initial_armed_bearing = ahrs.yaw_sensor;
// Reset home position
// -------------------
if (ap.home_is_set) {
init_home();
calc_distance_and_bearing();
// Reset home position if it has already been set before (but not locked)
if (ap.home_state == HOME_SET_NOT_LOCKED) {
set_home_to_current_location();
}
calc_distance_and_bearing();
if(did_ground_start == false) {
startup_ground(true);