diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp new file mode 100644 index 0000000000..3bbc104c9e --- /dev/null +++ b/libraries/AC_PrecLand/AC_PrecLand.cpp @@ -0,0 +1,157 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include +#include +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +const AP_Param::GroupInfo AC_PrecLand::var_info[] PROGMEM = { + // @DisplayName: Precision Land enabled/disabled and behaviour + // @Description: Precision Land enabled/disabled and behaviour + // @Values: 0:Disabled, 1:Enabled Always Land, 2:Enabled Strict + // @User: Advanced + AP_GROUPINFO("ENABLED", 0, AC_PrecLand, _enabled, 0), + + // @Param: TYPE + // @DisplayName: Precision Land Type + // @Description: Precision Land Type + // @Values: 0:None, 1:CompanionComputer, 2:IRLock + // @User: Advanced + AP_GROUPINFO("TYPE", 1, AC_PrecLand, _type, 0), + + // @Param: SPEED + // @DisplayName: Precision Land horizontal speed maximum in cm/s + // @Description: Precision Land horizontal speed maximum in cm/s + // @Range: 0 500 + // @User: Advanced + AP_GROUPINFO("SPEED", 2, AC_PrecLand, _speed_xy, AC_PRECLAND_SPEED_XY_DEFAULT), + + AP_GROUPEND +}; + +// Default constructor. +// Note that the Vector/Matrix constructors already implicitly zero +// their values. +// +AC_PrecLand::AC_PrecLand(const AP_AHRS& ahrs, const AP_InertialNav& inav, + AC_PI_2D& pi_precland_xy, float dt) : + _ahrs(ahrs), + _inav(inav), + _pi_precland_xy(pi_precland_xy), + _dt(dt), + _have_estimate(false), + _backend(NULL) +{ + // set parameters to defaults + AP_Param::setup_object_defaults(this, var_info); + + // other initialisation + _backend_state.healthy = false; +} + + +// init - perform any required initialisation of backends +void AC_PrecLand::init() +{ + // exit immediately if init has already been run + if (_backend != NULL) { + return; + } + + // default health to false + _backend = NULL; + _backend_state.healthy = false; + + // instantiate backend based on type parameter + switch ((enum PrecLandType)(_type.get())) { + // no type defined + case PRECLAND_TYPE_NONE: + default: + return; + // companion computer + case PRECLAND_TYPE_COMPANION: + _backend = new AC_PrecLand_Companion(*this, _backend_state); + break; + // IR Lock + case PRECLAND_TYPE_IRLOCK: + _backend = new AC_PrecLand_IRLock(*this, _backend_state); + break; + } + + // init backend + if (_backend != NULL) { + _backend->init(); + } +} + +// update - give chance to driver to get updates from sensor +void AC_PrecLand::update(float alt_above_terrain_cm) +{ + // run backend update + if (_backend != NULL) { + // read from sensor + _backend->update(); + + // calculate angles to target and position estimate + calc_angles_and_pos(alt_above_terrain_cm); + } +} + +// get_target_shift - returns 3D vector of earth-frame position adjustments to target +Vector3f AC_PrecLand::get_target_shift(const Vector3f &orig_target) +{ + Vector3f shift; // default shift initialised to zero + + // do not shift target if not enabled or no position estimate + if (_backend == NULL || !_have_estimate) { + return shift; + } + + // shift is target_offset - (original target - current position) + Vector3f curr_offset_from_target = orig_target - _inav.get_position(); + shift = _target_pos_offset - curr_offset_from_target; + shift.z = 0.0f; + + // record we have consumed this reading (perhaps there is a cleaner way to do this using timestamps) + _have_estimate = false; + + // return adjusted target + return shift; +} + +// calc_angles_and_pos - converts sensor's body-frame angles to earth-frame angles and position estimate +// body-frame angles stored in _bf_angle_to_target +// earth-frame angles stored in _ef_angle_to_target +// position estimate is stored in _target_pos +void AC_PrecLand::calc_angles_and_pos(float alt_above_terrain_cm) +{ + // exit immediately if not enabled + if (_backend == NULL) { + _have_estimate = false; + } + + // get body-frame angles to target from backend + if (!_backend->get_angle_to_target(_bf_angle_to_target.x, _bf_angle_to_target.y)) { + _have_estimate = false; + } + + // subtract vehicle lean angles + float x_rad = _bf_angle_to_target.x - _ahrs.roll; + float y_rad = -_bf_angle_to_target.y + _ahrs.pitch; + + // rotate to earth-frame angles + _ef_angle_to_target.x = y_rad*_ahrs.cos_yaw() - x_rad*_ahrs.sin_yaw(); + _ef_angle_to_target.y = y_rad*_ahrs.sin_yaw() + x_rad*_ahrs.cos_yaw(); + + // get current altitude (constrained to no lower than 50cm) + float alt = max(alt_above_terrain_cm, 50.0f); + + // convert earth-frame angles to earth-frame position offset + _target_pos_offset.x = alt*tanf(_ef_angle_to_target.x); + _target_pos_offset.y = alt*tanf(_ef_angle_to_target.y); + _target_pos_offset.z = 0; // not used + + _have_estimate = true; +} diff --git a/libraries/AC_PrecLand/AC_PrecLand.h b/libraries/AC_PrecLand/AC_PrecLand.h new file mode 100644 index 0000000000..22f7e2f59f --- /dev/null +++ b/libraries/AC_PrecLand/AC_PrecLand.h @@ -0,0 +1,107 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#ifndef __AC_PRECLAND_H__ +#define __AC_PRECLAND_H__ + +#include +#include +#include // PID library +#include // Inertial Navigation library + +// definitions +#define AC_PRECLAND_SPEED_XY_DEFAULT 100.0f // maximum horizontal speed +#define PRECLAND_P 2.0f // velocity controller P gain default +#define PRECLAND_I 1.0f // velocity controller I gain default +#define PRECLAND_IMAX 500.0f // velocity controller IMAX default +#define PRECLAND_UPDATE_TIME 0.02f // precland runs at 50hz + +// declare backend classes +class AC_PrecLand_Backend; +class AC_PrecLand_Companion; +class AC_PrecLand_IRLock; + +class AC_PrecLand +{ + // declare backends as friends + friend class AC_PrecLand_Backend; + friend class AC_PrecLand_Companion; + friend class AC_PrecLand_IRLock; + +public: + + // precision landing behaviours (held in PRECLAND_ENABLED parameter) + enum PrecLandBehaviour { + PRECLAND_BEHAVIOUR_DISABLED, + PRECLAND_BEHAVIOR_ALWAYSLAND, + PRECLAND_BEHAVIOR_CAUTIOUS + }; + + // types of precision landing (used for PRECLAND_TYPE parameter) + enum PrecLandType { + PRECLAND_TYPE_NONE = 0, + PRECLAND_TYPE_COMPANION, + PRECLAND_TYPE_IRLOCK + }; + + // Constructor + AC_PrecLand(const AP_AHRS& ahrs, const AP_InertialNav& inav, AC_PI_2D& pi_precland_xy, float dt); + + // init - perform any required initialisation of landing controllers + void init(); + + // healthy - returns true if precision landing is healthy + bool healthy() { return _backend_state.healthy; } + + // update - give chance to driver to get updates from sensor + void update(float alt_above_terrain_cm); + + // get_target_shift - returns 3D vector of earth-frame position adjustments to target + Vector3f get_target_shift(const Vector3f& orig_target); + + // accessors for logging + bool enabled() const { return _enabled; } + const Vector2f& last_bf_angle_to_target() const { return _bf_angle_to_target; } + const Vector2f& last_ef_angle_to_target() const { return _ef_angle_to_target; } + const Vector3f& last_target_pos_offset() const { return _target_pos_offset; } + + // parameter var table + static const struct AP_Param::GroupInfo var_info[]; + +private: + + // calc_angles_and_pos - converts sensor's body-frame angles to earth-frame angles and position estimate + // body-frame angles stored in _bf_angle_to_target + // earth-frame angles stored in _ef_angle_to_target + // position estimate is stored in _target_pos + void calc_angles_and_pos(float alt_above_terrain_cm); + + // get_behaviour - returns enabled parameter as an behaviour + enum PrecLandBehaviour get_behaviour() const { return (enum PrecLandBehaviour)(_enabled.get()); } + + // references to inertial nav and ahrs libraries + const AP_AHRS& _ahrs; + const AP_InertialNav& _inav; + AC_PI_2D& _pi_precland_xy; // horizontal velocity PI controller + + // parameters + AP_Int8 _enabled; // enabled/disabled and behaviour + AP_Int8 _type; // precision landing controller type + AP_Float _speed_xy; // maximum horizontal speed in cm/s + + // internal variables + float _dt; // time difference (in seconds) between calls from the main program + + // output from sensor (stored for logging) + Vector2f _bf_angle_to_target;// last body-frame angle to target + Vector2f _ef_angle_to_target;// last earth-frame angle to target + + // output from controller + bool _have_estimate; // true if we have a recent estimated position offset + Vector3f _target_pos_offset; // estimate target position offset from vehicle in earth-frame + + // backend state + struct precland_state { + bool healthy; + } _backend_state; + AC_PrecLand_Backend *_backend; // pointers to backend precision landing driver +}; +#endif // __AC_PRECLAND_H__ diff --git a/libraries/AC_PrecLand/AC_PrecLand_Backend.h b/libraries/AC_PrecLand/AC_PrecLand_Backend.h new file mode 100644 index 0000000000..6a39da693a --- /dev/null +++ b/libraries/AC_PrecLand/AC_PrecLand_Backend.h @@ -0,0 +1,41 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#ifndef __AC_PRECLAND_BACKEND_H__ +#define __AC_PRECLAND_BACKEND_H__ + +#include +#include +#include // PID library +#include // Inertial Navigation library +#include // Precision Landing frontend + +class AC_PrecLand_Backend +{ +public: + + // Constructor + AC_PrecLand_Backend(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state) : + _frontend(frontend), + _state(state) {} + + // destructor + virtual ~AC_PrecLand_Backend() {} + + // init - perform any required initialisation of backend controller + virtual void init() = 0; + + // update - give chance to driver to get updates from sensor + // returns true if new data available + virtual bool update() = 0; + + // get_angle_to_target - returns body frame angles (in radians) to target + // returns true if angles are available, false if not (i.e. no target) + // x_angle_rad : body-frame roll direction, positive = target is to right (looking down) + // y_angle_rad : body-frame pitch direction, postiive = target is forward (looking down) + virtual bool get_angle_to_target(float &x_angle_rad, float &y_angle_rad) const = 0; + +protected: + + const AC_PrecLand& _frontend; // reference to precision landing front end + AC_PrecLand::precland_state &_state; // reference to this instances state +}; +#endif // __AC_PRECLAND_BACKEND_H__ diff --git a/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp b/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp new file mode 100644 index 0000000000..65d13891e7 --- /dev/null +++ b/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp @@ -0,0 +1,35 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include +#include + +extern const AP_HAL::HAL& hal; + +// Constructor +AC_PrecLand_Companion::AC_PrecLand_Companion(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state) +: AC_PrecLand_Backend(frontend, state) +{ +} + +// init - perform initialisation of this backend +void AC_PrecLand_Companion::init() +{ + // set healthy + _state.healthy = true; +} + +// update - give chance to driver to get updates from sensor +// returns true if new data available +bool AC_PrecLand_Companion::update() +{ + // To-Do: read target position from companion computer via MAVLink + return false; +} + +// get_angle_to_target - returns body frame angles (in radians) to target +// returns true if angles are available, false if not (i.e. no target) +// x_angle_rad : body-frame roll direction, positive = target is to right (looking down) +// y_angle_rad : body-frame pitch direction, postiive = target is forward (looking down) +bool AC_PrecLand_Companion::get_angle_to_target(float &x_angle_rad, float &y_angle_rad) const +{ + return false; +} diff --git a/libraries/AC_PrecLand/AC_PrecLand_Companion.h b/libraries/AC_PrecLand/AC_PrecLand_Companion.h new file mode 100644 index 0000000000..79cef3bccb --- /dev/null +++ b/libraries/AC_PrecLand/AC_PrecLand_Companion.h @@ -0,0 +1,39 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#ifndef __AC_PRECLAND_COMPANION_H__ +#define __AC_PRECLAND_COMPANION_H__ + +#include +#include +#include // Precision Landing backend + +/* + * AC_PrecLand_Companion - implements precision landing using target vectors provided + * by a companion computer (i.e. Odroid) communicating via MAVLink + */ + +class AC_PrecLand_Companion : public AC_PrecLand_Backend +{ +public: + + // Constructor + AC_PrecLand_Companion(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state); + + // init - perform any required initialisation of backend controller + void init(); + + // update - give chance to driver to get updates from sensor + // returns true if new data available + bool update(); + + // get_angle_to_target - returns body frame angles (in radians) to target + // returns true if angles are available, false if not (i.e. no target) + // x_angle_rad : body-frame roll direction, positive = target is to right (looking down) + // y_angle_rad : body-frame pitch direction, postiive = target is forward (looking down) + bool get_angle_to_target(float &x_angle_rad, float &y_angle_rad) const; + +private: + + mavlink_channel_t _chan; // mavlink channel used to communicate with companion computer + +}; +#endif // __AC_PRECLAND_COMPANION_H__ diff --git a/libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp b/libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp new file mode 100644 index 0000000000..0a89a82586 --- /dev/null +++ b/libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp @@ -0,0 +1,37 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#include +#include + +extern const AP_HAL::HAL& hal; + +// Constructor +AC_PrecLand_IRLock::AC_PrecLand_IRLock(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state) + : AC_PrecLand_Backend(frontend, state), + irlock() +{ +} + +// init - perform initialisation of this backend +void AC_PrecLand_IRLock::init() +{ + irlock.init(); + + // set healthy + _state.healthy = irlock.healthy(); +} + +// update - give chance to driver to get updates from sensor +bool AC_PrecLand_IRLock::update() +{ + // get new sensor data + return (irlock.update()); +} + +// get_angle_to_target - returns body frame angles (in radians) to target +// returns true if angles are available, false if not (i.e. no target) +// x_angle_rad : body-frame roll direction, positive = target is to right (looking down) +// y_angle_rad : body-frame pitch direction, postiive = target is forward (looking down) +bool AC_PrecLand_IRLock::get_angle_to_target(float &x_angle_rad, float &y_angle_rad) const +{ + return irlock.get_angle_to_target(x_angle_rad, y_angle_rad); +} diff --git a/libraries/AC_PrecLand/AC_PrecLand_IRLock.h b/libraries/AC_PrecLand/AC_PrecLand_IRLock.h new file mode 100644 index 0000000000..6e0f7e480e --- /dev/null +++ b/libraries/AC_PrecLand/AC_PrecLand_IRLock.h @@ -0,0 +1,39 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#ifndef __AC_PRECLAND_IRLOCK_H__ +#define __AC_PRECLAND_IRLOCK_H__ + +#include +#include +#include // Precision Landing backend +#include + +/* + * AC_PrecLand_IRLock - implements precision landing using target vectors provided + * by a companion computer (i.e. Odroid) communicating via MAVLink + */ + +class AC_PrecLand_IRLock : public AC_PrecLand_Backend +{ +public: + + // Constructor + AC_PrecLand_IRLock(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state); + + // init - perform any required initialisation of backend controller + void init(); + + // update - give chance to driver to get updates from sensor + // returns true if new data available + bool update(); + + // get_angle_to_target - returns body frame angles (in radians) to target + // returns true if angles are available, false if not (i.e. no target) + // x_angle_rad : body-frame roll direction, positive = target is to right (looking down) + // y_angle_rad : body-frame pitch direction, postiive = target is forward (looking down) + bool get_angle_to_target(float &x_angle_rad, float &y_angle_rad) const; + +private: + AP_IRLock_PX4 irlock; + +}; +#endif // __AC_PRECLAND_IRLOCK_H__