diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index f9e67b6094..b7d36e7e81 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand.cpp @@ -3,6 +3,7 @@ #include "AC_PrecLand_Backend.h" #include "AC_PrecLand_Companion.h" #include "AC_PrecLand_IRLock.h" +#include "AC_PrecLand_SITL_Gazebo.h" extern const AP_HAL::HAL& hal; @@ -17,7 +18,7 @@ const AP_Param::GroupInfo AC_PrecLand::var_info[] = { // @Param: TYPE // @DisplayName: Precision Land Type // @Description: Precision Land Type - // @Values: 0:None, 1:CompanionComputer, 2:IRLock + // @Values: 0:None, 1:CompanionComputer, 2:IRLock, 3:SITL_Gazebo // @User: Advanced AP_GROUPINFO("TYPE", 1, AC_PrecLand, _type, 0), @@ -93,10 +94,15 @@ void AC_PrecLand::init() _backend = new AC_PrecLand_Companion(*this, _backend_state); break; // IR Lock -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN || CONFIG_HAL_BOARD == HAL_BOARD_SITL +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN case PRECLAND_TYPE_IRLOCK: _backend = new AC_PrecLand_IRLock(*this, _backend_state); break; +#endif +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + case PRECLAND_TYPE_SITL_GAZEBO: + _backend = new AC_PrecLand_SITL_Gazebo(*this, _backend_state); + break; #endif } diff --git a/libraries/AC_PrecLand/AC_PrecLand.h b/libraries/AC_PrecLand/AC_PrecLand.h index f759a67917..18f2f71aad 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.h +++ b/libraries/AC_PrecLand/AC_PrecLand.h @@ -12,6 +12,7 @@ class AC_PrecLand_Backend; class AC_PrecLand_Companion; class AC_PrecLand_IRLock; +class AC_PrecLand_SITL_Gazebo; class AC_PrecLand { @@ -19,6 +20,7 @@ class AC_PrecLand friend class AC_PrecLand_Backend; friend class AC_PrecLand_Companion; friend class AC_PrecLand_IRLock; + friend class AC_PrecLand_SITL_Gazebo; public: @@ -33,7 +35,8 @@ public: enum PrecLandType { PRECLAND_TYPE_NONE = 0, PRECLAND_TYPE_COMPANION, - PRECLAND_TYPE_IRLOCK + PRECLAND_TYPE_IRLOCK, + PRECLAND_TYPE_SITL_GAZEBO, }; // constructor diff --git a/libraries/AC_PrecLand/AC_PrecLand_IRLock.h b/libraries/AC_PrecLand/AC_PrecLand_IRLock.h index 0759d85523..e1fec64f4e 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_IRLock.h +++ b/libraries/AC_PrecLand/AC_PrecLand_IRLock.h @@ -41,11 +41,7 @@ public: bool have_los_meas(); private: -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL - AP_IRLock_SITL irlock; -#else AP_IRLock_PX4 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 diff --git a/libraries/AC_PrecLand/AC_PrecLand_SITL_Gazebo.cpp b/libraries/AC_PrecLand/AC_PrecLand_SITL_Gazebo.cpp new file mode 100644 index 0000000000..cc4e148d0f --- /dev/null +++ b/libraries/AC_PrecLand/AC_PrecLand_SITL_Gazebo.cpp @@ -0,0 +1,58 @@ +#include +#include "AC_PrecLand_SITL_Gazebo.h" + +extern const AP_HAL::HAL& hal; + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + +// Constructor +AC_PrecLand_SITL_Gazebo::AC_PrecLand_SITL_Gazebo(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state) + : AC_PrecLand_Backend(frontend, state), + irlock() +{ +} + +// init - perform initialisation of this backend +void AC_PrecLand_SITL_Gazebo::init() +{ + irlock.init(); +} + +// update - give chance to driver to get updates from sensor +void AC_PrecLand_SITL_Gazebo::update() +{ + // update health + _state.healthy = irlock.healthy(); + + // get new sensor data + irlock.update(); + + if (irlock.num_targets() > 0 && irlock.last_update_ms() != _los_meas_time_ms) { + irlock.get_unit_vector_body(_los_meas_body); + _have_los_meas = true; + _los_meas_time_ms = irlock.last_update_ms(); + } + _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; +} + +// returns system time in milliseconds of last los measurement +uint32_t AC_PrecLand_SITL_Gazebo::los_meas_time_ms() { + return _los_meas_time_ms; +} + +// return true if there is a valid los measurement available +bool AC_PrecLand_SITL_Gazebo::have_los_meas() { + return _have_los_meas; +} + +#endif diff --git a/libraries/AC_PrecLand/AC_PrecLand_SITL_Gazebo.h b/libraries/AC_PrecLand/AC_PrecLand_SITL_Gazebo.h new file mode 100644 index 0000000000..207b04bf32 --- /dev/null +++ b/libraries/AC_PrecLand/AC_PrecLand_SITL_Gazebo.h @@ -0,0 +1,45 @@ +#pragma once + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL +#include +#include +#include +#include + +/* + * AC_PrecLand_SITL_Gazebo - implements precision landing using target + * vectors provided Gazebo via a network socket + */ + +class AC_PrecLand_SITL_Gazebo : public AC_PrecLand_Backend +{ +public: + + // Constructor + AC_PrecLand_SITL_Gazebo(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state); + + // perform any required initialisation of backend + void init(); + + // retrieve updates from sensor + void update(); + + // provides a unit vector towards the target in body frame + // returns same as have_los_meas() + bool get_los_body(Vector3f& ret); + + // returns system time in milliseconds of last los measurement + uint32_t los_meas_time_ms(); + + // return true if there is a valid los measurement available + bool have_los_meas(); + +private: + AP_IRLock_SITL 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