From 876faa93fe591ca6d9468367145a283d915a81b8 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Tue, 17 Mar 2026 12:58:14 +1100 Subject: [PATCH 1/5] AP_Scripting: add mount-driver example --- .../AP_Scripting/examples/mount-driver.lua | 153 ++++++++++++++++++ 1 file changed, 153 insertions(+) create mode 100644 libraries/AP_Scripting/examples/mount-driver.lua diff --git a/libraries/AP_Scripting/examples/mount-driver.lua b/libraries/AP_Scripting/examples/mount-driver.lua new file mode 100644 index 0000000000000..1eb25497bf3aa --- /dev/null +++ b/libraries/AP_Scripting/examples/mount-driver.lua @@ -0,0 +1,153 @@ +-- mount-driver.lua: Example scripting gimbal driver +-- +-- Template for writing a Lua gimbal driver using the scripting mount backend. +-- Populate send_target_angles and send_target_rates with your gimbal's +-- protocol (serial, CAN, etc). This example simulates a gimbal by tracking +-- targets internally and reporting them back as attitude. +-- +-- Setup: +-- Set MNT1_TYPE = 9 (Scripting) and reboot +-- Copy this script to the APM/scripts directory and reboot +-- +-- Advanced usage: +-- The gimbal can be used as the Nth mount by setting MNTn_TYPE = 9 and +-- modifying the MOUNT_INSTANCE below. + +-- user definitions +local MOUNT_INSTANCE = 0 -- default to MNT1 + +-- global definitions +local INIT_INTERVAL_MS = 3000 -- attempt to initialise the gimbal at this interval +local UPDATE_INTERVAL_MS = 100 -- update at 10hz +local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} + +-- local variables +local sim_state = { + roll_ef_deg=0, -- roll/pitch earth frame, yaw body frame + pitch_ef_deg=0, -- (common for pwm-controlled brushless gimbals) + yaw_bf_deg=0 +} +local initialised = false +local last_update_ms = 0 + +-- wrap yaw angle in degrees to value between 0 and 360 +local function wrap_360(angle) + local res = math.fmod(angle, 360.0) + if res < 0 then + res = res + 360.0 + end + return res +end + +-- wrap yaw angle in degrees to value between -180 and +180 +local function wrap_180(angle_deg) + local res = wrap_360(angle_deg) + if res > 180 then + res = res - 360 + end + return res +end + +-- bind mount type parameter +local MNT_TYPE = Parameter("MNT" .. (MOUNT_INSTANCE + 1) .. "_TYPE") + +-- perform any required initialisation +local function init() + if MNT_TYPE:get() ~= 9 then + gcs:send_text(MAV_SEVERITY.CRITICAL, "MountDriver: set MNT" .. (MOUNT_INSTANCE + 1) .. "_TYPE=9") + return + end + + initialised = true + last_update_ms = millis():tofloat() + gcs:send_text(MAV_SEVERITY.INFO, "MountDriver: started") +end + +-- send target angles (in degrees) to gimbal +local function send_target_angles(roll_ef_deg, pitch_ef_deg, yaw_deg, yaw_is_ef) + -- default argument values + roll_ef_deg = roll_ef_deg or 0 + pitch_ef_deg = pitch_ef_deg or 0 + yaw_deg = yaw_deg or 0 + yaw_is_ef = yaw_is_ef or false + + if yaw_is_ef then + -- convert to body-frame + yaw_deg = wrap_180(yaw_deg - math.deg(ahrs:get_yaw_rad())) + end + + sim_state.roll_ef_deg = roll_ef_deg + sim_state.pitch_ef_deg = pitch_ef_deg + sim_state.yaw_bf_deg = yaw_deg +end + +-- send target rates (in deg/sec) to gimbal +local function send_target_rates(roll_degs, pitch_degs, yaw_degs, yaw_is_ef, dt_s) + -- default argument values + roll_degs = roll_degs or 0 + pitch_degs = pitch_degs or 0 + yaw_degs = yaw_degs or 0 + yaw_is_ef = yaw_is_ef or false + + if yaw_is_ef then + yaw_degs = yaw_degs - math.deg(ahrs:get_gyro():z()) + end + + send_target_angles( + sim_state.roll_ef_deg + roll_degs * dt_s, + sim_state.pitch_ef_deg + pitch_degs * dt_s, + sim_state.yaw_bf_deg + yaw_degs * dt_s, + false + ) +end + +-- the main update function +local function update() + + -- initialise connection to gimbal + if not initialised then + init() + return + end + + -- calculate dt + local now_ms = millis():tofloat() + local dt_s = (now_ms - last_update_ms) / 1000.0 + last_update_ms = now_ms + + -- report gimbal attitude. Must be called periodically or the backend reports + -- unhealthy. Ideally, populate this from a gimbal attitude message. If your + -- gimbal doesn't report attitude but you can detect it is alive, stop calling + -- this when it stops responding so ArduPilot gets real health feedback. Here + -- we just report our sim state directly since there is no real gimbal. + mount:set_attitude_euler(MOUNT_INSTANCE, sim_state.roll_ef_deg, sim_state.pitch_ef_deg, sim_state.yaw_bf_deg) + + -- send angle target + local roll_deg, pitch_deg, yaw_deg, yaw_is_ef = mount:get_angle_target(MOUNT_INSTANCE) + if roll_deg and pitch_deg and yaw_deg then + send_target_angles(roll_deg, pitch_deg, yaw_deg, yaw_is_ef) + return + end + + -- send rate target + local roll_degs, pitch_degs, yaw_degs + roll_degs, pitch_degs, yaw_degs, yaw_is_ef = mount:get_rate_target(MOUNT_INSTANCE) + if roll_degs and pitch_degs and yaw_degs then + send_target_rates(roll_degs, pitch_degs, yaw_degs, yaw_is_ef, dt_s) + return + end +end + +local function protected_wrapper() + local success, err = pcall(update) + if not success then + gcs:send_text(MAV_SEVERITY.ERROR, "MountDriver: " .. err) + return protected_wrapper, 1000 + end + if not initialised then + return protected_wrapper, INIT_INTERVAL_MS + end + return protected_wrapper, UPDATE_INTERVAL_MS +end + +return protected_wrapper() From 8993398d3ca0c30fd6116ba1cebd2c3f74124f52 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Tue, 17 Mar 2026 12:58:15 +1100 Subject: [PATCH 2/5] autotest: add ScriptMountDriver test Co-authored-by: Bob Long --- Tools/autotest/arducopter.py | 116 +++++++++++++++++++++++++++++++++++ 1 file changed, 116 insertions(+) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index b2e29ce704d5d..b11a763bb15f1 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -13480,6 +13480,121 @@ def ScriptMountAllModes(self): self.do_RTL() + def ScriptMountDriver(self): + '''test scripting mount driver with all modes and camera''' + self.context_push() + + self.set_parameters({ + "SCR_ENABLE": 1, + "MNT1_TYPE": 9, + "MNT1_PITCH_MIN": -45, + "MNT1_PITCH_MAX": 45, + }) + self.reboot_sitl() + + self.install_example_script_context('mount-driver.lua') + self.context_collect('STATUSTEXT') + self.reboot_sitl() + + self.wait_statustext("MountDriver: started", check_context=True, timeout=30) + self.wait_ready_to_arm() + + self.takeoff(20, mode='GUIDED') + + # test RETRACT mode - exercises angle_converted path + self.start_subtest("RETRACT mode") + retract_pitch = -15 + self.set_parameter("MNT1_RETRACT_Y", retract_pitch) + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL, + p7=mavutil.mavlink.MAV_MOUNT_MODE_RETRACT, + ) + self.test_mount_pitch(retract_pitch, 1, mavutil.mavlink.MAV_MOUNT_MODE_RETRACT) + + # test NEUTRAL mode - exercises angle_converted path + self.start_subtest("NEUTRAL mode") + neutral_pitch = -10 + self.set_parameter("MNT1_NEUTRAL_Y", neutral_pitch) + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL, + p7=mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL, + ) + self.test_mount_pitch(neutral_pitch, 1, mavutil.mavlink.MAV_MOUNT_MODE_NEUTRAL) + + # test MAVLINK_TARGETING with angle + self.start_subtest("MAVLINK_TARGETING") + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL, + p1=20, # pitch + p2=0, # roll + p3=0, # yaw + p7=mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING, + ) + self.test_mount_pitch(20, 1, mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING) + + # test MAVLINK_TARGETING with rate + self.start_subtest("MAVLINK_TARGETING rate") + # start at pitch 0 + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW, + p1=0, # pitch angle + p2=0, # yaw angle + ) + self.test_mount_pitch(0, 5, mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING) + # send pitch rate of -30 deg/s for 2 seconds + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW, + p1=float('nan'), # pitch angle (NaN = use rate) + p2=float('nan'), # yaw angle (NaN = use rate) + p3=-30, # pitch rate deg/s + p4=0, # yaw rate deg/s + ) + self.delay_sim_time(2) + # expect pitch around -60 + _, mount_pitch, _, _ = self.get_mount_roll_pitch_yaw_deg() + if abs(mount_pitch - (-60)) > 20: + raise NotAchievedException( + "Rate mode pitch incorrect: got=%f want=-60 (+/-20)" % mount_pitch) + self.progress("Rate mode pitch correct: %f degrees (~-60)" % mount_pitch) + + # test GPS_POINT (ROI) + self.start_subtest("GPS_POINT (ROI)") + takeoff_loc = self.mav.location() + t = self.offset_location_ne(takeoff_loc, 20, 0) + self.run_cmd_int( + mavutil.mavlink.MAV_CMD_DO_SET_ROI_LOCATION, + p5=int(t.lat * 1e7), + p6=int(t.lng * 1e7), + p7=0, + frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, + ) + # We took off to 20m and the target is 20m North, so pitch should be around -45 degrees + self.test_mount_pitch(-45, 5, mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT) + + # Reposition the aircraft 20m South of the takeoff location, + # so it's now 40m from the target, and pitch should be around + # -27 degrees + t = self.offset_location_ne(takeoff_loc, -20, 0) + self.send_set_position_target_global_int(int(t.lat * 1e7), int(t.lng * 1e7), 20) + self.test_mount_pitch(-27, 5, mavutil.mavlink.MAV_MOUNT_MODE_GPS_POINT, constrained=False) + + # test HOME_LOCATION + self.start_subtest("HOME_LOCATION") + self.run_cmd( + mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL, + p7=mavutil.mavlink.MAV_MOUNT_MODE_HOME_LOCATION, + ) + # We are 20m South of home and at 20m altitude, so pitch should be around -45 degrees + self.test_mount_pitch(-45, 5, mavutil.mavlink.MAV_MOUNT_MODE_HOME_LOCATION, constrained=False) + + # Reposition over home again, pitch should move to -90 degrees + self.send_set_position_target_global_int(int(takeoff_loc.lat * 1e7), int(takeoff_loc.lng * 1e7), 20) + self.test_mount_pitch(-90, 5, mavutil.mavlink.MAV_MOUNT_MODE_HOME_LOCATION, constrained=False) + + self.do_RTL() + self.context_pop() + self.reboot_sitl() + def ScriptCopterPosOffsets(self): '''test the copter-posoffset.lua example script''' self.context_push() @@ -16226,6 +16341,7 @@ def tests2b(self): # this block currently around 9.5mins here self.ThrottleGainBoost, self.ScriptMountPOI, self.ScriptMountAllModes, + self.ScriptMountDriver, self.ScriptCopterPosOffsets, self.MountSolo, self.MountSiyiZT30, From 58108e60d52ccdc2edd38544cc739629fcafa4e4 Mon Sep 17 00:00:00 2001 From: Bob Long Date: Tue, 17 Mar 2026 12:58:19 +1100 Subject: [PATCH 3/5] AP_Mount: fix get_angle_target for converted cmds If a backend gets the target mode converted to angle, get_angle_target will return the converted angles. This fixes scripting backends that relied on get_angle_target --- libraries/AP_Mount/AP_Mount_Backend.cpp | 9 ++++++++- libraries/AP_Mount/AP_Mount_Backend.h | 1 + libraries/AP_Mount/AP_Mount_Scripting.cpp | 2 ++ libraries/AP_Mount/AP_Mount_Scripting.h | 3 ++- 4 files changed, 13 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 5ab29e7722f12..3178b90370746 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -1199,6 +1199,9 @@ void AP_Mount_Backend::_update_mnt_target() void AP_Mount_Backend::send_target_to_gimbal() { + // clear valid flag; set below if angles are sent + mnt_target.angle_converted = false; + // process any pending clear-roi-target // it is assumed that we have already zeroed _roi_target if (clear_roi_pending && natively_supports(MountTargetType::LOCATION)) { @@ -1240,6 +1243,7 @@ void AP_Mount_Backend::send_target_to_gimbal() if (natively_supports(MountTargetType::ANGLE)) { // we integrate the rates into the angle: update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad); + mnt_target.angle_converted = true; send_target_angles(mnt_target.angle_rad); return; } @@ -1250,6 +1254,7 @@ void AP_Mount_Backend::send_target_to_gimbal() // we update mnt_target for reporting purposes const Vector3f &angle_bf_target = _params.retract_angles.get(); mnt_target.angle_rad.set(angle_bf_target*DEG_TO_RAD, false); + mnt_target.angle_converted = true; send_target_angles(mnt_target.angle_rad); return; } @@ -1260,6 +1265,7 @@ void AP_Mount_Backend::send_target_to_gimbal() // we update mnt_target for reporting purposes const Vector3f &angle_bf_target = _params.neutral_angles.get(); mnt_target.angle_rad.set(angle_bf_target*DEG_TO_RAD, false); + mnt_target.angle_converted = true; send_target_angles(mnt_target.angle_rad); return; } @@ -1267,6 +1273,7 @@ void AP_Mount_Backend::send_target_to_gimbal() case MountTargetType::LOCATION: if (natively_supports(MountTargetType::ANGLE)) { if (get_angle_target_to_roi(mnt_target.angle_rad)) { + mnt_target.angle_converted = true; send_target_angles(mnt_target.angle_rad); } return; @@ -1294,7 +1301,7 @@ bool AP_Mount_Backend::get_rate_target(float& roll_degs, float& pitch_degs, floa // get target angle in deg. returns true on success bool AP_Mount_Backend::get_angle_target(float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame) { - if (mnt_target.target_type == MountTargetType::ANGLE) { + if (mnt_target.target_type == MountTargetType::ANGLE || mnt_target.angle_converted) { roll_deg = degrees(mnt_target.angle_rad.roll); pitch_deg = degrees(mnt_target.angle_rad.pitch); yaw_deg = degrees(mnt_target.angle_rad.yaw); diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 14139a9a43077..007d46e352abb 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -398,6 +398,7 @@ class AP_Mount_Backend uint32_t last_rate_request_ms; uint32_t poi_start_ms; // time we started trying to find the gimbal POI for an AuxFunc::MOUNT_POI_LOCK bool pointing_at_poi_at_home_alt; + bool angle_converted; // true if a non-angle target was converted to angles by send_target_to_gimbal } mnt_target; // RP earth frame locks accessible by backend diff --git a/libraries/AP_Mount/AP_Mount_Scripting.cpp b/libraries/AP_Mount/AP_Mount_Scripting.cpp index cee5d98b3c7d4..e50b96583f855 100644 --- a/libraries/AP_Mount/AP_Mount_Scripting.cpp +++ b/libraries/AP_Mount/AP_Mount_Scripting.cpp @@ -21,6 +21,8 @@ void AP_Mount_Scripting::update() AP_Mount_Backend::update(); update_mnt_target(); + + send_target_to_gimbal(); } // return true if healthy diff --git a/libraries/AP_Mount/AP_Mount_Scripting.h b/libraries/AP_Mount/AP_Mount_Scripting.h index e16bc0e411c69..273ba75f418a5 100644 --- a/libraries/AP_Mount/AP_Mount_Scripting.h +++ b/libraries/AP_Mount/AP_Mount_Scripting.h @@ -41,9 +41,10 @@ class AP_Mount_Scripting : public AP_Mount_Backend // Scripting doesn't actually send anything (the script polls the // library for the targets) uint8_t natively_supported_mount_target_types() const override { - return NATIVE_ANGLES_ONLY; + return NATIVE_ANGLES_AND_RATES_ONLY; }; void send_target_angles(const MountAngleTarget &angle_rad) override {}; + void send_target_rates(const MountRateTarget &rate_rads) override {}; // get attitude as a quaternion. returns true on success bool get_attitude_quaternion(Quaternion& att_quat) override; From ced62706dfde1f91e85b68195e4e9e2b2afb9b5f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 24 Mar 2026 22:15:08 +1100 Subject: [PATCH 4/5] AP_Mount: fix scripting backend for non-angle target modes Scripting backends (e.g. mount-djirs2-driver.lua) poll get_angle_target() and get_rate_target() rather than receiving pushed targets. Before this fix, get_angle_target() returned false for RETRACT, NEUTRAL, and LOCATION modes because it only checked target_type == ANGLE, even though send_target_to_gimbal() had already written the converted angle into mnt_target.angle_rad. Fix by calling send_target_to_gimbal() from AP_Mount_Scripting::update() (matching every other backend) and implementing send_target_angles() to stamp mnt_target.target_type = ANGLE after send_target_to_gimbal() has written the converted value into mnt_target.angle_rad. This makes get_angle_target() return the converted value without any changes to the base class. Declare NATIVE_ANGLES_AND_RATES_ONLY so that RATE targets are not converted to angles internally; the Lua script receives them via get_rate_target() and performs its own integration. send_target_rates() is a no-op because mnt_target.target_type stays RATE, which is all get_rate_target() checks. Co-Authored-By: Claude Sonnet 4.6 --- libraries/AP_Mount/AP_Mount_Backend.cpp | 9 +-------- libraries/AP_Mount/AP_Mount_Backend.h | 1 - libraries/AP_Mount/AP_Mount_Scripting.cpp | 10 ++++++++++ libraries/AP_Mount/AP_Mount_Scripting.h | 11 ++++++++--- 4 files changed, 19 insertions(+), 12 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 3178b90370746..5ab29e7722f12 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -1199,9 +1199,6 @@ void AP_Mount_Backend::_update_mnt_target() void AP_Mount_Backend::send_target_to_gimbal() { - // clear valid flag; set below if angles are sent - mnt_target.angle_converted = false; - // process any pending clear-roi-target // it is assumed that we have already zeroed _roi_target if (clear_roi_pending && natively_supports(MountTargetType::LOCATION)) { @@ -1243,7 +1240,6 @@ void AP_Mount_Backend::send_target_to_gimbal() if (natively_supports(MountTargetType::ANGLE)) { // we integrate the rates into the angle: update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad); - mnt_target.angle_converted = true; send_target_angles(mnt_target.angle_rad); return; } @@ -1254,7 +1250,6 @@ void AP_Mount_Backend::send_target_to_gimbal() // we update mnt_target for reporting purposes const Vector3f &angle_bf_target = _params.retract_angles.get(); mnt_target.angle_rad.set(angle_bf_target*DEG_TO_RAD, false); - mnt_target.angle_converted = true; send_target_angles(mnt_target.angle_rad); return; } @@ -1265,7 +1260,6 @@ void AP_Mount_Backend::send_target_to_gimbal() // we update mnt_target for reporting purposes const Vector3f &angle_bf_target = _params.neutral_angles.get(); mnt_target.angle_rad.set(angle_bf_target*DEG_TO_RAD, false); - mnt_target.angle_converted = true; send_target_angles(mnt_target.angle_rad); return; } @@ -1273,7 +1267,6 @@ void AP_Mount_Backend::send_target_to_gimbal() case MountTargetType::LOCATION: if (natively_supports(MountTargetType::ANGLE)) { if (get_angle_target_to_roi(mnt_target.angle_rad)) { - mnt_target.angle_converted = true; send_target_angles(mnt_target.angle_rad); } return; @@ -1301,7 +1294,7 @@ bool AP_Mount_Backend::get_rate_target(float& roll_degs, float& pitch_degs, floa // get target angle in deg. returns true on success bool AP_Mount_Backend::get_angle_target(float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame) { - if (mnt_target.target_type == MountTargetType::ANGLE || mnt_target.angle_converted) { + if (mnt_target.target_type == MountTargetType::ANGLE) { roll_deg = degrees(mnt_target.angle_rad.roll); pitch_deg = degrees(mnt_target.angle_rad.pitch); yaw_deg = degrees(mnt_target.angle_rad.yaw); diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 007d46e352abb..14139a9a43077 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -398,7 +398,6 @@ class AP_Mount_Backend uint32_t last_rate_request_ms; uint32_t poi_start_ms; // time we started trying to find the gimbal POI for an AuxFunc::MOUNT_POI_LOCK bool pointing_at_poi_at_home_alt; - bool angle_converted; // true if a non-angle target was converted to angles by send_target_to_gimbal } mnt_target; // RP earth frame locks accessible by backend diff --git a/libraries/AP_Mount/AP_Mount_Scripting.cpp b/libraries/AP_Mount/AP_Mount_Scripting.cpp index e50b96583f855..cc6b132d8d627 100644 --- a/libraries/AP_Mount/AP_Mount_Scripting.cpp +++ b/libraries/AP_Mount/AP_Mount_Scripting.cpp @@ -41,6 +41,16 @@ void AP_Mount_Scripting::set_attitude_euler(float roll_deg, float pitch_deg, flo current_angle_deg.z = yaw_bf_deg; } +// called by send_target_to_gimbal() after it has written the converted angle +// into mnt_target.angle_rad. Stamp the target_type as ANGLE so that the +// base-class get_angle_target() returns the value to the Lua script. +// This covers every non-ANGLE mode that converts to angles (RETRACT, NEUTRAL, +// LOCATION, and rate-to-angle if the backend were NATIVE_ANGLES_ONLY). +void AP_Mount_Scripting::send_target_angles(const MountAngleTarget &angle_rad) +{ + mnt_target.target_type = MountTargetType::ANGLE; +} + // get attitude as a quaternion. returns true on success bool AP_Mount_Scripting::get_attitude_quaternion(Quaternion& att_quat) { diff --git a/libraries/AP_Mount/AP_Mount_Scripting.h b/libraries/AP_Mount/AP_Mount_Scripting.h index 273ba75f418a5..f880b3df6692a 100644 --- a/libraries/AP_Mount/AP_Mount_Scripting.h +++ b/libraries/AP_Mount/AP_Mount_Scripting.h @@ -38,12 +38,17 @@ class AP_Mount_Scripting : public AP_Mount_Backend protected: - // Scripting doesn't actually send anything (the script polls the - // library for the targets) + // Scripting backends poll get_angle_target / get_rate_target rather than + // receiving pushed targets, so native support for both types is declared so + // send_target_to_gimbal() never converts rates to angles on the backend's + // behalf. send_target_angles() stamps mnt_target.target_type = ANGLE so + // that get_angle_target() returns the converted value for non-angle modes + // (RETRACT, NEUTRAL, LOCATION) that send_target_to_gimbal() converts and + // stores into mnt_target.angle_rad before calling send_target_angles(). uint8_t natively_supported_mount_target_types() const override { return NATIVE_ANGLES_AND_RATES_ONLY; }; - void send_target_angles(const MountAngleTarget &angle_rad) override {}; + void send_target_angles(const MountAngleTarget &angle_rad) override; void send_target_rates(const MountRateTarget &rate_rads) override {}; // get attitude as a quaternion. returns true on success From 01bc95473da163b175c3c3057409d26e9694e8b5 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 24 Mar 2026 22:46:38 +1100 Subject: [PATCH 5/5] AP_Mount: gate get_angle/rate_target behind AP_SCRIPTING_ENABLED; scripting backend fully self-contained get_angle_target() and get_rate_target() in AP_Mount_Backend and AP_Mount are now declared and defined only when AP_SCRIPTING_ENABLED is set, matching the existing treatment of get_location_target(). AP_Mount_Backend::write_log() read mnt_target directly when scripting is not compiled in, avoiding the dependency on those methods. AP_Mount_Scripting gains its own ScriptTargetType enum and _angle_target/_rate_target members. send_target_angles() and send_target_rates() now store the pushed target into those members. get_angle_target() / get_rate_target() overrides return the stored values, removing the previous hack of stamping mnt_target.target_type = ANGLE inside send_target_angles(). _script_target_type is reset to NONE at the top of each update() cycle so stale targets are never returned. Co-Authored-By: Claude Sonnet 4.6 --- libraries/AP_Mount/AP_Mount.cpp | 2 +- libraries/AP_Mount/AP_Mount.h | 2 +- libraries/AP_Mount/AP_Mount_Backend.cpp | 9 ++++- libraries/AP_Mount/AP_Mount_Backend.h | 6 +-- libraries/AP_Mount/AP_Mount_Scripting.cpp | 48 ++++++++++++++++++++--- libraries/AP_Mount/AP_Mount_Scripting.h | 27 +++++++++---- 6 files changed, 74 insertions(+), 20 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index a8dcb1865fcb6..cf411cdd156ce 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -728,6 +728,7 @@ bool AP_Mount::pre_arm_checks(char *failure_msg, uint8_t failure_msg_len) return true; } +#if AP_SCRIPTING_ENABLED // get target rate in deg/sec. returns true on success bool AP_Mount::get_rate_target(uint8_t instance, float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame) { @@ -748,7 +749,6 @@ bool AP_Mount::get_angle_target(uint8_t instance, float& roll_deg, float& pitch_ return backend->get_angle_target(roll_deg, pitch_deg, yaw_deg, yaw_is_earth_frame); } -#if AP_SCRIPTING_ENABLED // get mount target location. returns true on success bool AP_Mount::get_location_target(uint8_t instance, Location& target_loc) { diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 88c206ed3a18d..ec5a52402ec21 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -246,13 +246,13 @@ class AP_Mount // any failure_msg returned will not include a prefix bool pre_arm_checks(char *failure_msg, uint8_t failure_msg_len); +#if AP_SCRIPTING_ENABLED // get target rate in deg/sec. returns true on success bool get_rate_target(uint8_t instance, float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame); // get target angle in deg. returns true on success bool get_angle_target(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame); -#if AP_SCRIPTING_ENABLED // get mount target location. returns true on success bool get_location_target(uint8_t instance, Location& target_loc); #endif diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 5ab29e7722f12..4ceafb2d03a0a 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -619,7 +619,12 @@ void AP_Mount_Backend::write_log(uint64_t timestamp_us) float target_pitch = nanf; float target_yaw = nanf; bool target_yaw_is_ef = false; - IGNORE_RETURN(get_angle_target(target_roll, target_pitch, target_yaw, target_yaw_is_ef)); + if (mnt_target.target_type == MountTargetType::ANGLE) { + target_roll = degrees(mnt_target.angle_rad.roll); + target_pitch = degrees(mnt_target.angle_rad.pitch); + target_yaw = degrees(mnt_target.angle_rad.yaw); + target_yaw_is_ef = mnt_target.angle_rad.yaw_is_ef; + } // get rangefinder distance float rangefinder_dist = nanf; @@ -1278,6 +1283,7 @@ void AP_Mount_Backend::send_target_to_gimbal() } +#if AP_SCRIPTING_ENABLED // get target rate in deg/sec. returns true on success bool AP_Mount_Backend::get_rate_target(float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame) { @@ -1304,7 +1310,6 @@ bool AP_Mount_Backend::get_angle_target(float& roll_deg, float& pitch_deg, float return false; } -#if AP_SCRIPTING_ENABLED // return target location if available // returns true if a target location is available and fills in target_loc argument bool AP_Mount_Backend::get_location_target(Location &_target_loc) diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index 14139a9a43077..baf87101129ae 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -155,13 +155,13 @@ class AP_Mount_Backend // handle GIMBAL_DEVICE_ATTITUDE_STATUS message virtual void handle_gimbal_device_attitude_status(const mavlink_message_t &msg) {} +#if AP_SCRIPTING_ENABLED // get target rate in deg/sec. returns true on success - bool get_rate_target(float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame); + virtual bool get_rate_target(float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame); // get target angle in deg. returns true on success - bool get_angle_target(float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame); + virtual bool get_angle_target(float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame); -#if AP_SCRIPTING_ENABLED // get mount target location. returns true on success bool get_location_target(Location &target_loc); #endif diff --git a/libraries/AP_Mount/AP_Mount_Scripting.cpp b/libraries/AP_Mount/AP_Mount_Scripting.cpp index cc6b132d8d627..d5521b33dd95a 100644 --- a/libraries/AP_Mount/AP_Mount_Scripting.cpp +++ b/libraries/AP_Mount/AP_Mount_Scripting.cpp @@ -20,6 +20,10 @@ void AP_Mount_Scripting::update() { AP_Mount_Backend::update(); + // reset script target type so get_angle_target / get_rate_target return + // false until send_target_to_gimbal() writes a fresh target this cycle + _script_target_type = ScriptTargetType::NONE; + update_mnt_target(); send_target_to_gimbal(); @@ -41,14 +45,46 @@ void AP_Mount_Scripting::set_attitude_euler(float roll_deg, float pitch_deg, flo current_angle_deg.z = yaw_bf_deg; } -// called by send_target_to_gimbal() after it has written the converted angle -// into mnt_target.angle_rad. Stamp the target_type as ANGLE so that the -// base-class get_angle_target() returns the value to the Lua script. -// This covers every non-ANGLE mode that converts to angles (RETRACT, NEUTRAL, -// LOCATION, and rate-to-angle if the backend were NATIVE_ANGLES_ONLY). +// called by send_target_to_gimbal() with the angle target for this cycle. +// Store it so get_angle_target() can return it to the Lua script. void AP_Mount_Scripting::send_target_angles(const MountAngleTarget &angle_rad) { - mnt_target.target_type = MountTargetType::ANGLE; + _angle_target = angle_rad; + _script_target_type = ScriptTargetType::ANGLE; +} + +// called by send_target_to_gimbal() with the rate target for this cycle. +// Store it so get_rate_target() can return it to the Lua script. +void AP_Mount_Scripting::send_target_rates(const MountRateTarget &rate_rads) +{ + _rate_target = rate_rads; + _script_target_type = ScriptTargetType::RATE; +} + +// get target angle in deg. returns true on success +bool AP_Mount_Scripting::get_angle_target(float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame) +{ + if (_script_target_type != ScriptTargetType::ANGLE) { + return false; + } + roll_deg = degrees(_angle_target.roll); + pitch_deg = degrees(_angle_target.pitch); + yaw_deg = degrees(_angle_target.yaw); + yaw_is_earth_frame = _angle_target.yaw_is_ef; + return true; +} + +// get target rate in deg/sec. returns true on success +bool AP_Mount_Scripting::get_rate_target(float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame) +{ + if (_script_target_type != ScriptTargetType::RATE) { + return false; + } + roll_degs = degrees(_rate_target.roll); + pitch_degs = degrees(_rate_target.pitch); + yaw_degs = degrees(_rate_target.yaw); + yaw_is_earth_frame = _rate_target.yaw_is_ef; + return true; } // get attitude as a quaternion. returns true on success diff --git a/libraries/AP_Mount/AP_Mount_Scripting.h b/libraries/AP_Mount/AP_Mount_Scripting.h index f880b3df6692a..940f2cc7db0da 100644 --- a/libraries/AP_Mount/AP_Mount_Scripting.h +++ b/libraries/AP_Mount/AP_Mount_Scripting.h @@ -36,29 +36,42 @@ class AP_Mount_Scripting : public AP_Mount_Backend // accessors for scripting backends void set_attitude_euler(float roll_deg, float pitch_deg, float yaw_bf_deg) override; + // get target rate in deg/sec. returns true on success + bool get_rate_target(float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame) override; + + // get target angle in deg. returns true on success + bool get_angle_target(float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame) override; + protected: // Scripting backends poll get_angle_target / get_rate_target rather than // receiving pushed targets, so native support for both types is declared so // send_target_to_gimbal() never converts rates to angles on the backend's - // behalf. send_target_angles() stamps mnt_target.target_type = ANGLE so - // that get_angle_target() returns the converted value for non-angle modes - // (RETRACT, NEUTRAL, LOCATION) that send_target_to_gimbal() converts and - // stores into mnt_target.angle_rad before calling send_target_angles(). + // behalf. send_target_angles() / send_target_rates() store the converted + // target for retrieval via get_angle_target() / get_rate_target(). uint8_t natively_supported_mount_target_types() const override { return NATIVE_ANGLES_AND_RATES_ONLY; }; void send_target_angles(const MountAngleTarget &angle_rad) override; - void send_target_rates(const MountRateTarget &rate_rads) override {}; + void send_target_rates(const MountRateTarget &rate_rads) override; // get attitude as a quaternion. returns true on success bool get_attitude_quaternion(Quaternion& att_quat) override; private: + enum class ScriptTargetType : uint8_t { + NONE = 0, + ANGLE = 1, + RATE = 2, + }; + // internal variables - uint32_t last_update_ms; // system time of last call to one of the get_ methods. Used for health reporting - Vector3f current_angle_deg; // current gimbal angles in degrees (x=roll, y=pitch, z=yaw) + uint32_t last_update_ms; // system time of last call to one of the get_ methods. Used for health reporting + Vector3f current_angle_deg; // current gimbal angles in degrees (x=roll, y=pitch, z=yaw) + ScriptTargetType _script_target_type {ScriptTargetType::NONE}; + MountAngleTarget _angle_target {}; // last angle target pushed by send_target_angles() + MountRateTarget _rate_target {}; // last rate target pushed by send_target_rates() };