diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index b2e29ce704d5dc..b11a763bb15f1b 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, diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index a8dcb1865fcb6e..cf411cdd156ce4 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 88c206ed3a18dd..ec5a52402ec21c 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 5ab29e7722f12d..4ceafb2d03a0ad 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 14139a9a430774..baf87101129ae9 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 cee5d98b3c7d4a..d5521b33dd95a0 100644 --- a/libraries/AP_Mount/AP_Mount_Scripting.cpp +++ b/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) { diff --git a/libraries/AP_Mount/AP_Mount_Scripting.h b/libraries/AP_Mount/AP_Mount_Scripting.h index e16bc0e411c69a..940f2cc7db0da6 100644 --- a/libraries/AP_Mount/AP_Mount_Scripting.h +++ b/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() }; 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()