Browse Source

Copter: split home-set and home-locked state

mission-4.1.18
Peter Barker 7 years ago committed by Andrew Tridgell
parent
commit
c09ccf5b61
  1. 7
      ArduCopter/commands.cpp
  2. 2
      ArduCopter/motors.cpp

7
ArduCopter/commands.cpp

@ -72,15 +72,16 @@ bool Copter::set_home(const Location& loc, bool lock)
return false; return false;
} }
const bool home_was_set = ahrs.home_is_set();
// set ahrs home (used for RTL) // set ahrs home (used for RTL)
ahrs.set_home(loc); ahrs.set_home(loc);
// init inav and compass declination // init inav and compass declination
if (!ahrs.home_is_set()) { if (!home_was_set) {
// update navigation scalers. used to offset the shrinking longitude as we go towards the poles // update navigation scalers. used to offset the shrinking longitude as we go towards the poles
scaleLongDown = longitude_scale(loc); scaleLongDown = longitude_scale(loc);
// record home is set // record home is set
ahrs.set_home_status(HOME_SET_NOT_LOCKED);
Log_Write_Event(DATA_SET_HOME); Log_Write_Event(DATA_SET_HOME);
#if MODE_AUTO_ENABLED == ENABLED #if MODE_AUTO_ENABLED == ENABLED
@ -96,7 +97,7 @@ bool Copter::set_home(const Location& loc, bool lock)
// lock home position // lock home position
if (lock) { if (lock) {
ahrs.set_home_status(HOME_SET_AND_LOCKED); ahrs.lock_home();
} }
// log ahrs home and ekf origin dataflash // log ahrs home and ekf origin dataflash

2
ArduCopter/motors.cpp

@ -180,7 +180,7 @@ bool Copter::init_arm_motors(bool arming_from_gcs)
// we have reset height, so arming height is zero // we have reset height, so arming height is zero
arming_altitude_m = 0; arming_altitude_m = 0;
} else if (ahrs.home_status() == HOME_SET_NOT_LOCKED) { } else if (!ahrs.home_is_locked()) {
// Reset home position if it has already been set before (but not locked) // Reset home position if it has already been set before (but not locked)
set_home_to_current_location(false); set_home_to_current_location(false);

Loading…
Cancel
Save