AC_PrecLand: add companion computer implementation

libraries/AC_PrecLand/AC_PrecLand.cpp

@@ -159,3 +159,12 @@ void AC_PrecLand::calc_angles_and_pos(float alt_above_terrain_cm)
 
     _have_estimate = true;
 }
+
+// handle_msg - Process a LANDING_TARGET mavlink message
+void AC_PrecLand::handle_msg(mavlink_message_t* msg)
+{
+    // run backend update
+    if (_backend != NULL) {
+        _backend->handle_msg(msg);
+    }
+}

libraries/AC_PrecLand/AC_PrecLand.h

@@ -57,6 +57,9 @@ public:
     // get_target_shift - returns 3D vector of earth-frame position adjustments to target
     Vector3f get_target_shift(const Vector3f& orig_target);
 
+    // handle_msg - Process a LANDING_TARGET mavlink message
+    void handle_msg(mavlink_message_t* msg);
+
     // accessors for logging
     bool enabled() const { return _enabled; }
     const Vector2f& last_bf_angle_to_target() const { return _bf_angle_to_target; }

libraries/AC_PrecLand/AC_PrecLand_Backend.h

@@ -33,6 +33,9 @@ public:
     //  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) = 0;
 
+    // handle_msg - parses a mavlink message from the companion computer
+    virtual void handle_msg(mavlink_message_t* msg) = 0;
+
 protected:
 
     const AC_PrecLand&  _frontend;          // reference to precision landing front end

libraries/AC_PrecLand/AC_PrecLand_Companion.cpp

@@ -6,8 +6,7 @@ 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),
-  _chan(MAVLINK_COMM_0)
+    : AC_PrecLand_Backend(frontend, state)
 {
 }
 
@@ -16,21 +15,45 @@ void AC_PrecLand_Companion::init()
 {
     // set healthy
     _state.healthy = true;
+    _new_estimate = false;
 }
 
 // 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;
+    // Mavlink commands are received asynchronous so all new data is processed by handle_msg()
+    return _new_estimate;
 }
 
 // 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
+bool AC_PrecLand_Companion::get_angle_to_target(float &x_angle_rad, float &y_angle_rad)
 {
+    if (_new_estimate){
+        x_angle_rad = _bf_angle_to_target.x;
+        y_angle_rad = _bf_angle_to_target.y;
+
+        // reset and wait for new data
+        _new_estimate = false;
+        return true;
+    }
+
     return false;
 }
+
+void AC_PrecLand_Companion::handle_msg(mavlink_message_t* msg)
+{
+    // parse mavlink message
+    __mavlink_landing_target_t packet;
+    mavlink_msg_landing_target_decode(msg, &packet);
+
+    _timestamp_us = packet.time_usec;
+    _bf_angle_to_target.x = packet.angle_x;
+    _bf_angle_to_target.y = packet.angle_y;
+    _distance_to_target = packet.distance;
+    _state.healthy = true;
+    _new_estimate = true;
+}

libraries/AC_PrecLand/AC_PrecLand_Companion.h

@@ -29,11 +29,17 @@ public:
     //  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;
+    bool get_angle_to_target(float &x_angle_rad, float &y_angle_rad);
+
+    // handle_msg - parses a mavlink message from the companion computer
+    void handle_msg(mavlink_message_t* msg);
 
 private:
 
-    mavlink_channel_t   _chan;      // mavlink channel used to communicate with companion computer
-
+    // output from camera
+    Vector2f            _bf_angle_to_target;    // last body-frame angle to target
+    float               _distance_to_target;    // distance from the camera to target in meters
+    uint64_t            _timestamp_us;          // timestamp when the image was captured(synced via UAVCAN)
+    bool                _new_estimate;          // true if new data from the camera has been received
 };
 #endif	// __AC_PRECLAND_COMPANION_H__

libraries/AC_PrecLand/AC_PrecLand_IRLock.h

@@ -35,6 +35,9 @@ public:
     //  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);
 
+    // handle_msg - parses a mavlink message from the companion computer
+    void handle_msg(mavlink_message_t* msg) { /* do nothing */ }
+
 private:
     AP_IRLock_PX4 irlock;