AC_PrecLand: precision landing lib for IR-Lock and companion computer

libraries/AC_PrecLand/AC_PrecLand.cpp

@@ -0,0 +1,157 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+#include <AP_HAL.h>
+#include <AC_PrecLand.h>
+#include <AC_PrecLand_Backend.h>
+#include <AC_PrecLand_Companion.h>
+#include <AC_PrecLand_IRLock.h>
+
+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;
+}

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 <AP_Common.h>
+#include <AP_Math.h>
+#include <AC_PI_2D.h>             // PID library
+#include <AP_InertialNav.h>     // 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__

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 <AP_Common.h>
+#include <AP_Math.h>
+#include <AC_PID.h>             // PID library
+#include <AP_InertialNav.h>     // Inertial Navigation library
+#include <AC_PrecLand.h>        // 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__

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 <AP_HAL.h>
+#include <AC_PrecLand_Companion.h>
+
+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;
+}

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 <AP_Common.h>
+#include <AP_Math.h>
+#include <AC_PrecLand_Backend.h>    // 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__

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 <AP_HAL.h>
+#include <AC_PrecLand_IRLock.h>
+
+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);
+}

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 <AP_Common.h>
+#include <AP_Math.h>
+#include <AC_PrecLand_Backend.h>    // Precision Landing backend
+#include <AP_IRLock.h>
+
+/*
+ * 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__