From f0fc447aed925cebe283344b1bc670aa7803606e Mon Sep 17 00:00:00 2001
From: "Dr.-Ing. Amilcar do Carmo Lucas" <amilcar.lucas@iav.de>
Date: Thu, 22 Feb 2024 15:08:29 +0100
Subject: [PATCH] AC_PrecLand: reduce code duplication, move common
 functionally to the base class

---
 libraries/AC_PrecLand/AC_PrecLand_Backend.h   | 19 +++++++++++++++----
 .../AC_PrecLand/AC_PrecLand_Companion.cpp     | 11 -----------
 libraries/AC_PrecLand/AC_PrecLand_Companion.h | 18 ------------------
 libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp  | 10 ----------
 libraries/AC_PrecLand/AC_PrecLand_IRLock.h    | 15 +--------------
 libraries/AC_PrecLand/AC_PrecLand_SITL.cpp    | 10 ----------
 libraries/AC_PrecLand/AC_PrecLand_SITL.h      | 17 -----------------
 .../AC_PrecLand/AC_PrecLand_SITL_Gazebo.cpp   | 10 ----------
 .../AC_PrecLand/AC_PrecLand_SITL_Gazebo.h     | 14 --------------
 9 files changed, 16 insertions(+), 108 deletions(-)

diff --git a/libraries/AC_PrecLand/AC_PrecLand_Backend.h b/libraries/AC_PrecLand/AC_PrecLand_Backend.h
index 41facd0deb..869ce7eb16 100644
--- a/libraries/AC_PrecLand/AC_PrecLand_Backend.h
+++ b/libraries/AC_PrecLand/AC_PrecLand_Backend.h
@@ -28,16 +28,22 @@ public:
 
     // provides a unit vector towards the target in body frame
     //  returns same as have_los_meas()
-    virtual bool get_los_body(Vector3f& dir_body) = 0;
+    bool get_los_body(Vector3f& dir_body) {
+        if (have_los_meas()) {
+            dir_body = _los_meas_body;
+            return true;
+        }
+        return false;
+    };
 
     // returns system time in milliseconds of last los measurement
-    virtual uint32_t los_meas_time_ms() = 0;
+    uint32_t los_meas_time_ms() { return _los_meas_time_ms; };
 
     // return true if there is a valid los measurement available
-    virtual bool have_los_meas() = 0;
+    bool have_los_meas() { return _have_los_meas; };
 
     // returns distance to target in meters (0 means distance is not known)
-    virtual float distance_to_target() { return 0.0f; };
+    float distance_to_target() { return _distance_to_target; };
 
     // parses a mavlink message from the companion computer
     virtual void handle_msg(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) {};
@@ -48,6 +54,11 @@ public:
 protected:
     const AC_PrecLand&  _frontend;          // reference to precision landing front end
     AC_PrecLand::precland_state &_state;    // reference to this instances state
+
+    Vector3f            _los_meas_body;         // unit vector in body frame pointing towards target
+    uint32_t            _los_meas_time_ms;      // system time in milliseconds when los was measured
+    bool                _have_los_meas;         // true if there is a valid measurement from the sensor
+    float               _distance_to_target;    // distance from the sensor to landing target in meters
 };
 
 #endif // AC_PRECLAND_ENABLED
diff --git a/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp b/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp
index 1183626467..e55bf22315 100644
--- a/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp
+++ b/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp
@@ -11,7 +11,6 @@ void AC_PrecLand_Companion::init()
 {
     // set healthy
     _state.healthy = true;
-    _have_los_meas = false;
 }
 
 // retrieve updates from sensor
@@ -20,16 +19,6 @@ void AC_PrecLand_Companion::update()
     _have_los_meas = _have_los_meas && AP_HAL::millis()-_los_meas_time_ms <= 1000;
 }
 
