From 9a73f0c260e51be9b0ecc4c15d31af03aede24b7 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 16 May 2018 13:30:28 +1000 Subject: [PATCH] Sub: move Log_Write_Home_And_Origin into AP_AHRS --- ArduSub/Log.cpp | 18 +----------------- ArduSub/Sub.h | 1 - ArduSub/commands.cpp | 4 ++-- 3 files changed, 3 insertions(+), 20 deletions(-) diff --git a/ArduSub/Log.cpp b/ArduSub/Log.cpp index 1574804bcf..28a420e060 100644 --- a/ArduSub/Log.cpp +++ b/ArduSub/Log.cpp @@ -274,21 +274,6 @@ void Sub::Log_Write_Error(uint8_t sub_system, uint8_t error_code) DataFlash.WriteCriticalBlock(&pkt, sizeof(pkt)); } -// log EKF origin and ahrs home to dataflash -void Sub::Log_Write_Home_And_Origin() -{ - // log ekf origin if set - Location ekf_orig; - if (ahrs.get_origin(ekf_orig)) { - DataFlash.Log_Write_Origin(LogOriginType::ekf_origin, ekf_orig); - } - - // log ahrs home if set - if (ahrs.home_is_set()) { - DataFlash.Log_Write_Origin(LogOriginType::ahrs_home, ahrs.get_home()); - } -} - // logs when baro or compass becomes unhealthy void Sub::Log_Sensor_Health() { @@ -367,7 +352,7 @@ void Sub::Log_Write_Vehicle_Startup_Messages() { // only 200(?) bytes are guaranteed by DataFlash DataFlash.Log_Write_Mode(control_mode, control_mode_reason); - Log_Write_Home_And_Origin(); + ahrs.Log_Write_Home_And_Origin(); gps.Write_DataFlash_Log_Startup_messages(); } @@ -391,7 +376,6 @@ void Sub::Log_Write_Data(uint8_t id, int16_t value) {} void Sub::Log_Write_Data(uint8_t id, uint16_t value) {} void Sub::Log_Write_Data(uint8_t id, float value) {} void Sub::Log_Write_Error(uint8_t sub_system, uint8_t error_code) {} -void Sub::Log_Write_Home_And_Origin() {} void Sub::Log_Sensor_Health() {} void Sub::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {} void Sub::Log_Write_Vehicle_Startup_Messages() {} diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 96070134c2..6b79cbbf5c 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -503,7 +503,6 @@ private: void Log_Write_Data(uint8_t id, uint16_t value); void Log_Write_Data(uint8_t id, float value); void Log_Write_Error(uint8_t sub_system, uint8_t error_code); - void Log_Write_Home_And_Origin(); void Log_Sensor_Health(); void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target); void Log_Write_Vehicle_Startup_Messages(); diff --git a/ArduSub/commands.cpp b/ArduSub/commands.cpp index e57905ea08..35dbea2459 100644 --- a/ArduSub/commands.cpp +++ b/ArduSub/commands.cpp @@ -95,7 +95,7 @@ bool Sub::set_home(const Location& loc, bool lock) } // log ahrs home and ekf origin dataflash - Log_Write_Home_And_Origin(); + ahrs.Log_Write_Home_And_Origin(); // send new home and ekf origin to GCS gcs().send_home(loc); @@ -125,7 +125,7 @@ void Sub::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);