diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index fa3afb8a5b..de3b844e5e 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/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); + } +} diff --git a/libraries/AC_PrecLand/AC_PrecLand.h b/libraries/AC_PrecLand/AC_PrecLand.h index 2ff623c6d8..86eb5b5979 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.h +++ b/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; } diff --git a/libraries/AC_PrecLand/AC_PrecLand_Backend.h b/libraries/AC_PrecLand/AC_PrecLand_Backend.h index 0d447ce7e2..ed75f3b3bf 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_Backend.h +++ b/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 diff --git a/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp b/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp index 8641b07fff..aeb8eaac7f 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp +++ b/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; +} diff --git a/libraries/AC_PrecLand/AC_PrecLand_Companion.h b/libraries/AC_PrecLand/AC_PrecLand_Companion.h index 65b5545a77..6e48991c00 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_Companion.h +++ b/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); -private: + // handle_msg - parses a mavlink message from the companion computer + void handle_msg(mavlink_message_t* msg); - mavlink_channel_t _chan; // mavlink channel used to communicate with companion computer +private: + // 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__ diff --git a/libraries/AC_PrecLand/AC_PrecLand_IRLock.h b/libraries/AC_PrecLand/AC_PrecLand_IRLock.h index 381cf0b90f..16964c83fa 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_IRLock.h +++ b/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;