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