-// provides a unit vector towards the target in body frame
-//  returns same as have_los_meas()
-bool AC_PrecLand_Companion::get_los_body(Vector3f& ret) {
-    if (have_los_meas()) {
-        ret = _los_meas_body;
-        return true;
-    }
-    return false;
-}
-
 void AC_PrecLand_Companion::handle_msg(const mavlink_landing_target_t &packet, uint32_t timestamp_ms)
 {
     _distance_to_target = packet.distance;
diff --git a/libraries/AC_PrecLand/AC_PrecLand_Companion.h b/libraries/AC_PrecLand/AC_PrecLand_Companion.h
index ff140b3c16..2f36267e36 100644
--- a/libraries/AC_PrecLand/AC_PrecLand_Companion.h
+++ b/libraries/AC_PrecLand/AC_PrecLand_Companion.h
@@ -26,28 +26,10 @@ public:
     // retrieve updates from sensor
     void update() override;
 
-    // provides a unit vector towards the target in body frame
-    //  returns same as have_los_meas()
-    bool get_los_body(Vector3f& ret) override;
-
-    // returns system time in milliseconds of last los measurement
-    uint32_t los_meas_time_ms() override { return _los_meas_time_ms; }
-
-    // return true if there is a valid los measurement available
-    bool have_los_meas() override { return _have_los_meas; }
-
-    // returns distance to target in meters (0 means distance is not known)
-    float distance_to_target() override { return _distance_to_target; }
-
     // parses a mavlink message from the companion computer
     void handle_msg(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) override;
 
 private:
-    float               _distance_to_target;    // distance from the camera to target in meters
-
-    Vector3f            _los_meas_body;         // unit vector in body frame pointing towards target
-    bool                _have_los_meas;         // true if there is a valid measurement from the camera
-    uint32_t            _los_meas_time_ms;      // system time in milliseconds when los was measured
     bool                _wrong_frame_msg_sent;
 };
 
diff --git a/libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp b/libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp
index ac6c8373e7..226dd1551e 100644
--- a/libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp
+++ b/libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp
@@ -35,14 +35,4 @@ void AC_PrecLand_IRLock::update()
     _have_los_meas = _have_los_meas && AP_HAL::millis()-_los_meas_time_ms <= 1000;
 }
 
-// provides a unit vector towards the target in body frame
-//  returns same as have_los_meas()
-bool AC_PrecLand_IRLock::get_los_body(Vector3f& ret) {
-    if (have_los_meas()) {
-        ret = _los_meas_body;
-        return true;
-    }
-    return false;
-}
-
 #endif // AC_PRECLAND_IRLOCK_ENABLED
diff --git a/libraries/AC_PrecLand/AC_PrecLand_IRLock.h b/libraries/AC_PrecLand/AC_PrecLand_IRLock.h
index c7c975c2e7..b54d3d53d5 100644
--- a/libraries/AC_PrecLand/AC_PrecLand_IRLock.h
+++ b/libraries/AC_PrecLand/AC_PrecLand_IRLock.h
@@ -15,7 +15,7 @@
 
 /*
  * AC_PrecLand_IRLock - implements precision landing using target vectors provided
- *                         by a companion computer (i.e. Odroid) communicating via MAVLink
+ *                         by an IRLock
  */
 
 class AC_PrecLand_IRLock : public AC_PrecLand_Backend
@@ -31,25 +31,12 @@ public:
     // retrieve updates from sensor
     void update() override;
 
-    // provides a unit vector towards the target in body frame
-    //  returns same as have_los_meas()
-    bool get_los_body(Vector3f& ret) override;
-
-    // returns system time in milliseconds of last los measurement
-    uint32_t los_meas_time_ms() override { return _los_meas_time_ms; }
-
-    // return true if there is a valid los measurement available
-    bool have_los_meas() override { return _have_los_meas; }
-
 private:
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     AP_IRLock_SITL irlock;
 #else
     AP_IRLock_I2C irlock;
 #endif
-    Vector3f            _los_meas_body;         // unit vector in body frame pointing towards target
-    bool                _have_los_meas;         // true if there is a valid measurement from the camera
-    uint32_t            _los_meas_time_ms;      // system time in milliseconds when los was measured
 };
 
 #endif // AC_PRECLAND_IRLOCK_ENABLED
diff --git a/libraries/AC_PrecLand/AC_PrecLand_SITL.cpp b/libraries/AC_PrecLand/AC_PrecLand_SITL.cpp
index c8394948d1..fc7f48057d 100644
--- a/libraries/AC_PrecLand/AC_PrecLand_SITL.cpp
+++ b/libraries/AC_PrecLand/AC_PrecLand_SITL.cpp
@@ -39,14 +39,4 @@ void AC_PrecLand_SITL::update()
     _have_los_meas = _have_los_meas && AP_HAL::millis() - _los_meas_time_ms <= 1000;
 }
 
