AP_Mount: fix scripting backend

Tools/autotest/arducopter.py

@@ -7190,7 +7190,7 @@
 
         # triple-notch should do better, but check for within 5%
         if peakdb2 * 1.05 > peakdb1:
             raise NotAchievedException(
                 "Triple-notch peak was higher than single-notch peak %fdB > %fdB" %
                 (peakdb2, peakdb1))
 
@@ -13208,6 +13208,110 @@
 
         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 @@
             self.ThrottleGainBoost,
             self.ScriptMountPOI,
             self.ScriptMountAllModes,
+            self.ScriptMountDriver,
             self.ScriptCopterPosOffsets,
             self.MountSolo,
             self.FlyMissionTwice,

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,13 +1262,15 @@ 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;
         }
         break;
     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);

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

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

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;

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()