diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 1becb7fa86..8e61d2fd5a 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1354,6 +1354,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) new_home_loc.alt = (int32_t)(packet.param7 * 100.0f); plane.ahrs.set_home(new_home_loc); plane.home_is_set = HOME_SET_NOT_LOCKED; + plane.Log_Write_Home_And_Origin(); result = MAV_RESULT_ACCEPTED; plane.gcs_send_text_fmt(PSTR("set home to %.6f %.6f at %um"), (double)(new_home_loc.lat*1.0e-7f), diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index 487ce21e69..491995dc38 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -450,6 +450,23 @@ void Plane::Log_Write_Airspeed(void) DataFlash.Log_Write_Airspeed(airspeed); } +// log ahrs home and EKF origin to dataflash +void Plane::Log_Write_Home_And_Origin() +{ +#if AP_AHRS_NAVEKF_AVAILABLE + // log ekf origin if set + Location ekf_orig; + if (ahrs.get_NavEKF_const().getOriginLLH(ekf_orig)) { + DataFlash.Log_Write_Origin(LogOriginType::ekf_origin, ekf_orig); + } +#endif + + // log ahrs home if set + if (home_is_set != HOME_UNSET) { + DataFlash.Log_Write_Origin(LogOriginType::ahrs_home, ahrs.get_home()); + } +} + static const struct LogStructure log_structure[] PROGMEM = { LOG_COMMON_STRUCTURES, { LOG_PERFORMANCE_MSG, sizeof(log_Performance), diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index ba6036c2fd..b21c3259f8 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -685,6 +685,7 @@ private: void Log_Write_RC(void); void Log_Write_Baro(void); void Log_Write_Airspeed(void); + void Log_Write_Home_And_Origin(); void Log_Read(uint16_t log_num, int16_t start_page, int16_t end_page); void start_logging(); void load_parameters(void); diff --git a/ArduPlane/commands.cpp b/ArduPlane/commands.cpp index edc8231af5..d6ba570e8e 100644 --- a/ArduPlane/commands.cpp +++ b/ArduPlane/commands.cpp @@ -104,6 +104,7 @@ void Plane::init_home() ahrs.set_home(gps.location()); home_is_set = HOME_SET_NOT_LOCKED; + Log_Write_Home_And_Origin(); gcs_send_text_fmt(PSTR("gps alt: %lu"), (unsigned long)home.alt); @@ -124,6 +125,7 @@ void Plane::update_home() { if (home_is_set == HOME_SET_NOT_LOCKED) { ahrs.set_home(gps.location()); + Log_Write_Home_And_Origin(); } barometer.update_calibration(); } diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 5469a30365..cf921b8df2 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -790,6 +790,7 @@ void Plane::do_set_home(const AP_Mission::Mission_Command& cmd) } else { ahrs.set_home(cmd.content.location); home_is_set = HOME_SET_NOT_LOCKED; + Log_Write_Home_And_Origin(); } }