-// provides a unit vector towards the target in body frame
-//  returns same as have_los_meas()
-bool AC_PrecLand_SITL::get_los_body(Vector3f& ret) {
-    if (have_los_meas()) {
-        ret = _los_meas_body;
-        return true;
-    }
-    return false;
-}
-
 #endif  // AC_PRECLAND_SITL_ENABLED
diff --git a/libraries/AC_PrecLand/AC_PrecLand_SITL.h b/libraries/AC_PrecLand/AC_PrecLand_SITL.h
index 736adfbdb1..0bfb948cab 100644
--- a/libraries/AC_PrecLand/AC_PrecLand_SITL.h
+++ b/libraries/AC_PrecLand/AC_PrecLand_SITL.h
@@ -25,25 +25,8 @@ public:
     // retrieve updates from sensor
     void update() override;
 
-    // provides a unit vector towards the target in body frame
-    //  returns same as have_los_meas()
-    bool get_los_body(Vector3f& ret) override;
-
-    // returns system time in milliseconds of last los measurement
-    uint32_t los_meas_time_ms() override { return _los_meas_time_ms; }
-
-    // return true if there is a valid los measurement available
-    bool have_los_meas() override { return _have_los_meas; }
-
-    // returns distance to target in meters (0 means distance is not known)
-    float distance_to_target() override { return _distance_to_target; }
-
 private:
     SITL::SIM           *_sitl;                 // sitl instance pointer
-    Vector3f            _los_meas_body;         // unit vector in body frame pointing towards target
-    uint32_t            _los_meas_time_ms;      // system time in milliseconds when los was measured
-    bool                _have_los_meas;         // true if there is a valid measurement from the camera
-    float               _distance_to_target;    // distance to target in meters
 };
 
 #endif  // AC_PRECLAND_SITL_ENABLED
diff --git a/libraries/AC_PrecLand/AC_PrecLand_SITL_Gazebo.cpp b/libraries/AC_PrecLand/AC_PrecLand_SITL_Gazebo.cpp
index 7bcae4ef09..8177043a26 100644
--- a/libraries/AC_PrecLand/AC_PrecLand_SITL_Gazebo.cpp
+++ b/libraries/AC_PrecLand/AC_PrecLand_SITL_Gazebo.cpp
@@ -35,14 +35,4 @@ void AC_PrecLand_SITL_Gazebo::update()
     _have_los_meas = _have_los_meas && AP_HAL::millis()-_los_meas_time_ms <= 1000;
 }
 
-// provides a unit vector towards the target in body frame
-//  returns same as have_los_meas()
-bool AC_PrecLand_SITL_Gazebo::get_los_body(Vector3f& ret) {
-    if (have_los_meas()) {
-        ret = _los_meas_body;
-        return true;
-    }
-    return false;
-}
-
 #endif  // AC_PRECLAND_SITL_GAZEBO_ENABLED
diff --git a/libraries/AC_PrecLand/AC_PrecLand_SITL_Gazebo.h b/libraries/AC_PrecLand/AC_PrecLand_SITL_Gazebo.h
index e66b73a688..fb4458461e 100644
--- a/libraries/AC_PrecLand/AC_PrecLand_SITL_Gazebo.h
+++ b/libraries/AC_PrecLand/AC_PrecLand_SITL_Gazebo.h
@@ -26,22 +26,8 @@ public:
     // retrieve updates from sensor
     void update() override;
 
-    // provides a unit vector towards the target in body frame
-    //  returns same as have_los_meas()
-    bool get_los_body(Vector3f& ret) override;
-
-    // returns system time in milliseconds of last los measurement
-    uint32_t los_meas_time_ms() override { return _los_meas_time_ms; }
-
-    // return true if there is a valid los measurement available
-    bool have_los_meas() override { return _have_los_meas; }
-
 private:
     AP_IRLock_SITL_Gazebo irlock;
-
-    Vector3f            _los_meas_body;         // unit vector in body frame pointing towards target
-    bool                _have_los_meas;         // true if there is a valid measurement from the camera
-    uint32_t            _los_meas_time_ms;      // system time in milliseconds when los was measured
 };
 
 #endif  // AC_PRECLAND_SITL_GAZEBO_ENABLED