diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index d0de241a793ba1..65ed95f432d600 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -13208,6 +13208,110 @@ 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) + + # 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 directly above home, so pitch should be around -90 degrees + 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() @@ -15880,6 +15984,7 @@ def tests2b(self): # this block currently around 9.5mins here self.ThrottleGainBoost, self.ScriptMountPOI, self.ScriptMountAllModes, + self.ScriptMountDriver, self.ScriptCopterPosOffsets, self.MountSolo, self.FlyMissionTwice, diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp index 96c7690eca92c1..3ecb4f82d13d78 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.cpp +++ b/libraries/AP_Mount/AP_Mount_Backend.cpp @@ -1196,6 +1196,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)) { @@ -1237,6 +1240,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; } @@ -1247,6 +1251,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; } @@ -1257,6 +1262,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; } @@ -1264,6 +1270,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; @@ -1291,7 +1298,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 d0e0beae3d1414..b335654b6153af 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -396,6 +396,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 cee5d98b3c7d4a..e50b96583f8556 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 e16bc0e411c69a..273ba75f418a54 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; diff --git a/libraries/AP_Scripting/examples/mount-driver.lua b/libraries/AP_Scripting/examples/mount-driver.lua new file mode 100644 index 00000000000000..1eb25497bf3aa9 --- /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()