AC_PrecLand: reduce code duplication, move common functionally to the…

… base class

libraries/AC_PrecLand/AC_PrecLand_Backend.h

@@ -15,7 +15,11 @@ class AC_PrecLand_Backend
     // Constructor
     AC_PrecLand_Backend(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state) :
         _frontend(frontend),
-        _state(state) {}
+        _state(state),
+        _los_meas_body(Vector3f(0,  0,  0)), // Assuming Vector3f has a constructor that takes three floats
+        _los_meas_time_ms(0),
+        _have_los_meas(false),
+        _distance_to_target(0.0f) {}
 
     // destructor
     virtual ~AC_PrecLand_Backend() {}
@@ -28,16 +32,22 @@ class AC_PrecLand_Backend
 
     // 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;
+    virtual 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;
+    virtual 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;
+    virtual 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; };
+    virtual 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 +58,11 @@ class AC_PrecLand_Backend
 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 camera
+    float               _distance_to_target;    // distance from the sensor to landing target in meters
 };
 
 #endif // AC_PRECLAND_ENABLED

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;

libraries/AC_PrecLand/AC_PrecLand_Companion.h

@@ -26,28 +26,10 @@ class AC_PrecLand_Companion : public AC_PrecLand_Backend
     // 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;
 };
 

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

libraries/AC_PrecLand/AC_PrecLand_IRLock.h

@@ -31,25 +31,12 @@ class AC_PrecLand_IRLock : public AC_PrecLand_Backend
     // 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

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

libraries/AC_PrecLand/AC_PrecLand_SITL.h

@@ -25,25 +25,8 @@ class AC_PrecLand_SITL : public AC_PrecLand_Backend
     // 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

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

libraries/AC_PrecLand/AC_PrecLand_SITL_Gazebo.h

@@ -26,22 +26,8 @@ class AC_PrecLand_SITL_Gazebo : public AC_PrecLand_Backend
     // 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