From 052b76d0170bfc6f3c13e45b7771123ad1561c56 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 16 May 2018 13:30:09 +1000 Subject: [PATCH] Plane: move Log_Write_Home_And_Origin into AP_AHRS --- ArduPlane/GCS_Mavlink.cpp | 6 +++--- ArduPlane/Log.cpp | 20 +------------------- ArduPlane/Plane.h | 1 - ArduPlane/commands.cpp | 6 +++--- ArduPlane/commands_logic.cpp | 2 +- 5 files changed, 8 insertions(+), 27 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index ebf9387fdc..01dff75a7d 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -876,7 +876,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) } plane.ahrs.set_home(new_home_loc); AP::ahrs().set_home_status(HOME_SET_NOT_LOCKED); - plane.Log_Write_Home_And_Origin(); + AP::ahrs().Log_Write_Home_And_Origin(); gcs().send_home(new_home_loc); result = MAV_RESULT_ACCEPTED; gcs().send_text(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %um", @@ -1162,7 +1162,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) new_home_loc.alt = (int32_t)(packet.param7 * 100.0f); plane.ahrs.set_home(new_home_loc); AP::ahrs().set_home_status(HOME_SET_NOT_LOCKED); - plane.Log_Write_Home_And_Origin(); + AP::ahrs().Log_Write_Home_And_Origin(); gcs().send_home(new_home_loc); result = MAV_RESULT_ACCEPTED; gcs().send_text(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %um", @@ -1580,7 +1580,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) new_home_loc.alt = packet.altitude / 10; plane.ahrs.set_home(new_home_loc); plane.ahrs.set_home_status(HOME_SET_NOT_LOCKED); - plane.Log_Write_Home_And_Origin(); + plane.ahrs.Log_Write_Home_And_Origin(); gcs().send_home(new_home_loc); gcs().send_text(MAV_SEVERITY_INFO, "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 4b244108c6..6125c284a5 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -282,23 +282,6 @@ void Plane::Log_Write_RC(void) Log_Write_AETR(); } -// 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_origin(ekf_orig)) { - DataFlash.Log_Write_Origin(LogOriginType::ekf_origin, ekf_orig); - } -#endif - - // log ahrs home if set - if (ahrs.home_is_set()) { - DataFlash.Log_Write_Origin(LogOriginType::ahrs_home, ahrs.get_home()); - } -} - // type and unit information can be found in // libraries/DataFlash/Logstructure.h; search for "log_Units" for // units and "Format characters" for field type information @@ -344,7 +327,7 @@ void Plane::Log_Write_Vehicle_Startup_Messages() Log_Write_Startup(TYPE_GROUNDSTART_MSG); DataFlash.Log_Write_Mode(control_mode, control_mode_reason); DataFlash.Log_Write_Rally(rally); - Log_Write_Home_And_Origin(); + ahrs.Log_Write_Home_And_Origin(); gps.Write_DataFlash_Log_Startup_messages(); } @@ -373,7 +356,6 @@ void Plane::Log_Write_Optflow() {} void Plane::Log_Arm_Disarm() {} void Plane::Log_Write_RC(void) {} -void Plane::Log_Write_Home_And_Origin() {} void Plane::Log_Write_Vehicle_Startup_Messages() {} void Plane::log_init(void) {} diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 191fe21ab4..1180832864 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -801,7 +801,6 @@ private: void Log_Write_Optflow(); void Log_Arm_Disarm(); void Log_Write_RC(void); - void Log_Write_Home_And_Origin(); void Log_Write_Vehicle_Startup_Messages(); void Log_Write_AOA_SSA(); void Log_Write_AETR(); diff --git a/ArduPlane/commands.cpp b/ArduPlane/commands.cpp index bc46fa0b01..4efb44acf3 100644 --- a/ArduPlane/commands.cpp +++ b/ArduPlane/commands.cpp @@ -111,7 +111,7 @@ void Plane::init_home() ahrs.set_home(gps.location()); ahrs.set_home_status(HOME_SET_NOT_LOCKED); - Log_Write_Home_And_Origin(); + ahrs.Log_Write_Home_And_Origin(); gcs().send_home(gps.location()); // Save Home to EEPROM @@ -142,7 +142,7 @@ void Plane::update_home() Location loc; if(ahrs.get_position(loc)) { ahrs.set_home(loc); - Log_Write_Home_And_Origin(); + ahrs.Log_Write_Home_And_Origin(); gcs().send_home(loc); } } @@ -169,7 +169,7 @@ void Plane::set_ekf_origin(const Location& loc) } // log ahrs home and ekf origin dataflash - Log_Write_Home_And_Origin(); + ahrs.Log_Write_Home_And_Origin(); // send ekf origin to GCS gcs().send_ekf_origin(loc); diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 04383112df..c72d18f868 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -938,7 +938,7 @@ void Plane::do_set_home(const AP_Mission::Mission_Command& cmd) } else { ahrs.set_home(cmd.content.location); ahrs.set_home_status(HOME_SET_NOT_LOCKED); - Log_Write_Home_And_Origin(); + ahrs.Log_Write_Home_And_Origin(); gcs().send_home(cmd.content.location); // send ekf origin if set Location ekf_origin;