Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 20:48:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: split home-set and home-locked state

Browse Source
This commit is contained in:
Peter Barker 2018-05-18 13:00:01 +10:00 committed by Andrew Tridgell
parent ad600fff68
commit 69d8980608
2 changed files with 5 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
ArduSub/commands.cpp
View File

@ -1,12 +1,5 @@
#include "Sub.h" #include "Sub.h"
/*
* the home_state has a number of possible values (see enum HomeState in defines.h's)
* HOME_UNSET = home is not set, no GPS positions yet received
* HOME_SET_NOT_LOCKED = home is set to EKF origin or armed location (can be moved)
* HOME_SET_AND_LOCKED = home has been set by user, cannot be moved except by user initiated do-set-home command
*/
// checks if we should update ahrs/RTL home position from the EKF // checks if we should update ahrs/RTL home position from the EKF
void Sub::update_home_from_EKF() void Sub::update_home_from_EKF()
{ {
@ -69,15 +62,16 @@ bool Sub::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);
// log new home position which mission library will pull from ahrs // log new home position which mission library will pull from ahrs
@ -91,7 +85,7 @@ bool Sub::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
ArduSub/motors.cpp
View File

@ -50,7 +50,7 @@ bool Sub::init_arm_motors(bool arming_from_gcs)
// Always use absolute altitude for ROV // Always use absolute altitude for ROV
// ahrs.resetHeightDatum(); // ahrs.resetHeightDatum();
// Log_Write_Event(DATA_EKF_ALT_RESET); // Log_Write_Event(DATA_EKF_ALT_RESET);
} else if (ahrs.home_status() == HOME_SET_NOT_LOCKED) { } else if (ahrs.home_is_set() && !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);
} }