Correct mount script backend functionality

Tools/autotest/arducopter.py

@@ -7317,7 +7317,7 @@
                 raise NotAchievedException("Did not detect a motor peak, found %fHz at %fdB" % (freq, peakdb))
         else:
             if reverse is not None:
                 raise NotAchievedException(
                     "Detected motor peak at %fHz, throttle %f%%, %fdB" %
                     (freq, hover_throttle, peakdb))
             else:
@@ -7398,7 +7398,7 @@
 
         # double-notch should do better, but check for within 5%
         if peakdb2 * 1.05 > peakdb1:
             raise NotAchievedException(
                 "Double-notch peak was higher than single-notch peak %fdB > %fdB" %
                 (peakdb2, peakdb1))
 
@@ -13480,6 +13480,121 @@
 
         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 @@
             self.ThrottleGainBoost,
             self.ScriptMountPOI,
             self.ScriptMountAllModes,
+            self.ScriptMountDriver,
             self.ScriptCopterPosOffsets,
             self.MountSolo,
             self.MountSiyiZT30,

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

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

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)

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

libraries/AP_Mount/AP_Mount_Scripting.cpp

@@ -20,7 +20,13 @@ 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();
 }
 
 // return true if healthy
@@ -39,6 +45,48 @@ 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() 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)
+{
+    _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
 bool AP_Mount_Scripting::get_attitude_quaternion(Quaternion& att_quat)
 {

libraries/AP_Mount/AP_Mount_Scripting.h

@@ -36,23 +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 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() / 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_ONLY;
+        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
     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()
 
 };