Skip to content

Conversation

@rmackay9
Copy link
Contributor

@rmackay9 rmackay9 commented Jun 24, 2025

This enhances the AC_PrecLand MAVLink backend so that it can accept LANDING_TARGET messages with the LOCAL_FRD frame (previously it only supported the BODY_FRD frame). This essentially turns off the roll/pitch compensation (but leaves yaw compensation in place) so that a vehicle's main camera gimbal can be used for precision landing.

The PrecisionLoiter autotest has been enhanced slightly to test sending messages using LOCAL_FRD

I've now tested this on a real vehicle running this BlueOS Extension

Copy link

Copilot AI left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Pull Request Overview

This PR updates the AC_PrecLand MAVLink backend to support the LOCAL_FRD frame for LANDING_TARGET messages, replacing previous Companion-related functionality. Key changes include:

  • Renaming and macro updates from COMPANION to MAVLINK throughout the Precision Landing code.
  • Modifications in los measurement handling to use the unified _los_meas structure with vector and timing updates.
  • Updates to the SITL, MAVLink, and IRLock interfaces and corresponding autotest adjustments to allow selection between body-frame and local-frame target messages.

Reviewed Changes

Copilot reviewed 12 out of 12 changed files in this pull request and generated no comments.

Show a summary per file
File Description
libraries/AC_PrecLand/AC_PrecLand_config.h Updated macro name from COMPANION to MAVLINK
libraries/AC_PrecLand/* Replaced _los_meas_body with unified _los_meas usage
libraries/AC_PrecLand/AC_PrecLand_MAVLink.* Added new MAVLink backend handling LOCAL_FRD frame
libraries/AC_PrecLand/AC_PrecLand_Backend.h Modified API for los measurement retrieval
libraries/AC_PrecLand/AC_PrecLand.cpp Updated los_meas retrieval/rotation and estimator logic
Tools/scripts/build_options.py Adjusted build option feature from Companion to MAVLINK
Tools/autotest/arducopter.py Extended precision loiter to select frame type for messages
Comments suppressed due to low confidence (3)

libraries/AC_PrecLand/AC_PrecLand_Backend.h:35

  • The API has been updated from get_los_body to get_los_meas; please ensure that all backend implementations consistently use the unified _los_meas structure for representing los measurements.
        vec_norm = _los_meas.vec_norm;

libraries/AC_PrecLand/AC_PrecLand.cpp:629

  • Verify that _yaw_align is provided in the expected unit (e.g., centi-degrees) since it is converted via cd_to_rad; consistent unit usage is critical for correct vector rotation.
            target_vec_norm.rotate_xy(cd_to_rad(_yaw_align));

Tools/autotest/arducopter.py:5349

  • [nitpick] Consider renaming the parameter 'send_bf' to a more descriptive name like 'use_body_frame' to improve clarity on whether the landing target is sent in body-frame or local FRD frame.
    def precision_loiter_to_pos(self, x, y, z, send_bf = True, timeout=40):

@rmackay9 rmackay9 requested a review from peterbarker June 24, 2025 05:02
@rmackay9 rmackay9 force-pushed the precland-ef branch 3 times, most recently from 8f0a5d4 to 74796f4 Compare June 24, 2025 06:48
Copy link
Contributor

@rishabsingh3003 rishabsingh3003 left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@rmackay9 thanks! Very requested feature.
Two small optional comments, not a blocker.


void AC_PrecLand_MAVLink::handle_msg(const mavlink_landing_target_t &packet, uint32_t timestamp_ms)
{
_distance_to_target = packet.distance;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We should probably have a _los_meas.valid = false here, to prevent stale usage of _los_meas.vec_unit incase this function returns early? We check the timestamp of the last consumed packet to make sure we don't consume duplicates, so this is probably not a problem yet, but my worry is that someone may use this in the future by mistake elsewhere.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think the other backends may need this as well

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hi @rishabsingh3003,

Thanks very much for the review, very few others are familiar with this library.

Re adding specifically setting "_los_meas.valid = false", I've just done a light review and I don't think this is necessary because the target's position will only be updated when "_los_meas.time_ms" is changed by a backend and each backend's update() function includes a timeout. Put another way, I don't think we need to invalidate the last known reading before the timeout does.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@rishabsingh3003,

Ah, I see your point re _distance_to_target though. You're absolutely right that the driver would consume that field even if the LANDING_TARGET was using an invalid frame. I've added a fix so that the field is only consumed when other fields are also consumed, thanks!

@rmackay9 rmackay9 merged commit 2193434 into ArduPilot:master Jul 7, 2025
105 checks passed
@rmackay9 rmackay9 deleted the precland-ef branch July 7, 2025 23:52
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Projects

None yet

Development

Successfully merging this pull request may close these issues.

3 participants