diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index cf411cdd156ce4..9eda7c2987a175 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -770,6 +770,17 @@ void AP_Mount::set_attitude_euler(uint8_t instance, float roll_deg, float pitch_ backend->set_attitude_euler(roll_deg, pitch_deg, yaw_bf_deg); } +#if AP_SCRIPTING_ENABLED +void AP_Mount::set_natively_supported_mount_target_types(uint8_t instance, uint8_t types_mask) +{ + auto *backend = get_instance(instance); + if (backend == nullptr) { + return; + } + backend->set_natively_supported_mount_target_types(types_mask); +} +#endif // AP_SCRIPTING_ENABLED + #if HAL_LOGGING_ENABLED // write mount log packet for all backends void AP_Mount::write_log() diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index ec5a52402ec21c..d0a4406295c30c 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -260,6 +260,13 @@ class AP_Mount // accessors for scripting backends and logging void set_attitude_euler(uint8_t instance, float roll_deg, float pitch_deg, float yaw_bf_deg); + #if AP_SCRIPTING_ENABLED + // used by a script sending messages to a physical device to + // indicate what targets can be either natively sent to the device + // or internally handled by the script + void set_natively_supported_mount_target_types(uint8_t instance, uint8_t types_mask); +#endif // AP_SCRIPTING_ENABLED + // write mount log packet for all backends void write_log(); diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h index baf87101129ae9..72b2893d15a906 100644 --- a/libraries/AP_Mount/AP_Mount_Backend.h +++ b/libraries/AP_Mount/AP_Mount_Backend.h @@ -169,6 +169,13 @@ class AP_Mount_Backend // accessors for scripting backends virtual void set_attitude_euler(float roll_deg, float pitch_deg, float yaw_bf_deg) {}; +#if AP_SCRIPTING_ENABLED + // used by a script sending messages to a physical device to + // indicate what targets can be either natively sent to the device + // or internally handled by the script + virtual void set_natively_supported_mount_target_types(uint8_t types_mask) { } +#endif // AP_SCRIPTING_ENABLED + // write mount log packet void write_log(uint64_t timestamp_us); diff --git a/libraries/AP_Mount/AP_Mount_Scripting.h b/libraries/AP_Mount/AP_Mount_Scripting.h index 940f2cc7db0da6..d2c857f10357d6 100644 --- a/libraries/AP_Mount/AP_Mount_Scripting.h +++ b/libraries/AP_Mount/AP_Mount_Scripting.h @@ -44,13 +44,14 @@ class AP_Mount_Scripting : public AP_Mount_Backend 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() / send_target_rates() store the converted - // target for retrieval via get_angle_target() / get_rate_target(). + void set_natively_supported_mount_target_types(uint8_t types_mask) override { + supported_target_types_mask = types_mask; + } + + // 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_AND_RATES_ONLY; + return supported_target_types_mask; }; void send_target_angles(const MountAngleTarget &angle_rad) override; void send_target_rates(const MountRateTarget &rate_rads) override; @@ -73,6 +74,7 @@ class AP_Mount_Scripting : public AP_Mount_Backend MountAngleTarget _angle_target {}; // last angle target pushed by send_target_angles() MountRateTarget _rate_target {}; // last rate target pushed by send_target_rates() + uint8_t supported_target_types_mask = NATIVE_ANGLES_AND_RATES_ONLY; }; #endif // HAL_MOUNT_SCRIPTING_ENABLED diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index a6a8b61d4f3f6f..8ebf7769bcfe45 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -1732,74 +1732,108 @@ function mavlink_video_stream_information_t_ud:encoding(value) end ---@param stream_info mavlink_video_stream_information_t_ud function camera:set_stream_information(instance, stream_info) end --- desc +-- Mount/gimbal control and driver interface mount = {} --- desc ----@param instance integer +-- methods to interact with ArduPilot's mount/gimbal frontend + +-- Returns the current mount mode +---@param instance integer -- mount instance (0 or 1) +---@return integer +---| '0' # Retract +---| '1' # Neutral +---| '2' # MAVLink targeting +---| '3' # RC targeting +---| '4' # GPS point +---| '5' # SysID target +---| '6' # Home location +function mount:get_mode(instance) end + +-- Set mount operating mode +---@param instance integer -- mount instance (0 or 1) +---@param mode integer +---| '0' # Retract +---| '1' # Neutral +---| '2' # MAVLink targeting +---| '3' # RC targeting +---| '4' # GPS point +---| '5' # SysID target +---| '6' # Home location +function mount:set_mode(instance, mode) end + +-- Set target angles for the gimbal. Roll and pitch are always earth-frame, yaw frame is selectable +---@param instance integer -- mount instance (0 or 1) ---@param roll_deg number ---@param pitch_deg number ---@param yaw_deg number -function mount:set_attitude_euler(instance, roll_deg, pitch_deg, yaw_deg) end +---@param yaw_is_earth_frame boolean -- true for earth-frame yaw, false for body-frame +function mount:set_angle_target(instance, roll_deg, pitch_deg, yaw_deg, yaw_is_earth_frame) end --- desc ----@param instance integer ----@return Location_ud|nil -function mount:get_location_target(instance) end +-- Set target angular rates for the gimbal in degrees per second +---@param instance integer -- mount instance (0 or 1) +---@param roll_degs number +---@param pitch_degs number +---@param yaw_degs number +---@param yaw_is_earth_frame boolean -- true for earth-frame yaw rate, false for body-frame +function mount:set_rate_target(instance, roll_degs, pitch_degs, yaw_degs, yaw_is_earth_frame) end --- desc ----@param instance integer +-- Point the gimbal at a Location (Region of Interest) +---@param instance integer -- mount instance (0 or 1) +---@param target_loc Location_ud +function mount:set_roi_target(instance, target_loc) end + +-- Returns the gimbal's current attitude as Euler angles in degrees. Yaw is in body-frame +---@param instance integer -- mount instance (0 or 1) +---@return number|nil -- roll_deg +---@return number|nil -- pitch_deg +---@return number|nil -- yaw_bf_deg +function mount:get_attitude_euler(instance) end + +-- methods used to implement a gimbal driver in Lua (MNTn_TYPE = 9): + +-- Declare which target types the gimbal driver natively supports as a bitmask of MountTargetType +-- bit values. The mount frontend uses this to decide what to pass through and what to convert. +-- e.g., if your gimbal only supports setting the angle targets, then the frontend will convert +-- location and rate commands to an angle for you. +---@param instance integer -- mount instance (0 or 1) +---@param types_mask integer -- bitmask of supported MountTargetType values, combine with bitwise OR +---| '1' # angle (1 << 0) +---| '2' # rate (1 << 1) +---| '4' # retracted (1 << 2) +---| '8' # neutral (1 << 3) +---| '16' # location (1 << 4) +function mount:set_natively_supported_mount_target_types(instance, types_mask) end + +-- Get the angle target that the frontend has requested, in degrees. Returns nil if angles are not the active target type +---@param instance integer -- mount instance (0 or 1) ---@return number|nil -- roll_deg ---@return number|nil -- pitch_deg ---@return number|nil -- yaw_deg ---@return boolean|nil -- yaw_is_earth_frame function mount:get_angle_target(instance) end --- desc ----@param instance integer +-- Get the rate target that the frontend has requested, in degrees per second. Returns nil if rates are not the active target type +---@param instance integer -- mount instance (0 or 1) ---@return number|nil -- roll_degs ---@return number|nil -- pitch_degs ---@return number|nil -- yaw_degs ---@return boolean|nil -- yaw_is_earth_frame function mount:get_rate_target(instance) end --- desc ----@param instance integer ----@param target_loc Location_ud -function mount:set_roi_target(instance, target_loc) end - --- desc ----@param instance integer ----@param roll_degs number ----@param pitch_degs number ----@param yaw_degs number ----@param yaw_is_earth_frame boolean -function mount:set_rate_target(instance, roll_degs, pitch_degs, yaw_degs, yaw_is_earth_frame) end +-- Get the location the mount should be pointing at, if any. Returns a target when the mount mode +-- implies a location (GPS_POINT, HOME_LOCATION, SYSID_TARGET), nil otherwise. +-- Unlike get_angle_target/get_rate_target this is not affected by natively supported types +---@param instance integer -- mount instance (0 or 1) +---@return Location_ud|nil +function mount:get_location_target(instance) end --- desc ----@param instance integer +-- Report the gimbal's current attitude back to ArduPilot. Must be called regularly to indicate +-- the mount is healthy; stop calling when the gimbal is unhealthy to signal the failure +---@param instance integer -- mount instance (0 or 1) ---@param roll_deg number ---@param pitch_deg number ---@param yaw_deg number ----@param yaw_is_earth_frame boolean -function mount:set_angle_target(instance, roll_deg, pitch_deg, yaw_deg, yaw_is_earth_frame) end - --- desc ----@param instance integer ----@param mode integer -function mount:set_mode(instance, mode) end - --- desc ----@param instance integer ----@return integer -function mount:get_mode(instance) end - --- desc ----@param instance integer ----@return number|nil -- roll_deg ----@return number|nil -- pitch_deg ----@return number|nil -- yaw_bf_deg -function mount:get_attitude_euler(instance) end +function mount:set_attitude_euler(instance, roll_deg, pitch_deg, yaw_deg) end -- desc motors = {} diff --git a/libraries/AP_Scripting/drivers/mount-djirs2-driver.lua b/libraries/AP_Scripting/drivers/mount-djirs2-driver.lua index 911017276e846a..2dbae06a111521 100644 --- a/libraries/AP_Scripting/drivers/mount-djirs2-driver.lua +++ b/libraries/AP_Scripting/drivers/mount-djirs2-driver.lua @@ -89,6 +89,7 @@ local SET_ATTITUDE_INTERVAL_MS = 100 -- set attitude at 10hz local SEND_FRAMEID = 0x223 -- send CAN messages with this frame id local RECEIVE_FRAMEID = 0x222 -- receive CAN messages with this frame id local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} +local MOUNT_TARGET_TYPE = {ANGLE=1, RATE=2, RETRACTED=4, NEUTRAL=8, LOCATION=16} -- parameters local PARAM_TABLE_KEY = 38 @@ -328,6 +329,8 @@ function init() else gcs:send_text(MAV_SEVERITY.CRITICAL, "DJIR: failed to connect to CAN bus") end + + mount:set_natively_supported_mount_target_types(MOUNT_INSTANCE, MOUNT_TARGET_TYPE.ANGLE | MOUNT_TARGET_TYPE.RATE) end -- send serial message over CAN bus diff --git a/libraries/AP_Scripting/examples/mount-driver.lua b/libraries/AP_Scripting/examples/mount-driver.lua index 1eb25497bf3aa9..4ef1a0489661eb 100644 --- a/libraries/AP_Scripting/examples/mount-driver.lua +++ b/libraries/AP_Scripting/examples/mount-driver.lua @@ -1,7 +1,7 @@ -- 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 +-- Populate send_target_angles with your gimbal's -- protocol (serial, CAN, etc). This example simulates a gimbal by tracking -- targets internally and reporting them back as attitude. -- @@ -20,6 +20,7 @@ local MOUNT_INSTANCE = 0 -- default to MNT1 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 MOUNT_TARGET_TYPE = {ANGLE=1, RATE=2, RETRACTED=4, NEUTRAL=8, LOCATION=16} -- local variables local sim_state = { @@ -28,7 +29,6 @@ local sim_state = { 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) @@ -59,8 +59,9 @@ local function init() end initialised = true - last_update_ms = millis():tofloat() gcs:send_text(MAV_SEVERITY.INFO, "MountDriver: started") + + mount:set_natively_supported_mount_target_types(MOUNT_INSTANCE, MOUNT_TARGET_TYPE.ANGLE) end -- send target angles (in degrees) to gimbal @@ -81,26 +82,6 @@ local function send_target_angles(roll_ef_deg, pitch_ef_deg, yaw_deg, yaw_is_ef) 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() @@ -110,11 +91,6 @@ local function update() 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 @@ -128,14 +104,6 @@ local function update() 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() diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index f6ff4a8e6ef7d4..c55c9647085bca 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -854,12 +854,14 @@ singleton AR_AttitudeControl method get_srate void float'Ref float'Ref include AP_Mount/AP_Mount.h singleton AP_Mount depends HAL_MOUNT_ENABLED == 1 singleton AP_Mount rename mount + singleton AP_Mount method get_mode MAV_MOUNT_MODE'enum uint8_t'skip_check singleton AP_Mount method set_mode void uint8_t'skip_check MAV_MOUNT_MODE'enum MAV_MOUNT_MODE_RETRACT MAV_MOUNT_MODE_HOME_LOCATION singleton AP_Mount method set_angle_target void uint8_t'skip_check float'skip_check float'skip_check float'skip_check boolean singleton AP_Mount method set_rate_target void uint8_t'skip_check float'skip_check float'skip_check float'skip_check boolean singleton AP_Mount method set_roi_target void uint8_t'skip_check Location singleton AP_Mount method get_attitude_euler boolean uint8_t'skip_check float'Null float'Null float'Null +singleton AP_Mount method set_natively_supported_mount_target_types void uint8_t'skip_check uint8_t'skip_check singleton AP_Mount method get_rate_target boolean uint8_t'skip_check float'Null float'Null float'Null boolean'Null singleton AP_Mount method get_angle_target boolean uint8_t'skip_check float'Null float'Null float'Null boolean'Null singleton AP_Mount method get_location_target boolean uint8_t'skip_check Location'Null