Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_PrecLand: pass in time corrected LANDING_TARGET msg

Browse Source 
should make companion based precision landing more accurate
This commit is contained in:
Andrew Tridgell 2021-04-13 12:49:59 +10:00 committed by Peter Barker
parent 5a869174e5
commit 1fe1e53260
5 changed files with 7 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AC_PrecLand/AC_PrecLand.cpp
View File

@ -263,11 +263,11 @@ bool AC_PrecLand::get_target_velocity_relative_cms(Vector2f& ret)
} }
// handle_msg - Process a LANDING_TARGET mavlink message // 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 // run backend update
if (_backend != nullptr) { if (_backend != nullptr) {
_backend->handle_msg(msg); _backend->handle_msg(packet, timestamp_ms);
} }
} }

2
libraries/AC_PrecLand/AC_PrecLand.h
View File

@ -86,7 +86,7 @@ public:
bool target_acquired(); bool target_acquired();
// process a LANDING_TARGET mavlink message // 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 // parameter var table
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];

2
libraries/AC_PrecLand/AC_PrecLand_Backend.h
View File

@ -35,7 +35,7 @@ public:
virtual float distance_to_target() { return 0.0f; }; virtual float distance_to_target() { return 0.0f; };
// parses a mavlink message from the companion computer // 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 // get bus parameter
int8_t get_bus(void) const { return _frontend._bus.get(); } int8_t get_bus(void) const { return _frontend._bus.get(); }

8
libraries/AC_PrecLand/AC_PrecLand_Companion.cpp
View File

@ -42,18 +42,14 @@ float AC_PrecLand_Companion::distance_to_target()
return _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; _distance_to_target = packet.distance;
// compute unit vector towards target // compute unit vector towards target
_los_meas_body = Vector3f(-tanf(packet.angle_y), tanf(packet.angle_x), 1.0f); _los_meas_body = Vector3f(-tanf(packet.angle_y), tanf(packet.angle_x), 1.0f);
_los_meas_body /= _los_meas_body.length(); _los_meas_body /= _los_meas_body.length();
_los_meas_time_ms = AP_HAL::millis(); _los_meas_time_ms = timestamp_ms;
_have_los_meas = true; _have_los_meas = true;
} }

2
libraries/AC_PrecLand/AC_PrecLand_Companion.h
View File

@ -36,7 +36,7 @@ public:
float distance_to_target() override; float distance_to_target() override;
// parses a mavlink message from the companion computer // 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: private:
float _distance_to_target; // distance from the camera to target in meters float _distance_to_target; // distance from the camera to target in meters