Rover: move Log_Write_Home_And_Origin into AP_AHRS

gChuNguyen · May 17, 2018 · a52f3d8 · a52f3d8
1 parent 9793703
commit a52f3d8
Show file tree

Hide file tree

Showing 3 changed files with 4 additions and 23 deletions.
diff --git a/APMrover2/Log.cpp b/APMrover2/Log.cpp
@@ -204,23 +204,6 @@ void Rover::Log_Write_Error(uint8_t sub_system, uint8_t error_code)
   DataFlash.WriteBlock(&pkt, sizeof(pkt));
 }
 
-// log ahrs home and EKF origin to dataflash
-void Rover::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());
-    }
-}
-
 // guided mode logging
 struct PACKED log_GuidedTarget {
     LOG_PACKET_HEADER;
@@ -324,7 +307,7 @@ void Rover::Log_Write_Vehicle_Startup_Messages()
     // only 200(?) bytes are guaranteed by DataFlash
     Log_Write_Startup(TYPE_GROUNDSTART_MSG);
     DataFlash.Log_Write_Mode(control_mode->mode_number(), control_mode_reason);
-    Log_Write_Home_And_Origin();
+    ahrs.Log_Write_Home_And_Origin();
     gps.Write_DataFlash_Log_Startup_messages();
 }
 
@@ -339,7 +322,6 @@ void Rover::Log_Write_Rangefinder() {}
 void Rover::Log_Write_Attitude() {}
 void Rover::Log_Write_RC(void) {}
 void Rover::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {}
-void Rover::Log_Write_Home_And_Origin() {}
 void Rover::Log_Arm_Disarm() {}
 void Rover::Log_Write_Error(uint8_t sub_system, uint8_t error_code) {}
 void Rover::Log_Write_Steering() {}

diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h
@@ -490,7 +490,6 @@ class Rover : public AP_HAL::HAL::Callbacks {
     void Log_Arm_Disarm();
     void Log_Write_RC(void);
     void Log_Write_Error(uint8_t sub_system, uint8_t error_code);
-    void Log_Write_Home_And_Origin();
     void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target);
     void Log_Write_WheelEncoder();
     void Log_Write_Proximity();

diff --git a/APMrover2/commands.cpp b/APMrover2/commands.cpp
@@ -72,7 +72,7 @@ bool Rover::set_home(const Location& loc, bool lock)
     mission.write_home_to_storage();
 
     // 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);
@@ -108,7 +108,7 @@ void Rover::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);
@@ -148,7 +148,7 @@ void Rover::update_home()
         if (ahrs.get_position(loc)) {
             if (get_distance(loc, ahrs.get_home()) > DISTANCE_HOME_MAX) {
                 ahrs.set_home(loc);
-                Log_Write_Home_And_Origin();
+                ahrs.Log_Write_Home_And_Origin();
                 gcs().send_home(gps.location());
             }
         }