AP_AHRS: Ensure home is always saved to mission

This commit is contained in:
Iampete1 2025-01-14 16:37:02 +00:00 committed by Andrew Tridgell
parent b5ce6f1efd
commit 82887c95d2

View File

@ -39,6 +39,12 @@
#include <GCS_MAVLink/GCS.h>
#include <AP_InertialSensor/AP_InertialSensor.h>
#include <AP_CustomRotations/AP_CustomRotations.h>
#include <AP_Mission/AP_Mission_config.h>
#if AP_MISSION_ENABLED
#include <AP_Mission/AP_Mission.h>
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <SITL/SITL.h>
#endif
@ -2985,6 +2991,14 @@ bool AP_AHRS::set_home(const Location &loc)
pd.home_lon = loc.lng;
pd.home_alt_cm = loc.alt;
#if AP_MISSION_ENABLED
// Save home to mission
AP_Mission *mission = AP::mission();
if (mission != nullptr) {
mission->write_home_to_storage();
}
#endif
return true;
}