From 1fe1e532607de07a38ff9eed44dbb3ae9d333a64 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 13 Apr 2021 12:49:59 +1000 Subject: [PATCH] AC_PrecLand: pass in time corrected LANDING_TARGET msg should make companion based precision landing more accurate --- libraries/AC_PrecLand/AC_PrecLand.cpp | 4 ++-- libraries/AC_PrecLand/AC_PrecLand.h | 2 +- libraries/AC_PrecLand/AC_PrecLand_Backend.h | 2 +- libraries/AC_PrecLand/AC_PrecLand_Companion.cpp | 8 ++------ libraries/AC_PrecLand/AC_PrecLand_Companion.h | 2 +- 5 files changed, 7 insertions(+), 11 deletions(-) diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index f76586ac4a..036e651f61 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand.cpp @@ -263,11 +263,11 @@ bool AC_PrecLand::get_target_velocity_relative_cms(Vector2f& ret) } // handle_msg - Process a LANDING_TARGET mavlink message -void AC_PrecLand::handle_msg(const mavlink_message_t &msg) +void AC_PrecLand::handle_msg(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) { // run backend update if (_backend != nullptr) { - _backend->handle_msg(msg); + _backend->handle_msg(packet, timestamp_ms); } } diff --git a/libraries/AC_PrecLand/AC_PrecLand.h b/libraries/AC_PrecLand/AC_PrecLand.h index f8eb4fe686..bbeec77df4 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.h +++ b/libraries/AC_PrecLand/AC_PrecLand.h @@ -86,7 +86,7 @@ public: bool target_acquired(); // process a LANDING_TARGET mavlink message - void handle_msg(const mavlink_message_t &msg); + void handle_msg(const mavlink_landing_target_t &packet, uint32_t timestamp_ms); // parameter var table static const struct AP_Param::GroupInfo var_info[]; diff --git a/libraries/AC_PrecLand/AC_PrecLand_Backend.h b/libraries/AC_PrecLand/AC_PrecLand_Backend.h index d6eeb33a45..d816a85d8d 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_Backend.h +++ b/libraries/AC_PrecLand/AC_PrecLand_Backend.h @@ -35,7 +35,7 @@ public: virtual float distance_to_target() { return 0.0f; }; // parses a mavlink message from the companion computer - virtual void handle_msg(const mavlink_message_t &msg) {}; + virtual void handle_msg(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) {}; // get bus parameter int8_t get_bus(void) const { return _frontend._bus.get(); } diff --git a/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp b/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp index 382968b971..f8125dee15 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp @@ -42,18 +42,14 @@ float AC_PrecLand_Companion::distance_to_target() return _distance_to_target; } -void AC_PrecLand_Companion::handle_msg(const mavlink_message_t &msg) +void AC_PrecLand_Companion::handle_msg(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) { - // parse mavlink message - __mavlink_landing_target_t packet; - mavlink_msg_landing_target_decode(&msg, &packet); - _distance_to_target = packet.distance; // compute unit vector towards target _los_meas_body = Vector3f(-tanf(packet.angle_y), tanf(packet.angle_x), 1.0f); _los_meas_body /= _los_meas_body.length(); - _los_meas_time_ms = AP_HAL::millis(); + _los_meas_time_ms = timestamp_ms; _have_los_meas = true; } diff --git a/libraries/AC_PrecLand/AC_PrecLand_Companion.h b/libraries/AC_PrecLand/AC_PrecLand_Companion.h index 559309a3af..b773f4d2b8 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_Companion.h +++ b/libraries/AC_PrecLand/AC_PrecLand_Companion.h @@ -36,7 +36,7 @@ public: float distance_to_target() override; // parses a mavlink message from the companion computer - void handle_msg(const mavlink_message_t &msg) override; + void handle_msg(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) override; private: float _distance_to_target; // distance from the camera to target in meters