From bc9342a39848669b9fb5b5900d6dc172faea3306 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 11 Apr 2023 16:16:43 +0900 Subject: [PATCH 01/24] AP_Scripting: move mount-poi to applets directory --- libraries/AP_Scripting/{examples => applets}/mount-poi.lua | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename libraries/AP_Scripting/{examples => applets}/mount-poi.lua (100%) diff --git a/libraries/AP_Scripting/examples/mount-poi.lua b/libraries/AP_Scripting/applets/mount-poi.lua similarity index 100% rename from libraries/AP_Scripting/examples/mount-poi.lua rename to libraries/AP_Scripting/applets/mount-poi.lua From c1f9b59d9b798a260b15940817126604a61119f0 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 11 Apr 2023 16:30:36 +0900 Subject: [PATCH 02/24] AP_Scripting: mount-poi gets param description send_text calls also use MAV_SEVERITY definition --- libraries/AP_Scripting/applets/mount-poi.lua | 29 ++++++++++++-------- 1 file changed, 18 insertions(+), 11 deletions(-) diff --git a/libraries/AP_Scripting/applets/mount-poi.lua b/libraries/AP_Scripting/applets/mount-poi.lua index 3c574ef96d5a0..d45398fbcf939 100644 --- a/libraries/AP_Scripting/applets/mount-poi.lua +++ b/libraries/AP_Scripting/applets/mount-poi.lua @@ -21,6 +21,7 @@ -- luacheck: only 0 -- global definitions +local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} local ALT_FRAME_ABSOLUTE = 0 local UPDATE_INTERVAL_MS = 100 @@ -29,8 +30,14 @@ local PARAM_TABLE_KEY = 78 assert(param:add_table(PARAM_TABLE_KEY, "POI_", 1), "could not add param table") assert(param:add_param(PARAM_TABLE_KEY, 1, "DIST_MAX", 10000), "could not add POI_DIST_MAX param") -local poi_dist_max = Parameter() -assert(poi_dist_max:init("POI_DIST_MAX"), "could not find POI_DIST_MAX param") +--[[ + // @Param: POI_DIST_MAX + // @DisplayName: Mount POI distance max + // @Description: POI's max distance (in meters) from the vehicle + // @Range: 0 10000 + // @User: Standard +--]] +local POI_DIST_MAX = Parameter("POI_DIST_MAX") -- bind to other parameters this script depends upon TERRAIN_SPACING = Parameter("TERRAIN_SPACING") @@ -87,7 +94,7 @@ function update() -- find RC channel used to trigger POI rc_switch_ch = rc:find_channel_for_option(300) --scripting ch 1 if (rc_switch_ch == nil) then - gcs:send_text(3, 'MountPOI: RCx_OPTION = 300 not set') -- MAV_SEVERITY_ERROR + gcs:send_text(MAV_SEVERITY.ERROR, 'MountPOI: RCx_OPTION = 300 not set') -- MAV_SEVERITY_ERROR return update, 10000 -- check again in 10 seconds end @@ -108,7 +115,7 @@ function update() -- retrieve vehicle location local vehicle_loc = ahrs:get_location() if vehicle_loc == nil then - gcs:send_text(3, "POI: vehicle pos unavailable") -- MAV_SEVERITY_ERROR + gcs:send_text(MAV_SEVERITY.ERROR, "POI: vehicle pos unavailable") -- MAV_SEVERITY_ERROR return update, UPDATE_INTERVAL_MS end @@ -118,7 +125,7 @@ function update() -- retrieve gimbal attitude local roll_deg, pitch_deg, yaw_bf_deg = mount:get_attitude_euler(0) if pitch_deg == nil or yaw_bf_deg == nil then - gcs:send_text(3, "POI: gimbal attitude unavailable") -- MAV_SEVERITY_ERROR + gcs:send_text(MAV_SEVERITY.ERROR, "POI: gimbal attitude unavailable") -- MAV_SEVERITY_ERROR return update, UPDATE_INTERVAL_MS end @@ -133,7 +140,7 @@ function update() -- fail if terrain alt cannot be retrieved if terrain_amsl_m == nil then - gcs:send_text(3, "POI: failed to get terrain alt") -- MAV_SEVERITY_ERROR + gcs:send_text(MAV_SEVERITY.ERROR, "POI: failed to get terrain alt") -- MAV_SEVERITY_ERROR return update, UPDATE_INTERVAL_MS end @@ -144,7 +151,7 @@ function update() -- initialise total distance test_loc has moved local total_dist = 0 - local dist_max = poi_dist_max:get() + local dist_max = POI_DIST_MAX:get() -- iteratively move test_loc forward until its alt-above-sea-level is below terrain-alt-above-sea-level while (total_dist < dist_max) and ((test_loc:alt() * 0.01) > terrain_amsl_m) do @@ -162,16 +169,16 @@ function update() -- fail if terrain alt cannot be retrieved if terrain_amsl_m == nil then - gcs:send_text(3, "POI: failed to get terrain alt") -- MAV_SEVERITY_ERROR + gcs:send_text(MAV_SEVERITY.ERROR, "POI: failed to get terrain alt") -- MAV_SEVERITY_ERROR return update, UPDATE_INTERVAL_MS end end -- check for errors if (total_dist >= dist_max) then - gcs:send_text(3, "POI: unable to find terrain within " .. tostring(dist_max) .. " m") + gcs:send_text(MAV_SEVERITY.ERROR, "POI: unable to find terrain within " .. tostring(dist_max) .. " m") elseif not terrain_amsl_m then - gcs:send_text(3, "POI: failed to retrieve terrain alt") + gcs:send_text(MAV_SEVERITY.ERROR, "POI: failed to retrieve terrain alt") else -- test location has dropped below terrain -- interpolate along line between prev_test_loc and test_loc @@ -179,7 +186,7 @@ function update() local poi_loc = prev_test_loc:copy() poi_loc:offset_bearing_and_pitch(mount_yaw_ef_deg, mount_pitch_deg, dist_interp_m) local poi_terr_asml_m = terrain:height_amsl(poi_loc, true) - gcs:send_text(6, string.format("POI %.7f, %.7f, %.2f (asml)", poi_loc:lat()/10000000.0, poi_loc:lng()/10000000.0, poi_loc:alt() * 0.01)) + gcs:send_text(MAV_SEVERITY.INFO, string.format("POI %.7f, %.7f, %.2f (asml)", poi_loc:lat()/10000000.0, poi_loc:lng()/10000000.0, poi_loc:alt() * 0.01)) end return update, UPDATE_INTERVAL_MS From 3ab63cc6b1fb98dc908010a7d0c3820d2dbec87e Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 11 Apr 2023 16:34:52 +0900 Subject: [PATCH 03/24] AP_Scripting: add mount-poi md --- libraries/AP_Scripting/applets/mount-poi.md | 30 +++++++++++++++++++++ 1 file changed, 30 insertions(+) create mode 100644 libraries/AP_Scripting/applets/mount-poi.md diff --git a/libraries/AP_Scripting/applets/mount-poi.md b/libraries/AP_Scripting/applets/mount-poi.md new file mode 100644 index 0000000000000..38eef69184ab0 --- /dev/null +++ b/libraries/AP_Scripting/applets/mount-poi.md @@ -0,0 +1,30 @@ +# Mount POI + +This script displays the location (lat, lon and altitude) that the gimbal is currently pointing towards + +# Parameters + +POI_DIST_MAX : POI's max distance (in meters) from the vehicle + +# How To Use + +1. Set RCx_OPTION to 300 (scripting1) to allow triggering the POI calculation from an auxiliary switch +2. Optionally set POI_DIST_MAX to the maximum distance (in meters) that the POI point could be from the vehicle +3. Fly the vehicle and point the camera gimbal at a point on the ground +4. Raise the RC auxiliary switch and check the GCS's messages tab for the latitude, longitude and alt (above sea-level) + +# How It Works + +The script's algorithm is implemented as follows + +1. Get the POI_DIST_MAX and TERRAIN_SPACING parameter values +2. Get the vehicle Location (lat, lon, height above sea-level), initialise test-loc and prev-test-loc +3. Get the vehicle's current alt-above-terrain +4. Get gimbal attitude (only pitch and yaw are used) +5. The test_loc variable is initialised to the vehicle's current location +6. The prev_test_loc variable is a backup of test_loc +7. test_loc is moved along the line defined by the gimbal's pitch and yaw by TERRAIN_SPACING (meters) +8. Get the terrain's altitude (above sea-level) at test_loc +9. Steps 6, 7 and 8 are repeated until test_loc's altitude falls below the terrain altitude +10. Interpolate between test_loc and prev_test_loc to find the lat, lon, alt (above sea-level) where alt-above-terrain is zero +11. Display the POI to the user From f40a939a0cb1928f8d57b8d35dc9e11da86fc27c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 11 Apr 2023 16:38:47 +0900 Subject: [PATCH 04/24] AP_Scripting: fix mount-poi script check errors remove unused variables re-enable script checks --- libraries/AP_Scripting/applets/mount-poi.lua | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/libraries/AP_Scripting/applets/mount-poi.lua b/libraries/AP_Scripting/applets/mount-poi.lua index d45398fbcf939..a0eb1e16d166c 100644 --- a/libraries/AP_Scripting/applets/mount-poi.lua +++ b/libraries/AP_Scripting/applets/mount-poi.lua @@ -18,7 +18,6 @@ -- 9. repeat step 6, 7 and 8 until the test_loc's altitude falls below the terrain altitude -- 10. interpolate between test_loc and prev_test_loc to find the lat, lon, alt (above sea-level) where alt-above-terrain is zero -- 11. display the POI to the user --- luacheck: only 0 -- global definitions local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} @@ -43,8 +42,6 @@ local POI_DIST_MAX = Parameter("POI_DIST_MAX") TERRAIN_SPACING = Parameter("TERRAIN_SPACING") -- local variables and definitions -local user_update_interval_ms = 10000 -- send user updates every 10 sec -local last_user_update_ms = 0 -- system time that update was last sent to user local last_rc_switch_pos = 0 -- last known rc switch position. Used to detect change in RC switch position -- helper functions @@ -88,9 +85,6 @@ end -- the main update function that performs a simplified version of RTL function update() - -- get current system time - local now_ms = millis() - -- find RC channel used to trigger POI rc_switch_ch = rc:find_channel_for_option(300) --scripting ch 1 if (rc_switch_ch == nil) then @@ -123,7 +117,7 @@ function update() vehicle_loc:change_alt_frame(ALT_FRAME_ABSOLUTE) -- retrieve gimbal attitude - local roll_deg, pitch_deg, yaw_bf_deg = mount:get_attitude_euler(0) + local _, pitch_deg, yaw_bf_deg = mount:get_attitude_euler(0) if pitch_deg == nil or yaw_bf_deg == nil then gcs:send_text(MAV_SEVERITY.ERROR, "POI: gimbal attitude unavailable") -- MAV_SEVERITY_ERROR return update, UPDATE_INTERVAL_MS @@ -185,7 +179,6 @@ function update() local dist_interp_m = interpolate(0, dist_increment_m, 0, prev_test_loc:alt() * 0.01 - prev_terrain_amsl_m, test_loc:alt() * 0.01 - terrain_amsl_m) local poi_loc = prev_test_loc:copy() poi_loc:offset_bearing_and_pitch(mount_yaw_ef_deg, mount_pitch_deg, dist_interp_m) - local poi_terr_asml_m = terrain:height_amsl(poi_loc, true) gcs:send_text(MAV_SEVERITY.INFO, string.format("POI %.7f, %.7f, %.2f (asml)", poi_loc:lat()/10000000.0, poi_loc:lng()/10000000.0, poi_loc:alt() * 0.01)) end From 322ef64aad528d80f18def3e5e8e197e9b2410eb Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 12 Apr 2023 14:10:15 +0900 Subject: [PATCH 05/24] Tools: ScriptMountPOI test uses applet directory --- Tools/autotest/arducopter.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 4729468177267..3aafe1a4f3a77 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -9742,7 +9742,7 @@ def ScriptMountPOI(self): self.setup_servo_mount() self.reboot_sitl() self.set_rc(6, 1300) - self.install_example_script_context('mount-poi.lua') + self.install_applet_script_context('mount-poi.lua') self.reboot_sitl() self.wait_ready_to_arm() self.context_collect('STATUSTEXT') From 03148a113f792797fb7e5ca2ab77c9e3c4b25656 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 7 Apr 2023 12:18:23 +0900 Subject: [PATCH 06/24] AP_Scripting: add camera bindings --- libraries/AP_Scripting/docs/docs.lua | 19 +++++++++++++++++++ .../generator/description/bindings.desc | 7 +++++++ 2 files changed, 26 insertions(+) diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index f869d375440ee..d1b1f23f20153 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -975,6 +975,25 @@ function winch:relax() end ---@return boolean function winch:healthy() end +-- desc +---@class camera +camera = {} + +-- desc +---@param instance integer +---@param distance_m number +function camera:set_trigger_distance(instance, distance_m) end + +-- desc +---@param instance integer +---@param start_recording boolean +---@return boolean +function camera:record_video(instance, start_recording) end + +-- desc +---@param instance integer +function camera:take_picture(instance) end + -- desc ---@class mount mount = {} diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 254a5417e7062..1a6d2ba93bd58 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -604,6 +604,13 @@ singleton AP_Mount method get_location_target boolean uint8_t'skip_check Locatio singleton AP_Mount method set_attitude_euler void uint8_t'skip_check float'skip_check float'skip_check float'skip_check singleton AP_Mount method get_camera_state boolean uint8_t'skip_check uint16_t'Null boolean'Null int8_t'Null int8_t'Null boolean'Null +include AP_Camera/AP_Camera.h +singleton AP_Camera depends AP_CAMERA_ENABLED == 1 +singleton AP_Camera rename camera +singleton AP_Camera method take_picture void uint8_t'skip_check +singleton AP_Camera method record_video boolean uint8_t'skip_check boolean +singleton AP_Camera method set_trigger_distance void uint8_t'skip_check float'skip_check + include AP_Winch/AP_Winch.h singleton AP_Winch depends AP_WINCH_ENABLED && APM_BUILD_COPTER_OR_HELI singleton AP_Winch rename winch From 5d29935039595d4f496c84e9781694b168f8d696 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 7 Apr 2023 12:33:16 +0900 Subject: [PATCH 07/24] AP_Scripting: add camera-test example script --- .../AP_Scripting/examples/camera-test.lua | 30 +++++++++++++++++++ 1 file changed, 30 insertions(+) create mode 100644 libraries/AP_Scripting/examples/camera-test.lua diff --git a/libraries/AP_Scripting/examples/camera-test.lua b/libraries/AP_Scripting/examples/camera-test.lua new file mode 100644 index 0000000000000..9dec743e6a472 --- /dev/null +++ b/libraries/AP_Scripting/examples/camera-test.lua @@ -0,0 +1,30 @@ +-- camera-test.lua. Tests triggering taking pictures at regular intervals + +-- global definitions +local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7} +local TAKE_PIC_INTERVAL_MS = 5000 -- take pictures at this interval +local CAMERA_INSTANCE = 0 -- always control the first camera + +-- local variables +local last_takepic_time_ms = 0 -- system time that picture was last taken + +-- the main update function that performs a simplified version of RTL +function update() + + -- get current system time + local now_ms = millis() + + -- check if time to take picture + if (now_ms - last_takepic_time_ms > TAKE_PIC_INTERVAL_MS) then + last_takepic_time_ms = now_ms + camera:take_picture(CAMERA_INSTANCE) + end + + -- update at 10hz + return update, 100 +end + +-- display startup message +gcs:send_text(MAV_SEVERITY.INFO, "camera-test.lua started") + +return update() From 58b73c3613a09da175fb7b101d7f757de7b0f1f8 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 8 Apr 2023 11:33:22 +0900 Subject: [PATCH 08/24] AP_Camera: constify get_instance --- libraries/AP_Camera/AP_Camera.cpp | 2 +- libraries/AP_Camera/AP_Camera.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index d5db4eda2132e..29cff3ae4b4ec 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -431,7 +431,7 @@ bool AP_Camera::set_auto_focus(uint8_t instance) } // return backend for instance number -AP_Camera_Backend *AP_Camera::get_instance(uint8_t instance) +AP_Camera_Backend *AP_Camera::get_instance(uint8_t instance) const { if (instance >= ARRAY_SIZE(_backends)) { return nullptr; diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index 84df4681338c2..7e99d74e26687 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -150,7 +150,7 @@ class AP_Camera { AP_Int16 _max_roll; // Maximum acceptable roll angle when trigging camera // check instance number is valid - AP_Camera_Backend *get_instance(uint8_t instance); + AP_Camera_Backend *get_instance(uint8_t instance) const; // perform any required parameter conversion void convert_params(); From f357bc53fe631d6c7a7c6e5f33b28c3ba37fa384 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 8 Apr 2023 11:35:51 +0900 Subject: [PATCH 09/24] AP_Camera: add scripting backend --- libraries/AP_Camera/AP_Camera.cpp | 20 +++++++ libraries/AP_Camera/AP_Camera.h | 22 ++++++++ libraries/AP_Camera/AP_Camera_Backend.h | 6 ++ libraries/AP_Camera/AP_Camera_Params.cpp | 2 +- libraries/AP_Camera/AP_Camera_Scripting.cpp | 56 +++++++++++++++++++ libraries/AP_Camera/AP_Camera_Scripting.h | 62 +++++++++++++++++++++ libraries/AP_Camera/AP_Camera_config.h | 4 ++ 7 files changed, 171 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_Camera/AP_Camera_Scripting.cpp create mode 100644 libraries/AP_Camera/AP_Camera_Scripting.h diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 29cff3ae4b4ec..872106059a672 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -17,6 +17,7 @@ #include "AP_Camera_Mount.h" #include "AP_Camera_MAVLink.h" #include "AP_Camera_MAVLinkCamV2.h" +#include "AP_Camera_Scripting.h" const AP_Param::GroupInfo AP_Camera::var_info[] = { @@ -170,6 +171,12 @@ void AP_Camera::init() case CameraType::MAVLINK_CAMV2: _backends[instance] = new AP_Camera_MAVLinkCamV2(*this, _params[instance], instance); break; +#endif +#if AP_CAMERA_SCRIPTING_ENABLED + // check for Scripting driver + case CameraType::SCRIPTING: + _backends[instance] = new AP_Camera_Scripting(*this, _params[instance], instance); + break; #endif case CameraType::NONE: break; @@ -430,6 +437,19 @@ bool AP_Camera::set_auto_focus(uint8_t instance) return backend->set_auto_focus(); } +#if AP_CAMERA_SCRIPTING_ENABLED +// accessor to allow scripting backend to retrieve state +// returns true on success and cam_state is filled in +bool AP_Camera::get_state(uint8_t instance, camera_state_t& cam_state) const +{ + auto *backend = get_instance(instance); + if (backend == nullptr) { + return false; + } + return backend->get_state(cam_state); +} +#endif // #if AP_CAMERA_SCRIPTING_ENABLED + // return backend for instance number AP_Camera_Backend *AP_Camera::get_instance(uint8_t instance) const { diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index 7e99d74e26687..28259c6f4214a 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -13,6 +13,17 @@ #include "AP_Camera_Params.h" #include "AP_Mount/AP_Mount_config.h" +#if AP_CAMERA_SCRIPTING_ENABLED + // structure and accessors for use by scripting backends + typedef struct { + uint16_t take_pic_incr; // incremented each time camera is requested to take a picture + bool recording_video; // true when recording video + int8_t zoom_step; // zoom out = -1, hold = 0, zoom in = 1 + int8_t focus_step; // focus in = -1, focus hold = 0, focus out = 1 + bool auto_focus; // true when auto focusing + } camera_state_t; +#endif + #define AP_CAMERA_MAX_INSTANCES 2 // maximum number of camera backends // declare backend classes @@ -23,6 +34,7 @@ class AP_Camera_SoloGimbal; class AP_Camera_Mount; class AP_Camera_MAVLink; class AP_Camera_MAVLinkCamV2; +class AP_Camera_Scripting; /// @class Camera /// @brief Object managing a Photo or video camera @@ -36,6 +48,7 @@ class AP_Camera { friend class AP_Camera_Mount; friend class AP_Camera_MAVLink; friend class AP_Camera_MAVLinkCamV2; + friend class AP_Camera_Scripting; public: @@ -68,6 +81,9 @@ class AP_Camera { #endif #if AP_CAMERA_MAVLINKCAMV2_ENABLED MAVLINK_CAMV2 = 6, // MAVLink camera v2 +#endif +#if AP_CAMERA_SCRIPTING_ENABLED + SCRIPTING = 7, // Scripting backend #endif }; @@ -124,6 +140,12 @@ class AP_Camera { // set if vehicle is in AUTO mode void set_is_auto_mode(bool enable) { _is_in_auto_mode = enable; } +#if AP_CAMERA_SCRIPTING_ENABLED + // accessor to allow scripting backend to retrieve state + // returns true on success and cam_state is filled in + bool get_state(uint8_t instance, camera_state_t& cam_state) const; +#endif + // parameter var table static const struct AP_Param::GroupInfo var_info[]; diff --git a/libraries/AP_Camera/AP_Camera_Backend.h b/libraries/AP_Camera/AP_Camera_Backend.h index bfd3006c1729f..5911d97c5d102 100644 --- a/libraries/AP_Camera/AP_Camera_Backend.h +++ b/libraries/AP_Camera/AP_Camera_Backend.h @@ -82,6 +82,12 @@ class AP_Camera_Backend // send camera feedback message to GCS void send_camera_feedback(mavlink_channel_t chan) const; +#if AP_CAMERA_SCRIPTING_ENABLED + // accessor to allow scripting backend to retrieve state + // returns true on success and cam_state is filled in + virtual bool get_state(camera_state_t& cam_state) const { return false; } +#endif + protected: // references diff --git a/libraries/AP_Camera/AP_Camera_Params.cpp b/libraries/AP_Camera/AP_Camera_Params.cpp index 0ecec96b124ef..cd0240a92e0b2 100644 --- a/libraries/AP_Camera/AP_Camera_Params.cpp +++ b/libraries/AP_Camera/AP_Camera_Params.cpp @@ -8,7 +8,7 @@ const AP_Param::GroupInfo AP_Camera_Params::var_info[] = { // @Param: _TYPE // @DisplayName: Camera shutter (trigger) type // @Description: how to trigger the camera to take a picture - // @Values: 1:Servo,2:Relay, 3:GoPro in Solo Gimbal, 4:Mount (Siyi), 5:MAVLink, 6:MAVLinkCamV2 + // @Values: 1:Servo,2:Relay, 3:GoPro in Solo Gimbal, 4:Mount (Siyi), 5:MAVLink, 6:MAVLinkCamV2, 7:Scripting // @User: Standard AP_GROUPINFO_FLAGS("_TYPE", 1, AP_Camera_Params, type, 0, AP_PARAM_FLAG_ENABLE), diff --git a/libraries/AP_Camera/AP_Camera_Scripting.cpp b/libraries/AP_Camera/AP_Camera_Scripting.cpp new file mode 100644 index 0000000000000..d4996e2cf3020 --- /dev/null +++ b/libraries/AP_Camera/AP_Camera_Scripting.cpp @@ -0,0 +1,56 @@ +#include "AP_Camera_Scripting.h" + +#if AP_CAMERA_SCRIPTING_ENABLED + +extern const AP_HAL::HAL& hal; + +// entry point to actually take a picture +bool AP_Camera_Scripting::trigger_pic() +{ + // increment counter to allow backend to notice request + _cam_state.take_pic_incr++; + return true; +} + +// start or stop video recording. returns true on success +// set start_recording = true to start record, false to stop recording +bool AP_Camera_Scripting::record_video(bool start_recording) +{ + _cam_state.recording_video = start_recording; + return true; +} + +// set camera zoom step. returns true on success +// zoom out = -1, hold = 0, zoom in = 1 +bool AP_Camera_Scripting::set_zoom_step(int8_t zoom_step) +{ + _cam_state.zoom_step = zoom_step; + return true; +} + +// set focus in, out or hold. returns true on success +// focus in = -1, focus hold = 0, focus out = 1 +bool AP_Camera_Scripting::set_manual_focus_step(int8_t focus_step) +{ + _cam_state.focus_step = focus_step; + _cam_state.auto_focus = false; + return true; +} + +// auto focus. returns true on success +bool AP_Camera_Scripting::set_auto_focus() +{ + _cam_state.auto_focus = true; + _cam_state.focus_step = 0; + return true; +} + +// access for scripting backend to retrieve state +// returns true on success and cam_state is filled in +bool AP_Camera_Scripting::get_state(camera_state_t& cam_state) const +{ + cam_state = _cam_state; + return true; +} + +#endif // AP_CAMERA_SCRIPTING_ENABLED diff --git a/libraries/AP_Camera/AP_Camera_Scripting.h b/libraries/AP_Camera/AP_Camera_Scripting.h new file mode 100644 index 0000000000000..3a203a37a289a --- /dev/null +++ b/libraries/AP_Camera/AP_Camera_Scripting.h @@ -0,0 +1,62 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* + Camera driver for scripting backends + */ +#pragma once + +#include "AP_Camera_Backend.h" + +#if AP_CAMERA_SCRIPTING_ENABLED + +class AP_Camera_Scripting : public AP_Camera_Backend +{ +public: + + // Constructor + using AP_Camera_Backend::AP_Camera_Backend; + + /* Do not allow copies */ + CLASS_NO_COPY(AP_Camera_Scripting); + + // entry point to actually take a picture + bool trigger_pic() override; + + // start or stop video recording. returns true on success + // set start_recording = true to start record, false to stop recording + bool record_video(bool start_recording) override; + + // set camera zoom step. returns true on success + // zoom out = -1, hold = 0, zoom in = 1 + bool set_zoom_step(int8_t zoom_step) override; + + // set focus in, out or hold. returns true on success + // focus in = -1, focus hold = 0, focus out = 1 + bool set_manual_focus_step(int8_t focus_step) override; + + // auto focus. returns true on success + bool set_auto_focus() override; + + // returns true on success and cam_state is filled in + bool get_state(camera_state_t& cam_state) const override; + +private: + + // current state + camera_state_t _cam_state; +}; + +#endif // AP_CAMERA_SCRIPTING_ENABLED diff --git a/libraries/AP_Camera/AP_Camera_config.h b/libraries/AP_Camera/AP_Camera_config.h index f814a1524c998..2b8fa5afadb30 100644 --- a/libraries/AP_Camera/AP_Camera_config.h +++ b/libraries/AP_Camera/AP_Camera_config.h @@ -34,3 +34,7 @@ #ifndef AP_CAMERA_SOLOGIMBAL_ENABLED #define AP_CAMERA_SOLOGIMBAL_ENABLED AP_CAMERA_BACKEND_DEFAULT_ENABLED && HAL_SOLO_GIMBAL_ENABLED #endif + +#ifndef AP_CAMERA_SCRIPTING_ENABLED +#define AP_CAMERA_SCRIPTING_ENABLED AP_CAMERA_BACKEND_DEFAULT_ENABLED && AP_SCRIPTING_ENABLED +#endif From 554d52b6fe4939756dfbf4212090d038df994808 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 8 Apr 2023 11:41:57 +0900 Subject: [PATCH 10/24] AP_Scripting: add AP_Camera::get_state bindings --- libraries/AP_Scripting/docs/docs.lua | 32 +++++++++++++++++++ .../generator/description/bindings.desc | 7 ++++ 2 files changed, 39 insertions(+) diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index d1b1f23f20153..c172f7131d908 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -994,6 +994,38 @@ function camera:record_video(instance, start_recording) end ---@param instance integer function camera:take_picture(instance) end +-- desc +---@class camera_state_t_ud +local camera_state_t_ud = {} + +---@return camera_state_t_ud +function camera_state_t() end + +-- get field +---@return boolean +function camera_state_t_ud:auto_focus() end + +-- get field +---@return integer +function camera_state_t_ud:focus_step() end + +-- get field +---@return integer +function camera_state_t_ud:zoom_step() end + +-- get field +---@return boolean +function camera_state_t_ud:recording_video() end + +-- get field +---@return integer +function camera_state_t_ud:take_pic_incr() end + +-- desc +---@param instance integer +---@return camera_state_t_ud|nil +function camera:get_state(instance) end + -- desc ---@class mount mount = {} diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 1a6d2ba93bd58..7dc272e95cd1d 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -610,6 +610,13 @@ singleton AP_Camera rename camera singleton AP_Camera method take_picture void uint8_t'skip_check singleton AP_Camera method record_video boolean uint8_t'skip_check boolean singleton AP_Camera method set_trigger_distance void uint8_t'skip_check float'skip_check +userdata camera_state_t depends (AP_CAMERA_SCRIPTING_ENABLED == 1) +userdata camera_state_t field take_pic_incr uint16_t'skip_check read +userdata camera_state_t field recording_video boolean read +userdata camera_state_t field zoom_step int8_t'skip_check read +userdata camera_state_t field focus_step int8_t'skip_check read +userdata camera_state_t field auto_focus boolean read +singleton AP_Camera method get_state boolean uint8_t'skip_check camera_state_t'Null include AP_Winch/AP_Winch.h singleton AP_Winch depends AP_WINCH_ENABLED && APM_BUILD_COPTER_OR_HELI From 1a375f1ff5355d621078a0268677f91620bdffe8 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 8 Apr 2023 12:11:09 +0900 Subject: [PATCH 11/24] AP_Scripting: remove mount get_camera_state AP_Camera scripting backend provides access --- libraries/AP_Scripting/docs/docs.lua | 9 --------- .../AP_Scripting/generator/description/bindings.desc | 1 - 2 files changed, 10 deletions(-) diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index c172f7131d908..b3d2b90c0917b 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -1030,15 +1030,6 @@ function camera:get_state(instance) end ---@class mount mount = {} --- desc ----@param param1 integer ----@return integer|nil -- pic_count ----@return boolean|nil -- record_video ----@return integer|nil -- zoom_step ----@return integer|nil -- focus_step ----@return boolean|nil -- auto_focus -function mount:get_camera_state(param1) end - -- desc ---@param instance integer ---@param roll_deg number diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 7dc272e95cd1d..c194d6f9ad491 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -602,7 +602,6 @@ singleton AP_Mount method get_rate_target boolean uint8_t'skip_check float'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 singleton AP_Mount method set_attitude_euler void uint8_t'skip_check float'skip_check float'skip_check float'skip_check -singleton AP_Mount method get_camera_state boolean uint8_t'skip_check uint16_t'Null boolean'Null int8_t'Null int8_t'Null boolean'Null include AP_Camera/AP_Camera.h singleton AP_Camera depends AP_CAMERA_ENABLED == 1 From db51915e27e4107ff82569f0b44d7605bf421256 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 8 Apr 2023 12:40:16 +0900 Subject: [PATCH 12/24] AP_Mount: scripting backend loses camera support drivers should use the camera scripting backend --- libraries/AP_Mount/AP_Mount_Scripting.cpp | 51 ----------------------- libraries/AP_Mount/AP_Mount_Scripting.h | 26 ------------ 2 files changed, 77 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Scripting.cpp b/libraries/AP_Mount/AP_Mount_Scripting.cpp index eed7f994b5f77..e1809c4a844cf 100644 --- a/libraries/AP_Mount/AP_Mount_Scripting.cpp +++ b/libraries/AP_Mount/AP_Mount_Scripting.cpp @@ -136,47 +136,6 @@ bool AP_Mount_Scripting::healthy() const return (AP_HAL::millis() - last_update_ms <= AP_MOUNT_SCRIPTING_TIMEOUT_MS); } -// take a picture. returns true on success -bool AP_Mount_Scripting::take_picture() -{ - picture_count++; - recording_video = false; - return true; -} - -// start or stop video recording. returns true on success -// set start_recording = true to start record, false to stop recording -bool AP_Mount_Scripting::record_video(bool start_recording) -{ - recording_video = start_recording; - return true; -} - -// set camera zoom step. returns true on success -// zoom out = -1, hold = 0, zoom in = 1 -bool AP_Mount_Scripting::set_zoom_step(int8_t zoom_step) -{ - manual_zoom_step = zoom_step; - return true; -} - -// set focus in, out or hold. returns true on success -// focus in = -1, focus hold = 0, focus out = 1 -bool AP_Mount_Scripting::set_manual_focus_step(int8_t focus_step) -{ - manual_focus_step = focus_step; - auto_focus_active = false; - return true; -} - -// auto focus. returns true on success -bool AP_Mount_Scripting::set_auto_focus() -{ - manual_focus_step = 0; - auto_focus_active = true; - return true; -} - // accessors for scripting backends bool AP_Mount_Scripting::get_rate_target(float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame) { @@ -222,16 +181,6 @@ void AP_Mount_Scripting::set_attitude_euler(float roll_deg, float pitch_deg, flo current_angle_deg.z = yaw_bf_deg; } -bool AP_Mount_Scripting::get_camera_state(uint16_t& pic_count, bool& record_video, int8_t& zoom_step, int8_t& focus_step, bool& auto_focus) -{ - pic_count = picture_count; - record_video = recording_video; - zoom_step = manual_zoom_step; - focus_step = manual_focus_step; - auto_focus = auto_focus_active; - 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 077a7e4aa3ce6..323b0b19ee208 100644 --- a/libraries/AP_Mount/AP_Mount_Scripting.h +++ b/libraries/AP_Mount/AP_Mount_Scripting.h @@ -34,30 +34,11 @@ class AP_Mount_Scripting : public AP_Mount_Backend // has_pan_control - returns true if this mount can control its pan (required for multicopters) bool has_pan_control() const override { return yaw_range_valid(); }; - // take a picture. returns true on success - bool take_picture() override; - - // start or stop video recording. returns true on success - // set start_recording = true to start record, false to stop recording - bool record_video(bool start_recording) override; - - // set camera zoom step. returns true on success - // zoom out = -1, hold = 0, zoom in = 1 - bool set_zoom_step(int8_t zoom_step) override; - - // set focus in, out or hold. returns true on success - // focus in = -1, focus hold = 0, focus out = 1 - bool set_manual_focus_step(int8_t focus_step) override; - - // auto focus. returns true on success - bool set_auto_focus() override; - // accessors for scripting backends bool get_rate_target(float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame) override; bool get_angle_target(float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame) override; bool get_location_target(Location& _target_loc) override; void set_attitude_euler(float roll_deg, float pitch_deg, float yaw_bf_deg) override; - bool get_camera_state(uint16_t& pic_count, bool& record_video, int8_t& zoom_step, int8_t& focus_step, bool& auto_focus) override; protected: @@ -78,13 +59,6 @@ class AP_Mount_Scripting : public AP_Mount_Backend Location target_loc; // target location bool target_loc_valid; // true if target_loc holds a valid target location - - // camera related internal variables - uint16_t picture_count; // number of pictures that have been taken (or at least requested) - bool recording_video; // true when record video has been requested - int8_t manual_zoom_step; // manual zoom step. zoom out = -1, hold = 0, zoom in = 1 - int8_t manual_focus_step; // manual focus step. focus in = -1, focus hold = 0, focus out = 1 - bool auto_focus_active; // auto focus active }; #endif // HAL_MOUNT_SIYISERIAL_ENABLED From ffc3ad1c1a3fe7d71a0fe6ac51df8b766a3f5bd5 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 7 Apr 2023 09:51:54 +0900 Subject: [PATCH 13/24] AP_Scripting: viewpro driver uses camera scripting backend --- .../drivers/mount-viewpro-driver.lua | 44 ++++++++++++------- .../drivers/mount-viewpro-driver.md | 2 +- 2 files changed, 29 insertions(+), 17 deletions(-) diff --git a/libraries/AP_Scripting/drivers/mount-viewpro-driver.lua b/libraries/AP_Scripting/drivers/mount-viewpro-driver.lua index a1e3b1e8dd52f..ebd825891cc5e 100644 --- a/libraries/AP_Scripting/drivers/mount-viewpro-driver.lua +++ b/libraries/AP_Scripting/drivers/mount-viewpro-driver.lua @@ -6,7 +6,7 @@ Set SERIALx_PROTOCOL = 28 (Scripting) where "x" corresponds to the serial port connected to the gimbal Set SCR_ENABLE = 1 to enable scripting and reboot the autopilot Set MNT1_TYPE = 9 (Scripting) to enable the mount/gimbal scripting driver - Set CAM1_TYPE = 4 (Mount) to enable the camera driver + Set CAM1_TYPE = 7 (Scripting) to enable the camera scripting driver Set RCx_OPTION = 300 (Scripting1) to allow real-time selection of the video feed and camera control Copy this script to the autopilot's SD card in the APM/scripts directory and reboot the autopilot Set VIEP_CAM_SWLOW, VIEP_CAM_SWMID, VIEP_CAM_SWHIGH to which cameras are controlled by the auxiliary switch @@ -93,6 +93,7 @@ local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTI local INIT_INTERVAL_MS = 3000 -- attempt to initialise the gimbal at this interval local UPDATE_INTERVAL_MS = 100 -- update at 10hz local MOUNT_INSTANCE = 0 -- always control the first mount/gimbal +local CAMERA_INSTANCE = 0 -- always use the first camera -- packet parsing definitions local HEADER1 = 0x55 -- 1st header byte @@ -213,6 +214,12 @@ function init() do return end end + -- check cam type parametr + if CAM1_TYPE:get() ~= 7 then + gcs:send_text(MAV_SEVERITY.CRITICAL, "ViewPro: set CAM1_TYPE=7") + do return end + end + -- find and init first instance of SERIALx_PROTOCOL = 28 (Scripting) uart = serial:find_serial(0) if uart == nil then @@ -504,12 +511,15 @@ function check_camera_state() send_camera_control(cam_choice, CAM_COMMAND_NO_ACTION) end - -- get latest state from AP driver - local pic_count, rec_video, zoom_step, focus_step, auto_focus = mount:get_camera_state(MOUNT_INSTANCE) + -- get latest camera state from AP driver + local cam_state = camera:get_state(CAMERA_INSTANCE) + if not cam_state then + return + end -- check for take picture - if pic_count and pic_count ~= cam_pic_count then - cam_pic_count = pic_count + if cam_state:take_pic_incr() and cam_state:take_pic_incr() ~= cam_pic_count then + cam_pic_count = cam_state:take_pic_incr() send_camera_control(cam_choice, CAM_COMMAND_TAKE_PICTURE) if VIEP_DEBUG:get() > 0 then gcs:send_text(MAV_SEVERITY.INFO, string.format("ViewPro: took pic %u", pic_count)) @@ -517,22 +527,24 @@ function check_camera_state() end -- check for start/stop recording video - if rec_video ~= cam_rec_video then - cam_rec_video = rec_video - if cam_rec_video == true then + if cam_state:recording_video() ~= cam_rec_video then + cam_rec_video = cam_state:recording_video() + if cam_rec_video > 0 then send_camera_control(cam_choice, CAM_COMMAND_START_RECORD) - elseif cam_rec_video == false then + gcs:send_text(MAV_SEVERITY.INFO, "ViewPro: start recording") + else send_camera_control(cam_choice, CAM_COMMAND_STOP_RECORD) + gcs:send_text(MAV_SEVERITY.INFO, "ViewPro: stop recording") end if VIEP_DEBUG:get() > 0 then gcs:send_text(MAV_SEVERITY.INFO, "ViewPro: rec video:" .. tostring(cam_rec_video)) end end - + -- check manual zoom -- zoom out = -1, hold = 0, zoom in = 1 - if zoom_step ~= cam_zoom_step then - cam_zoom_step = zoom_step + if cam_state:zoom_step() and cam_state:zoom_step() ~= cam_zoom_step then + cam_zoom_step = cam_state:zoom_step() if cam_zoom_step < 0 then send_camera_control(cam_choice, CAM_COMMAND_ZOOM_OUT) elseif cam_zoom_step == 0 then @@ -547,8 +559,8 @@ function check_camera_state() -- check manual focus -- focus in = -1, focus hold = 0, focus out = 1 - if focus_step ~= cam_focus_step then - cam_focus_step = focus_step + if cam_state:focus_step() and cam_state:focus_step() ~= cam_focus_step then + cam_focus_step = cam_state:focus_step() if cam_focus_step < 0 then send_camera_control(cam_choice, CAM_COMMAND_MANUAL_FOCUS) send_camera_control(cam_choice, CAM_COMMAND_FOCUS_MINUS) @@ -564,8 +576,8 @@ function check_camera_state() end -- check auto focus - if auto_focus ~= cam_autofocus then - cam_autofocus = auto_focus + if cam_state:auto_focus() and cam_state:auto_focus() ~= cam_autofocus then + cam_autofocus = cam_state:auto_focus() if cam_autofocus then send_camera_control(cam_choice, CAM_COMMAND_AUTO_FOCUS) end diff --git a/libraries/AP_Scripting/drivers/mount-viewpro-driver.md b/libraries/AP_Scripting/drivers/mount-viewpro-driver.md index 1c2f3609388dd..cd27ae537a5d5 100644 --- a/libraries/AP_Scripting/drivers/mount-viewpro-driver.md +++ b/libraries/AP_Scripting/drivers/mount-viewpro-driver.md @@ -8,7 +8,7 @@ Viewpro gimbal driver lua script Set SERIALx_PROTOCOL = 28 (Scripting) where "x" corresponds to the serial port connected to the gimbal Set SCR_ENABLE = 1 to enable scripting and reboot the autopilot Set MNT1_TYPE = 9 (Scripting) to enable the mount/gimbal scripting driver - Set CAM1_TYPE = 4 (Mount) to enable camera control using the mount driver + Set CAM1_TYPE = 7 (Scripting) to enable camera control using the scripting driver Set RCx_OPTION = 300 (Scripting1) to allow real-time selection of the video feed and camera control Copy this script to the autopilot's SD card in the APM/scripts directory and reboot the autopilot Set VIEP_CAM_SWLOW, VIEP_CAM_SWMID, VIEP_CAM_SWHIGH to which cameras are controlled by the auxiliary switch From de4b1890f5a3d1527fa5536c58eb8535baa44109 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 12 Apr 2023 09:11:26 +0900 Subject: [PATCH 14/24] AP_Camera: add semaphore to allow multi-threaded access --- libraries/AP_Camera/AP_Camera.cpp | 48 ++++++++++++++++++++- libraries/AP_Camera/AP_Camera.h | 8 +++- libraries/AP_Camera/AP_Camera_Backend.cpp | 2 +- libraries/AP_Camera/AP_Camera_Backend.h | 4 +- libraries/AP_Camera/AP_Camera_Scripting.cpp | 2 +- libraries/AP_Camera/AP_Camera_Scripting.h | 2 +- 6 files changed, 57 insertions(+), 9 deletions(-) diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 872106059a672..0dfbb10f08f8a 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -61,6 +61,8 @@ AP_Camera::AP_Camera(uint32_t _log_camera_bit) : // set camera trigger distance in a mission void AP_Camera::set_trigger_distance(float distance_m) { + WITH_SEMAPHORE(_rsem); + if (primary == nullptr) { return; } @@ -70,6 +72,8 @@ void AP_Camera::set_trigger_distance(float distance_m) // momentary switch to change camera between picture and video modes void AP_Camera::cam_mode_toggle() { + WITH_SEMAPHORE(_rsem); + if (primary == nullptr) { return; } @@ -79,6 +83,8 @@ void AP_Camera::cam_mode_toggle() // take a picture void AP_Camera::take_picture() { + WITH_SEMAPHORE(_rsem); + if (primary == nullptr) { return; } @@ -89,6 +95,8 @@ void AP_Camera::take_picture() // start_recording should be true to start recording, false to stop recording bool AP_Camera::record_video(bool start_recording) { + WITH_SEMAPHORE(_rsem); + if (primary == nullptr) { return false; } @@ -99,6 +107,8 @@ bool AP_Camera::record_video(bool start_recording) // zoom out = -1, hold = 0, zoom in = 1 bool AP_Camera::set_zoom_step(int8_t zoom_step) { + WITH_SEMAPHORE(_rsem); + if (primary == nullptr) { return false; } @@ -109,6 +119,8 @@ bool AP_Camera::set_zoom_step(int8_t zoom_step) // focus in = -1, focus hold = 0, focus out = 1 bool AP_Camera::set_manual_focus_step(int8_t focus_step) { + WITH_SEMAPHORE(_rsem); + if (primary == nullptr) { return false; } @@ -118,6 +130,8 @@ bool AP_Camera::set_manual_focus_step(int8_t focus_step) // auto focus bool AP_Camera::set_auto_focus() { + WITH_SEMAPHORE(_rsem); + if (primary == nullptr) { return false; } @@ -199,6 +213,8 @@ void AP_Camera::init() // handle incoming mavlink messages void AP_Camera::handle_message(mavlink_channel_t chan, const mavlink_message_t &msg) { + WITH_SEMAPHORE(_rsem); + if (msg.msgid == MAVLINK_MSG_ID_DIGICAM_CONTROL) { // decode deprecated MavLink message that controls camera. __mavlink_digicam_control_t packet; @@ -287,6 +303,8 @@ MAV_RESULT AP_Camera::handle_command_long(const mavlink_command_long_t &packet) // set camera trigger distance in a mission void AP_Camera::set_trigger_distance(uint8_t instance, float distance_m) { + WITH_SEMAPHORE(_rsem); + auto *backend = get_instance(instance); if (backend == nullptr) { return; @@ -299,6 +317,8 @@ void AP_Camera::set_trigger_distance(uint8_t instance, float distance_m) // momentary switch to change camera between picture and video modes void AP_Camera::cam_mode_toggle(uint8_t instance) { + WITH_SEMAPHORE(_rsem); + auto *backend = get_instance(instance); if (backend == nullptr) { return; @@ -311,6 +331,8 @@ void AP_Camera::cam_mode_toggle(uint8_t instance) // configure camera void AP_Camera::configure(float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time) { + WITH_SEMAPHORE(_rsem); + if (primary == nullptr) { return; } @@ -319,6 +341,8 @@ void AP_Camera::configure(float shooting_mode, float shutter_speed, float apertu void AP_Camera::configure(uint8_t instance, float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time) { + WITH_SEMAPHORE(_rsem); + auto *backend = get_instance(instance); if (backend == nullptr) { return; @@ -331,6 +355,8 @@ void AP_Camera::configure(uint8_t instance, float shooting_mode, float shutter_s // handle camera control void AP_Camera::control(float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id) { + WITH_SEMAPHORE(_rsem); + if (primary == nullptr) { return; } @@ -339,6 +365,8 @@ void AP_Camera::control(float session, float zoom_pos, float zoom_step, float fo void AP_Camera::control(uint8_t instance, float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id) { + WITH_SEMAPHORE(_rsem); + auto *backend = get_instance(instance); if (backend == nullptr) { return; @@ -351,8 +379,10 @@ void AP_Camera::control(uint8_t instance, float session, float zoom_pos, float z /* Send camera feedback to the GCS */ -void AP_Camera::send_feedback(mavlink_channel_t chan) const +void AP_Camera::send_feedback(mavlink_channel_t chan) { + WITH_SEMAPHORE(_rsem); + // call each instance for (uint8_t instance = 0; instance < AP_CAMERA_MAX_INSTANCES; instance++) { if (_backends[instance] != nullptr) { @@ -366,6 +396,8 @@ void AP_Camera::send_feedback(mavlink_channel_t chan) const */ void AP_Camera::update() { + WITH_SEMAPHORE(_rsem); + // call each instance for (uint8_t instance = 0; instance < AP_CAMERA_MAX_INSTANCES; instance++) { if (_backends[instance] != nullptr) { @@ -377,6 +409,8 @@ void AP_Camera::update() // take_picture - take a picture void AP_Camera::take_picture(uint8_t instance) { + WITH_SEMAPHORE(_rsem); + auto *backend = get_instance(instance); if (backend == nullptr) { return; @@ -390,6 +424,8 @@ void AP_Camera::take_picture(uint8_t instance) // start_recording should be true to start recording, false to stop recording bool AP_Camera::record_video(uint8_t instance, bool start_recording) { + WITH_SEMAPHORE(_rsem); + auto *backend = get_instance(instance); if (backend == nullptr) { return false; @@ -403,6 +439,8 @@ bool AP_Camera::record_video(uint8_t instance, bool start_recording) // zoom out = -1, hold = 0, zoom in = 1 bool AP_Camera::set_zoom_step(uint8_t instance, int8_t zoom_step) { + WITH_SEMAPHORE(_rsem); + auto *backend = get_instance(instance); if (backend == nullptr) { return false; @@ -416,6 +454,8 @@ bool AP_Camera::set_zoom_step(uint8_t instance, int8_t zoom_step) // focus in = -1, focus hold = 0, focus out = 1 bool AP_Camera::set_manual_focus_step(uint8_t instance, int8_t focus_step) { + WITH_SEMAPHORE(_rsem); + auto *backend = get_instance(instance); if (backend == nullptr) { return false; @@ -428,6 +468,8 @@ bool AP_Camera::set_manual_focus_step(uint8_t instance, int8_t focus_step) // auto focus. returns true on success bool AP_Camera::set_auto_focus(uint8_t instance) { + WITH_SEMAPHORE(_rsem); + auto *backend = get_instance(instance); if (backend == nullptr) { return false; @@ -440,8 +482,10 @@ bool AP_Camera::set_auto_focus(uint8_t instance) #if AP_CAMERA_SCRIPTING_ENABLED // accessor to allow scripting backend to retrieve state // returns true on success and cam_state is filled in -bool AP_Camera::get_state(uint8_t instance, camera_state_t& cam_state) const +bool AP_Camera::get_state(uint8_t instance, camera_state_t& cam_state) { + WITH_SEMAPHORE(_rsem); + auto *backend = get_instance(instance); if (backend == nullptr) { return false; diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index 28259c6f4214a..946c82824d449 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -96,7 +96,7 @@ class AP_Camera { // MAVLink methods void handle_message(mavlink_channel_t chan, const mavlink_message_t &msg); MAV_RESULT handle_command_long(const mavlink_command_long_t &packet); - void send_feedback(mavlink_channel_t chan) const; + void send_feedback(mavlink_channel_t chan); // configure camera void configure(float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time); @@ -143,9 +143,12 @@ class AP_Camera { #if AP_CAMERA_SCRIPTING_ENABLED // accessor to allow scripting backend to retrieve state // returns true on success and cam_state is filled in - bool get_state(uint8_t instance, camera_state_t& cam_state) const; + bool get_state(uint8_t instance, camera_state_t& cam_state); #endif + // allow threads to lock against AHRS update + HAL_Semaphore &get_semaphore() { return _rsem; } + // parameter var table static const struct AP_Param::GroupInfo var_info[]; @@ -177,6 +180,7 @@ class AP_Camera { // perform any required parameter conversion void convert_params(); + HAL_Semaphore _rsem; // semaphore for multi-thread access AP_Camera_Backend *primary; // primary camera backed bool _is_in_auto_mode; // true if in AUTO mode uint32_t log_camera_bit; // logging bit (from LOG_BITMASK) to enable camera logging diff --git a/libraries/AP_Camera/AP_Camera_Backend.cpp b/libraries/AP_Camera/AP_Camera_Backend.cpp index f0c8044a0e85e..ffe266e4294de 100644 --- a/libraries/AP_Camera/AP_Camera_Backend.cpp +++ b/libraries/AP_Camera/AP_Camera_Backend.cpp @@ -107,7 +107,7 @@ void AP_Camera_Backend::control(float session, float zoom_pos, float zoom_step, } // send camera feedback message to GCS -void AP_Camera_Backend::send_camera_feedback(mavlink_channel_t chan) const +void AP_Camera_Backend::send_camera_feedback(mavlink_channel_t chan) { int32_t altitude = 0; if (camera_feedback.location.initialised() && !camera_feedback.location.get_alt_cm(Location::AltFrame::ABSOLUTE, altitude)) { diff --git a/libraries/AP_Camera/AP_Camera_Backend.h b/libraries/AP_Camera/AP_Camera_Backend.h index 5911d97c5d102..4db6dbebfb074 100644 --- a/libraries/AP_Camera/AP_Camera_Backend.h +++ b/libraries/AP_Camera/AP_Camera_Backend.h @@ -80,12 +80,12 @@ class AP_Camera_Backend void set_trigger_distance(float distance_m) { _params.trigg_dist.set(distance_m); } // send camera feedback message to GCS - void send_camera_feedback(mavlink_channel_t chan) const; + void send_camera_feedback(mavlink_channel_t chan); #if AP_CAMERA_SCRIPTING_ENABLED // accessor to allow scripting backend to retrieve state // returns true on success and cam_state is filled in - virtual bool get_state(camera_state_t& cam_state) const { return false; } + virtual bool get_state(camera_state_t& cam_state) { return false; } #endif protected: diff --git a/libraries/AP_Camera/AP_Camera_Scripting.cpp b/libraries/AP_Camera/AP_Camera_Scripting.cpp index d4996e2cf3020..5da821735436a 100644 --- a/libraries/AP_Camera/AP_Camera_Scripting.cpp +++ b/libraries/AP_Camera/AP_Camera_Scripting.cpp @@ -47,7 +47,7 @@ bool AP_Camera_Scripting::set_auto_focus() // access for scripting backend to retrieve state // returns true on success and cam_state is filled in -bool AP_Camera_Scripting::get_state(camera_state_t& cam_state) const +bool AP_Camera_Scripting::get_state(camera_state_t& cam_state) { cam_state = _cam_state; return true; diff --git a/libraries/AP_Camera/AP_Camera_Scripting.h b/libraries/AP_Camera/AP_Camera_Scripting.h index 3a203a37a289a..3a6f77de6dd82 100644 --- a/libraries/AP_Camera/AP_Camera_Scripting.h +++ b/libraries/AP_Camera/AP_Camera_Scripting.h @@ -51,7 +51,7 @@ class AP_Camera_Scripting : public AP_Camera_Backend bool set_auto_focus() override; // returns true on success and cam_state is filled in - bool get_state(camera_state_t& cam_state) const override; + bool get_state(camera_state_t& cam_state) override; private: From 1a9189d602debc5ff03a7c4218d45670d00f8224 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 12 Apr 2023 09:11:49 +0900 Subject: [PATCH 15/24] AP_Scripting: add binding for camera semaphore --- libraries/AP_Scripting/generator/description/bindings.desc | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index c194d6f9ad491..d26217b2fa319 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -606,6 +606,7 @@ singleton AP_Mount method set_attitude_euler void uint8_t'skip_check float'skip_ include AP_Camera/AP_Camera.h singleton AP_Camera depends AP_CAMERA_ENABLED == 1 singleton AP_Camera rename camera +singleton AP_Camera semaphore singleton AP_Camera method take_picture void uint8_t'skip_check singleton AP_Camera method record_video boolean uint8_t'skip_check boolean singleton AP_Camera method set_trigger_distance void uint8_t'skip_check float'skip_check From 80ed6125aabc8db1738e737a2cb3966b1574d149 Mon Sep 17 00:00:00 2001 From: Ryan Friedman Date: Wed, 5 Apr 2023 00:16:36 -0600 Subject: [PATCH 16/24] AP_DDS: Add support for static transforms * This encodes the position of the GPS receivers relative to the aircraft origin Signed-off-by: Ryan Friedman --- libraries/AP_DDS/AP_DDS_Client.cpp | 69 +++++++++++++++++++++++ libraries/AP_DDS/AP_DDS_Client.h | 10 +++- libraries/AP_DDS/AP_DDS_Topic_Table.h | 11 ++++ libraries/AP_DDS/Idl/TFMessage.idl | 13 +++++ libraries/AP_DDS/Idl/Transform.idl | 18 ++++++ libraries/AP_DDS/Idl/TransformStamped.idl | 35 ++++++++++++ libraries/AP_DDS/README.md | 14 ++++- libraries/AP_DDS/dds_xrce_profile.xml | 28 +++++++++ libraries/AP_DDS/gen_config_h.py | 2 +- libraries/AP_DDS/wscript | 3 + 10 files changed, 197 insertions(+), 6 deletions(-) create mode 100644 libraries/AP_DDS/Idl/TFMessage.idl create mode 100644 libraries/AP_DDS/Idl/Transform.idl create mode 100644 libraries/AP_DDS/Idl/TransformStamped.idl diff --git a/libraries/AP_DDS/AP_DDS_Client.cpp b/libraries/AP_DDS/AP_DDS_Client.cpp index 8dacb4825397f..db47b5ed7ecf2 100644 --- a/libraries/AP_DDS/AP_DDS_Client.cpp +++ b/libraries/AP_DDS/AP_DDS_Client.cpp @@ -3,6 +3,7 @@ #if AP_DDS_ENABLED #include +#include #include #include #include @@ -10,9 +11,12 @@ #include "AP_DDS_Client.h" #include "generated/Time.h" + static constexpr uint16_t DELAY_TIME_TOPIC_MS = 10; static constexpr uint16_t DELAY_NAV_SAT_FIX_TOPIC_MS = 1000; static char WGS_84_FRAME_ID[] = "WGS-84"; +// https://www.ros.org/reps/rep-0105.html#base-link +static char BASE_LINK_FRAME_ID[] = "base_link"; AP_HAL::UARTDriver *dds_port; @@ -120,6 +124,52 @@ void AP_DDS_Client::update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t i msg.position_covariance[8] = vdopSq; } +#include "generated/TransformStamped.h" + +void AP_DDS_Client::populate_static_transforms(tf2_msgs_msg_TFMessage& msg) +{ + msg.transforms_size = 0; + + auto &gps = AP::gps(); + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + const auto gps_type = gps.get_type(i); + if (gps_type == AP_GPS::GPS_Type::GPS_TYPE_NONE) { + continue; + } + update_topic(msg.transforms[i].header.stamp); + char gps_frame_id[16]; + //! @todo should GPS frame ID's be 0 or 1 indexed in ROS? + hal.util->snprintf(gps_frame_id, sizeof(gps_frame_id), "GPS_%u", i); + strcpy(msg.transforms[i].header.frame_id, BASE_LINK_FRAME_ID); + strcpy(msg.transforms[i].child_frame_id, gps_frame_id); + // The body-frame offsets + // X - Forward + // Y - Right + // Z - Down + // https://ardupilot.org/copter/docs/common-sensor-offset-compensation.html#sensor-position-offset-compensation + + const auto offset = gps.get_antenna_offset(i); + + // In ROS REP 103, it follows this convention + // X - Forward + // Y - Left + // Z - Up + // https://www.ros.org/reps/rep-0103.html#axis-orientation + + msg.transforms[i].transform.translation.x = offset[0]; + msg.transforms[i].transform.translation.y = -1 * offset[1]; + msg.transforms[i].transform.translation.z = -1 * offset[2]; + + msg.transforms_size++; + } + + // msg.transforms[0] = transform; + + + + // const auto offset = AP::GPS:: +} + /* class constructor @@ -143,6 +193,10 @@ void AP_DDS_Client::main_loop(void) return; } GCS_SEND_TEXT(MAV_SEVERITY_INFO,"DDS Client: Initialization passed"); + + populate_static_transforms(static_transforms_topic); + write_static_transforms(); + while (true) { hal.scheduler->delay(1); update(); @@ -280,6 +334,21 @@ void AP_DDS_Client::write_nav_sat_fix_topic() } } +void AP_DDS_Client::write_static_transforms() +{ + WITH_SEMAPHORE(csem); + if (connected) { + ucdrBuffer ub; + const uint32_t topic_size = tf2_msgs_msg_TFMessage_size_of_topic(&static_transforms_topic, 0); + uxr_prepare_output_stream(&session,reliable_out,topics[2].dw_id,&ub,topic_size); + const bool success = tf2_msgs_msg_TFMessage_serialize_topic(&ub, &static_transforms_topic); + if (!success) { + // TODO sometimes serialization fails on bootup. Determine why. + // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n"); + } + } +} + void AP_DDS_Client::update() { WITH_SEMAPHORE(csem); diff --git a/libraries/AP_DDS/AP_DDS_Client.h b/libraries/AP_DDS/AP_DDS_Client.h index 3037cceb0f889..c44e0f825f03a 100644 --- a/libraries/AP_DDS/AP_DDS_Client.h +++ b/libraries/AP_DDS/AP_DDS_Client.h @@ -7,6 +7,8 @@ #include "generated/Time.h" #include "AP_DDS_Generic_Fn_T.h" #include "generated/NavSatFix.h" +#include "generated/TFMessage.h" + #include #include @@ -41,6 +43,7 @@ class AP_DDS_Client // Topic builtin_interfaces_msg_Time time_topic; sensor_msgs_msg_NavSatFix nav_sat_fix_topic; + tf2_msgs_msg_TFMessage static_transforms_topic; HAL_Semaphore csem; @@ -49,6 +52,7 @@ class AP_DDS_Client static void update_topic(builtin_interfaces_msg_Time& msg); static void update_topic(sensor_msgs_msg_NavSatFix& msg, const uint8_t instance); + static void populate_static_transforms(tf2_msgs_msg_TFMessage& msg); // The last ms timestamp AP_DDS wrote a Time message uint64_t last_time_time_ms; @@ -71,10 +75,12 @@ class AP_DDS_Client //! @return True on successful creation, false on failure bool create() WARN_IF_UNUSED; - //! @brief Serialize the current time state and publish to to the IO stream(s) + //! @brief Serialize the current time state and publish to the IO stream(s) void write_time_topic(); - //! @brief Serialize the current nav_sat_fix state and publish to to the IO stream(s) + //! @brief Serialize the current nav_sat_fix state and publish to the IO stream(s) void write_nav_sat_fix_topic(); + //! @brief Serialize the static transforms and publish to the IO stream(s) + void write_static_transforms(); //! @brief Update the internally stored DDS messages with latest data void update(); diff --git a/libraries/AP_DDS/AP_DDS_Topic_Table.h b/libraries/AP_DDS/AP_DDS_Topic_Table.h index 1b8b12badba49..d7d7af3f5c1f9 100644 --- a/libraries/AP_DDS/AP_DDS_Topic_Table.h +++ b/libraries/AP_DDS/AP_DDS_Topic_Table.h @@ -1,5 +1,6 @@ #include "generated/Time.h" #include "generated/NavSatFix.h" +#include "generated/TransformStamped.h" #include "AP_DDS_Generic_Fn_T.h" @@ -31,4 +32,14 @@ const struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = { .deserialize = Generic_deserialize_topic_fn_t(&sensor_msgs_msg_NavSatFix_deserialize_topic), .size_of = Generic_size_of_topic_fn_t(&sensor_msgs_msg_NavSatFix_size_of_topic), }, + { + .topic_id = 0x03, + .pub_id = 0x03, + .dw_id = uxrObjectId{.id=0x03, .type=UXR_DATAWRITER_ID}, + .topic_profile_label = "statictransforms__t", + .dw_profile_label = "statictransforms__dw", + .serialize = Generic_serialize_topic_fn_t(&tf2_msgs_msg_TFMessage_serialize_topic), + .deserialize = Generic_deserialize_topic_fn_t(&tf2_msgs_msg_TFMessage_deserialize_topic), + .size_of = Generic_size_of_topic_fn_t(&tf2_msgs_msg_TFMessage_size_of_topic), + }, }; diff --git a/libraries/AP_DDS/Idl/TFMessage.idl b/libraries/AP_DDS/Idl/TFMessage.idl new file mode 100644 index 0000000000000..69fd27bdde02a --- /dev/null +++ b/libraries/AP_DDS/Idl/TFMessage.idl @@ -0,0 +1,13 @@ +// generated from rosidl_adapter/resource/msg.idl.em +// with input from tf2_msgs/msg/TFMessage.msg +// generated code does not contain a copyright notice + +#include "TransformStamped.idl" + +module tf2_msgs { + module msg { + struct TFMessage { + sequence transforms; + }; + }; +}; \ No newline at end of file diff --git a/libraries/AP_DDS/Idl/Transform.idl b/libraries/AP_DDS/Idl/Transform.idl new file mode 100644 index 0000000000000..abcdbe435e1eb --- /dev/null +++ b/libraries/AP_DDS/Idl/Transform.idl @@ -0,0 +1,18 @@ +// generated from rosidl_adapter/resource/msg.idl.em +// with input from geometry_msgs/msg/Transform.msg +// generated code does not contain a copyright notice + +#include "Quaternion.idl" +#include "Vector3.idl" + +module geometry_msgs { + module msg { + @verbatim (language="comment", text= + "This represents the transform between two coordinate frames in free space.") + struct Transform { + geometry_msgs::msg::Vector3 translation; + + geometry_msgs::msg::Quaternion rotation; + }; + }; +}; \ No newline at end of file diff --git a/libraries/AP_DDS/Idl/TransformStamped.idl b/libraries/AP_DDS/Idl/TransformStamped.idl new file mode 100644 index 0000000000000..4a655484ee37c --- /dev/null +++ b/libraries/AP_DDS/Idl/TransformStamped.idl @@ -0,0 +1,35 @@ +// generated from rosidl_adapter/resource/msg.idl.em +// with input from geometry_msgs/msg/TransformStamped.msg +// generated code does not contain a copyright notice + +#include "Transform.idl" +#include "Header.idl" + +module geometry_msgs { + module msg { + @verbatim (language="comment", text= + "This expresses a transform from coordinate frame header.frame_id" "\n" + "to the coordinate frame child_frame_id at the time of header.stamp" "\n" + "" "\n" + "This message is mostly used by the" "\n" + "tf2 package." "\n" + "See its documentation for more information." "\n" + "" "\n" + "The child_frame_id is necessary in addition to the frame_id" "\n" + "in the Header to communicate the full reference for the transform" "\n" + "in a self contained message.") + struct TransformStamped { + @verbatim (language="comment", text= + "The frame id in the header is used as the reference frame of this transform.") + std_msgs::msg::Header header; + + @verbatim (language="comment", text= + "The frame id of the child frame to which this transform points.") + string child_frame_id; + + @verbatim (language="comment", text= + "Translation and rotation in 3-dimensions of child_frame_id from header.frame_id.") + geometry_msgs::msg::Transform transform; + }; + }; +}; \ No newline at end of file diff --git a/libraries/AP_DDS/README.md b/libraries/AP_DDS/README.md index 1dbb8bf712e11..9483f3e70117a 100644 --- a/libraries/AP_DDS/README.md +++ b/libraries/AP_DDS/README.md @@ -146,9 +146,11 @@ After your setups are complete, do the following: $ ros2 topic list -v Published topics: - * /ROS2_Time [builtin_interfaces/msg/Time] 1 publisher - * /parameter_events [rcl_interfaces/msg/ParameterEvent] 1 publisher - * /rosout [rcl_interfaces/msg/Log] 1 publisher + * /ROS2_NavSatFix0 [sensor_msgs/msg/NavSatFix] 1 publisher + * /ROS2_Time [builtin_interfaces/msg/Time] 1 publisher + * /parameter_events [rcl_interfaces/msg/ParameterEvent] 1 publisher + * /rosout [rcl_interfaces/msg/Log] 1 publisher + * /tf [tf2_msgs/msg/TFMessage] 1 publisher Subscribed topics: @@ -162,6 +164,12 @@ After your setups are complete, do the following: --- ``` + The static transforms for enabled sensors are also published, and can be recieved like so: + ```console + ros2 topic echo /tf --qos-depth 1 --qos-history keep_last --qos-reliability reliable --qos-durability transient_local --once + ``` + In order to consume the transforms, it's highly recommended to [create and run a transform broadcaster in ROS 2](https://docs.ros.org/en/humble/Concepts/About-Tf2.html#tutorials). + ## Adding DDS messages to Ardupilot Unlike the use of ROS 2 `.msg` files, since Ardupilot supports native DDS, the message files follow [OMG IDL DDS v4.2](https://www.omg.org/spec/IDL/4.2/PDF). diff --git a/libraries/AP_DDS/dds_xrce_profile.xml b/libraries/AP_DDS/dds_xrce_profile.xml index a8089b76af9df..aeed08bc8332c 100644 --- a/libraries/AP_DDS/dds_xrce_profile.xml +++ b/libraries/AP_DDS/dds_xrce_profile.xml @@ -58,4 +58,32 @@ + + rt/tf + tf2_msgs::msg::dds_::TFMessage_ + + KEEP_LAST + 1 + + + + PREALLOCATED_WITH_REALLOC + + + RELIABLE + + + TRANSIENT_LOCAL + + + + NO_KEY + rt/tf + tf2_msgs::msg::dds_::TFMessage_ + + KEEP_LAST + 1 + + + diff --git a/libraries/AP_DDS/gen_config_h.py b/libraries/AP_DDS/gen_config_h.py index 7a828f8e1d244..f35af8f7b89d1 100755 --- a/libraries/AP_DDS/gen_config_h.py +++ b/libraries/AP_DDS/gen_config_h.py @@ -22,7 +22,7 @@ 'UCLIENT_MIN_HEARTBEAT_TIME_INTERVAL': 200, 'UCLIENT_UDP_TRANSPORT_MTU': 300, 'UCLIENT_TCP_TRANSPORT_MTU': 350, - 'UCLIENT_SERIAL_TRANSPORT_MTU': 250, + 'UCLIENT_SERIAL_TRANSPORT_MTU': 1024, 'UCLIENT_CUSTOM_TRANSPORT_MTU': 400, 'CONFIG_MACHINE_ENDIANNESS': 1, # little endian 'UCLIENT_SHARED_MEMORY_MAX_ENTITIES': 0, diff --git a/libraries/AP_DDS/wscript b/libraries/AP_DDS/wscript index 0efd551a62e03..a8e1aa4d0c6d5 100644 --- a/libraries/AP_DDS/wscript +++ b/libraries/AP_DDS/wscript @@ -83,6 +83,9 @@ def build(bld): 'PoseStamped.idl', 'Quaternion.idl', 'Time.idl', + 'Transform.idl', + 'TransformStamped.idl', + "TFMessage.idl", 'Twist.idl', 'TwistStamped.idl', 'Vector3.idl', From 54aca7dc0dde04695c9b62656636d00460117857 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 12 Apr 2023 10:54:24 +1000 Subject: [PATCH 17/24] autotest: exempt sailboats from initial location check we have a sanity check that when you reboot a vehicle that soon afterwards the vehicle is in the place we expect it to start. That's fine for most vehicles, which stay put. But we start the ship simulation with wind, so it can actually drift before we get around to checking its location. Simply skip this check for now (we could, perhaps, allow for a very large radius instead?) --- Tools/autotest/common.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 0b75a17b37eca..6ce9a81cc187e 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -1986,7 +1986,8 @@ def reboot_sitl(self, required_bootcount=None, force=False): self.progress("Rebooting SITL") self.reboot_sitl_mav(required_bootcount=required_bootcount, force=force) self.do_heartbeats(force=True) - self.assert_simstate_location_is_at_startup_location() + if self.frame != 'sailboat': # sailboats drift with wind! + self.assert_simstate_location_is_at_startup_location() def reboot_sitl_mavproxy(self, required_bootcount=None): """Reboot SITL instance using MAVProxy and wait for it to reconnect.""" @@ -7879,7 +7880,6 @@ def run_one_test_attempt(self, test, interact=False, attempt=1): passed = False elif not passed: # implicit reboot after a failed test: self.progress("Test failed but ArduPilot process alive; rebooting") - self.progress("Force-rebooting SITL") self.reboot_sitl() # that'll learn it if self._mavproxy is not None: From 3eae62c69a042cc2b8d7e8f9b31841d7bf33ddcf Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 5 Apr 2023 22:59:30 +1000 Subject: [PATCH 18/24] AP_RangeFinder: add and use AP_RangeFinder_config.h --- libraries/AP_RangeFinder/AP_RangeFinder.h | 10 ++-------- libraries/AP_RangeFinder/AP_RangeFinder_config.h | 11 +++++++++++ 2 files changed, 13 insertions(+), 8 deletions(-) create mode 100644 libraries/AP_RangeFinder/AP_RangeFinder_config.h diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h index 3a66a5aa6de50..5e7e04e361061 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder.h @@ -14,6 +14,8 @@ */ #pragma once +#include "AP_RangeFinder_config.h" + #include #include #include @@ -22,14 +24,6 @@ #include #include "AP_RangeFinder_Params.h" -#ifndef AP_RANGEFINDER_ENABLED -#define AP_RANGEFINDER_ENABLED 1 -#endif - -#ifndef AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED -#define AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED AP_RANGEFINDER_ENABLED -#endif - // Maximum number of range finder instances available on this platform #ifndef RANGEFINDER_MAX_INSTANCES #if AP_RANGEFINDER_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_config.h b/libraries/AP_RangeFinder/AP_RangeFinder_config.h new file mode 100644 index 0000000000000..c3fdf94cdef0e --- /dev/null +++ b/libraries/AP_RangeFinder/AP_RangeFinder_config.h @@ -0,0 +1,11 @@ +#pragma once + +#include + +#ifndef AP_RANGEFINDER_ENABLED +#define AP_RANGEFINDER_ENABLED 1 +#endif + +#ifndef AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED +#define AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED AP_RANGEFINDER_ENABLED +#endif From 92dea596202cb16e790ad512624fe0ad2fb736ce Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 27 Mar 2023 09:05:28 +1100 Subject: [PATCH 19/24] hwdef: SkyViper doesn't need most proximity sensors --- libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/hwdef.dat | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/hwdef.dat index ef7814bd6d0fb..1792e036f6a0a 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/skyviper-v2450/hwdef.dat @@ -177,4 +177,8 @@ define AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED 0 define AC_PRECLAND_BACKEND_DEFAULT_ENABLED 0 define AC_PRECLAND_COMPANION_ENABLED 1 +// no proximity sensors are applicable: +define AP_PROXIMITY_BACKEND_DEFAULT_ENABLED 0 +define AP_PROXIMITY_MAV_ENABLED 1 + AUTOBUILD_TARGETS Copter From c110ac489c38a8f55e92903ed8e9c85571365bf0 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 5 Apr 2023 22:25:54 +1000 Subject: [PATCH 20/24] AP_Proximity: add and use defines for all Proximity backends --- libraries/AP_Proximity/AP_Proximity.cpp | 29 +++++++---- libraries/AP_Proximity/AP_Proximity.h | 20 +++++++- .../AP_Proximity/AP_Proximity_AirSimSITL.cpp | 13 ++--- .../AP_Proximity/AP_Proximity_AirSimSITL.h | 11 +++-- .../AP_Proximity/AP_Proximity_Cygbot_D1.cpp | 8 ++-- .../AP_Proximity/AP_Proximity_Cygbot_D1.h | 7 +-- .../AP_Proximity/AP_Proximity_DroneCAN.cpp | 1 - .../AP_Proximity/AP_Proximity_DroneCAN.h | 10 ++-- .../AP_Proximity_LightWareSF40C.cpp | 7 ++- .../AP_Proximity_LightWareSF40C.h | 8 ++-- .../AP_Proximity_LightWareSF45B.cpp | 7 ++- .../AP_Proximity_LightWareSF45B.h | 7 ++- libraries/AP_Proximity/AP_Proximity_MAV.cpp | 8 ++-- libraries/AP_Proximity/AP_Proximity_MAV.h | 8 ++-- .../AP_Proximity/AP_Proximity_RPLidarA2.cpp | 7 ++- .../AP_Proximity/AP_Proximity_RPLidarA2.h | 8 ++-- .../AP_Proximity/AP_Proximity_RangeFinder.cpp | 7 ++- .../AP_Proximity/AP_Proximity_RangeFinder.h | 8 ++-- libraries/AP_Proximity/AP_Proximity_SITL.cpp | 12 ++--- libraries/AP_Proximity/AP_Proximity_SITL.h | 10 ++-- .../AP_Proximity_TeraRangerTower.cpp | 7 ++- .../AP_Proximity_TeraRangerTower.h | 7 ++- .../AP_Proximity_TeraRangerTowerEvo.cpp | 7 ++- .../AP_Proximity_TeraRangerTowerEvo.h | 7 ++- libraries/AP_Proximity/AP_Proximity_config.h | 48 ++++++++++++++++++- 25 files changed, 189 insertions(+), 83 deletions(-) diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index 866e5b3460ae3..3ef0db827e70e 100644 --- a/libraries/AP_Proximity/AP_Proximity.cpp +++ b/libraries/AP_Proximity/AP_Proximity.cpp @@ -121,10 +121,12 @@ void AP_Proximity::init() // instantiate backends uint8_t serial_instance = 0; + (void)serial_instance; // in case no serial backends are compiled in for (uint8_t instance=0; instance. */ -#include "AP_Proximity_AirSimSITL.h" +#include "AP_Proximity_config.h" -#if HAL_PROXIMITY_ENABLED -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL -#include -#include +#if AP_PROXIMITY_AIRSIMSITL_ENABLED -extern const AP_HAL::HAL& hal; +#include "AP_Proximity_AirSimSITL.h" #define PROXIMITY_MAX_RANGE 100.0f #define PROXIMITY_ACCURACY 0.1f // minimum distance (in meters) between objects sent to object database @@ -98,6 +95,4 @@ bool AP_Proximity_AirSimSITL::get_upward_distance(float &distance) const return false; } -#endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL - -#endif // HAL_PROXIMITY_ENABLED +#endif // AP_PROXIMITY_AIRSIMSITL_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_AirSimSITL.h b/libraries/AP_Proximity/AP_Proximity_AirSimSITL.h index cf343734ff5d7..200f534b76a73 100644 --- a/libraries/AP_Proximity/AP_Proximity_AirSimSITL.h +++ b/libraries/AP_Proximity/AP_Proximity_AirSimSITL.h @@ -15,11 +15,12 @@ #pragma once -#include "AP_Proximity_Backend.h" +#include "AP_Proximity_config.h" + +#if AP_PROXIMITY_AIRSIMSITL_ENABLED -#if HAL_PROXIMITY_ENABLED +#include "AP_Proximity_Backend.h" -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #include class AP_Proximity_AirSimSITL : public AP_Proximity_Backend @@ -44,6 +45,6 @@ class AP_Proximity_AirSimSITL : public AP_Proximity_Backend AP_Proximity_Temp_Boundary temp_boundary; }; -#endif // CONFIG_HAL_BOARD -#endif // HAL_PROXIMITY_ENABLED +#endif // AP_PROXIMITY_AIRSIMSITL_ENABLED + diff --git a/libraries/AP_Proximity/AP_Proximity_Cygbot_D1.cpp b/libraries/AP_Proximity/AP_Proximity_Cygbot_D1.cpp index 5a81f3fe4a7d3..8d4e1959b321d 100644 --- a/libraries/AP_Proximity/AP_Proximity_Cygbot_D1.cpp +++ b/libraries/AP_Proximity/AP_Proximity_Cygbot_D1.cpp @@ -1,6 +1,8 @@ -#include "AP_Proximity_Cygbot_D1.h" +#include "AP_Proximity_config.h" + +#if AP_PROXIMITY_CYGBOT_ENABLED -#if HAL_PROXIMITY_ENABLED && AP_PROXIMITY_CYGBOT_ENABLED +#include "AP_Proximity_Cygbot_D1.h" // update the state of the sensor void AP_Proximity_Cygbot_D1::update() @@ -182,4 +184,4 @@ void AP_Proximity_Cygbot_D1::reset() _temp_boundary.reset(); } -#endif // HAL_PROXIMITY_ENABLED +#endif // AP_PROXIMITY_CYGBOT_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_Cygbot_D1.h b/libraries/AP_Proximity/AP_Proximity_Cygbot_D1.h index aaa1de91c8d69..e9e7bed7fabe4 100644 --- a/libraries/AP_Proximity/AP_Proximity_Cygbot_D1.h +++ b/libraries/AP_Proximity/AP_Proximity_Cygbot_D1.h @@ -1,8 +1,9 @@ #pragma once -#include "AP_Proximity.h" +#include "AP_Proximity_config.h" + +#if AP_PROXIMITY_CYGBOT_ENABLED -#if (HAL_PROXIMITY_ENABLED && AP_PROXIMITY_CYGBOT_ENABLED) #include "AP_Proximity_Backend_Serial.h" #define CYGBOT_MAX_MSG_SIZE 350 @@ -80,4 +81,4 @@ class AP_Proximity_Cygbot_D1 : public AP_Proximity_Backend_Serial }; -#endif // HAL_PROXIMITY_ENABLED +#endif // AP_PROXIMITY_CYGBOT_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_DroneCAN.cpp b/libraries/AP_Proximity/AP_Proximity_DroneCAN.cpp index b5b3e8265ab69..09f751491ea18 100644 --- a/libraries/AP_Proximity/AP_Proximity_DroneCAN.cpp +++ b/libraries/AP_Proximity/AP_Proximity_DroneCAN.cpp @@ -191,5 +191,4 @@ void AP_Proximity_DroneCAN::handle_measurement(AP_DroneCAN *ap_dronecan, const C } } - #endif // AP_PROXIMITY_DRONECAN_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_DroneCAN.h b/libraries/AP_Proximity/AP_Proximity_DroneCAN.h index a5c3c731d17de..deee2b77df1ea 100644 --- a/libraries/AP_Proximity/AP_Proximity_DroneCAN.h +++ b/libraries/AP_Proximity/AP_Proximity_DroneCAN.h @@ -1,14 +1,12 @@ #pragma once -#include "AP_Proximity_Backend.h" +#include "AP_Proximity_config.h" -#include +#if AP_PROXIMITY_DRONECAN_ENABLED -#ifndef AP_PROXIMITY_DRONECAN_ENABLED -#define AP_PROXIMITY_DRONECAN_ENABLED (HAL_ENABLE_DRONECAN_DRIVERS && HAL_PROXIMITY_ENABLED) -#endif +#include "AP_Proximity_Backend.h" -#if AP_PROXIMITY_DRONECAN_ENABLED +#include class AP_Proximity_DroneCAN : public AP_Proximity_Backend { diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp index 8204a08001bdf..2addb7cd016e2 100644 --- a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp +++ b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.cpp @@ -13,9 +13,12 @@ along with this program. If not, see . */ +#include "AP_Proximity_config.h" + +#if AP_PROXIMITY_LIGHTWARE_SF40C_ENABLED + #include "AP_Proximity_LightWareSF40C.h" -#if HAL_PROXIMITY_ENABLED #include #include #include @@ -421,4 +424,4 @@ uint16_t AP_Proximity_LightWareSF40C::buff_to_uint16(uint8_t b0, uint8_t b1) con return leval; } -#endif // HAL_PROXIMITY_ENABLED +#endif // AP_PROXIMITY_LIGHTWARE_SF40C_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.h b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.h index 2dc00e4b8880c..87ae4d3f5eabc 100644 --- a/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.h +++ b/libraries/AP_Proximity/AP_Proximity_LightWareSF40C.h @@ -1,8 +1,10 @@ #pragma once -#include "AP_Proximity_Backend_Serial.h" +#include "AP_Proximity_config.h" + +#if AP_PROXIMITY_LIGHTWARE_SF40C_ENABLED -#if HAL_PROXIMITY_ENABLED +#include "AP_Proximity_Backend_Serial.h" #define PROXIMITY_SF40C_TIMEOUT_MS 200 // requests timeout after 0.2 seconds #define PROXIMITY_SF40C_PAYLOAD_LEN_MAX 256 // maximum payload size we can accept (in some configurations sensor may send as large as 1023) @@ -152,4 +154,4 @@ class AP_Proximity_LightWareSF40C : public AP_Proximity_Backend_Serial uint16_t buff_to_uint16(uint8_t b0, uint8_t b1) const; }; -#endif // HAL_PROXIMITY_ENABLED +#endif // AP_PROXIMITY_LIGHTWARE_SF40C_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.cpp b/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.cpp index 79db05c6aed88..47e0e28e8e1d4 100644 --- a/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.cpp +++ b/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.cpp @@ -16,8 +16,11 @@ http://support.lightware.co.za/sf45/#/commands */ +#include "AP_Proximity_config.h" + +#if AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED + #include "AP_Proximity_LightWareSF45B.h" -#if HAL_PROXIMITY_ENABLED #include #include @@ -200,4 +203,4 @@ uint8_t AP_Proximity_LightWareSF45B::convert_angle_to_minisector(float angle_deg return wrap_360(angle_deg + (PROXIMITY_SF45B_COMBINE_READINGS_DEG * 0.5f)) / PROXIMITY_SF45B_COMBINE_READINGS_DEG; } -#endif // HAL_PROXIMITY_ENABLED +#endif // AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.h b/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.h index 2a3e2d2cbd57c..203c59bea6320 100644 --- a/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.h +++ b/libraries/AP_Proximity/AP_Proximity_LightWareSF45B.h @@ -1,8 +1,11 @@ #pragma once +#include "AP_Proximity_config.h" + +#if AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED + #include "AP_Proximity_LightWareSerial.h" -#if HAL_PROXIMITY_ENABLED #include class AP_Proximity_LightWareSF45B : public AP_Proximity_LightWareSerial @@ -104,4 +107,4 @@ class AP_Proximity_LightWareSF45B : public AP_Proximity_LightWareSerial }; -#endif // HAL_PROXIMITY_ENABLED +#endif // AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_MAV.cpp b/libraries/AP_Proximity/AP_Proximity_MAV.cpp index 254f1f36d732c..0a31893e26255 100644 --- a/libraries/AP_Proximity/AP_Proximity_MAV.cpp +++ b/libraries/AP_Proximity/AP_Proximity_MAV.cpp @@ -13,9 +13,11 @@ along with this program. If not, see . */ -#include "AP_Proximity_MAV.h" +#include "AP_Proximity_config.h" + +#if AP_PROXIMITY_MAV_ENABLED -#if HAL_PROXIMITY_ENABLED +#include "AP_Proximity_MAV.h" #include #include #include @@ -272,4 +274,4 @@ void AP_Proximity_MAV::handle_obstacle_distance_3d_msg(const mavlink_message_t & return; } -#endif // HAL_PROXIMITY_ENABLED +#endif // AP_PROXIMITY_MAV_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_MAV.h b/libraries/AP_Proximity/AP_Proximity_MAV.h index 7d594cf8848be..3ef8976fe2ced 100644 --- a/libraries/AP_Proximity/AP_Proximity_MAV.h +++ b/libraries/AP_Proximity/AP_Proximity_MAV.h @@ -1,8 +1,10 @@ #pragma once -#include "AP_Proximity_Backend.h" +#include "AP_Proximity_config.h" + +#if AP_PROXIMITY_MAV_ENABLED -#if HAL_PROXIMITY_ENABLED +#include "AP_Proximity_Backend.h" class AP_Proximity_MAV : public AP_Proximity_Backend { @@ -46,4 +48,4 @@ class AP_Proximity_MAV : public AP_Proximity_Backend float _distance_upward; // upward distance in meters }; -#endif // HAL_PROXIMITY_ENABLED +#endif // AP_PROXIMITY_MAV_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp b/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp index ab3d1ad05a528..26d75185a9cd6 100644 --- a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp +++ b/libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp @@ -26,9 +26,12 @@ * */ +#include "AP_Proximity_config.h" + +#if AP_PROXIMITY_RPLIDARA2_ENABLED + #include "AP_Proximity_RPLidarA2.h" -#if HAL_PROXIMITY_ENABLED #include #include #include @@ -361,4 +364,4 @@ void AP_Proximity_RPLidarA2::parse_response_data() } } -#endif // HAL_PROXIMITY_ENABLED +#endif // AP_PROXIMITY_RPLIDARA2_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.h b/libraries/AP_Proximity/AP_Proximity_RPLidarA2.h index a78d02cee52c8..9fd20108a3c35 100644 --- a/libraries/AP_Proximity/AP_Proximity_RPLidarA2.h +++ b/libraries/AP_Proximity/AP_Proximity_RPLidarA2.h @@ -29,9 +29,11 @@ #pragma once -#include "AP_Proximity_Backend_Serial.h" +#include "AP_Proximity_config.h" + +#if AP_PROXIMITY_RPLIDARA2_ENABLED -#if HAL_PROXIMITY_ENABLED +#include "AP_Proximity_Backend_Serial.h" class AP_Proximity_RPLidarA2 : public AP_Proximity_Backend_Serial { @@ -121,4 +123,4 @@ class AP_Proximity_RPLidarA2 : public AP_Proximity_Backend_Serial } payload; }; -#endif // HAL_PROXIMITY_ENABLED +#endif // AP_PROXIMITY_RPLIDARA2_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp b/libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp index ebcca88be7df1..4ec86b7f6e943 100644 --- a/libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp +++ b/libraries/AP_Proximity/AP_Proximity_RangeFinder.cpp @@ -13,9 +13,12 @@ along with this program. If not, see . */ +#include "AP_Proximity_config.h" + +#if AP_PROXIMITY_RANGEFINDER_ENABLED + #include "AP_Proximity_RangeFinder.h" -#if HAL_PROXIMITY_ENABLED #include #include #include @@ -94,4 +97,4 @@ bool AP_Proximity_RangeFinder::get_upward_distance(float &distance) const return false; } -#endif // HAL_PROXIMITY_ENABLED +#endif // AP_PROXIMITY_RANGEFINDER_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_RangeFinder.h b/libraries/AP_Proximity/AP_Proximity_RangeFinder.h index 635cc1bd29ad9..828b3a2d73e49 100644 --- a/libraries/AP_Proximity/AP_Proximity_RangeFinder.h +++ b/libraries/AP_Proximity/AP_Proximity_RangeFinder.h @@ -1,8 +1,10 @@ #pragma once -#include "AP_Proximity_Backend.h" +#include "AP_Proximity_config.h" + +#if AP_PROXIMITY_RANGEFINDER_ENABLED -#if HAL_PROXIMITY_ENABLED +#include "AP_Proximity_Backend.h" #define PROXIMITY_RANGEFIDER_TIMEOUT_MS 200 // requests timeout after 0.2 seconds @@ -35,4 +37,4 @@ class AP_Proximity_RangeFinder : public AP_Proximity_Backend float _distance_upward = -1; // upward distance in meters, negative if the last reading was out of range }; -#endif // HAL_PROXIMITY_ENABLED +#endif // AP_PROXIMITY_RANGEFINDER_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_SITL.cpp b/libraries/AP_Proximity/AP_Proximity_SITL.cpp index f5a71de2f3626..1d72a8ebed41d 100644 --- a/libraries/AP_Proximity/AP_Proximity_SITL.cpp +++ b/libraries/AP_Proximity/AP_Proximity_SITL.cpp @@ -13,14 +13,14 @@ along with this program. If not, see . */ +#include "AP_Proximity_config.h" + +#if AP_PROXIMITY_SITL_ENABLED + #include "AP_Proximity_SITL.h" -#if HAL_PROXIMITY_ENABLED #include - -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #include - #include #include @@ -135,6 +135,4 @@ bool AP_Proximity_SITL::get_upward_distance(float &distance) const return true; } -#endif // CONFIG_HAL_BOARD - -#endif // HAL_PROXIMITY_ENABLED +#endif // AP_PROXIMITY_SITL_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_SITL.h b/libraries/AP_Proximity/AP_Proximity_SITL.h index ba85dd966d42d..5fbe24f6ec03d 100644 --- a/libraries/AP_Proximity/AP_Proximity_SITL.h +++ b/libraries/AP_Proximity/AP_Proximity_SITL.h @@ -1,12 +1,13 @@ #pragma once +#include "AP_Proximity_config.h" + +#if AP_PROXIMITY_SITL_ENABLED + #include "AP_Proximity.h" -#if HAL_PROXIMITY_ENABLED #include "AP_Proximity_Backend.h" -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #include -#include #include class AP_Proximity_SITL : public AP_Proximity_Backend @@ -35,6 +36,5 @@ class AP_Proximity_SITL : public AP_Proximity_Backend bool get_distance_to_fence(float angle_deg, float &distance) const; }; -#endif // CONFIG_HAL_BOARD -#endif // HAL_PROXIMITY_ENABLED +#endif // AP_PROXIMITY_SITL_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp b/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp index a5c2027c3564e..f467045035b32 100644 --- a/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp +++ b/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.cpp @@ -13,9 +13,12 @@ along with this program. If not, see . */ +#include "AP_Proximity_config.h" + +#if AP_PROXIMITY_TERARANGERTOWER_ENABLED + #include "AP_Proximity_TeraRangerTower.h" -#if HAL_PROXIMITY_ENABLED #include #include #include @@ -109,4 +112,4 @@ void AP_Proximity_TeraRangerTower::update_sector_data(int16_t angle_deg, uint16_ _last_distance_received_ms = AP_HAL::millis(); } -#endif // HAL_PROXIMITY_ENABLED +#endif // AP_PROXIMITY_TERARANGERTOWER_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.h b/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.h index 35006cad84769..c060e022b6533 100644 --- a/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.h +++ b/libraries/AP_Proximity/AP_Proximity_TeraRangerTower.h @@ -1,8 +1,11 @@ #pragma once +#include "AP_Proximity_config.h" + +#if AP_PROXIMITY_TERARANGERTOWER_ENABLED + #include "AP_Proximity_Backend_Serial.h" -#if HAL_PROXIMITY_ENABLED #define PROXIMITY_TRTOWER_TIMEOUT_MS 300 // requests timeout after 0.3 seconds class AP_Proximity_TeraRangerTower : public AP_Proximity_Backend_Serial @@ -33,4 +36,4 @@ class AP_Proximity_TeraRangerTower : public AP_Proximity_Backend_Serial uint32_t _last_distance_received_ms; // system time of last distance measurement received from sensor }; -#endif // HAL_PROXIMITY_ENABLED +#endif // AP_PROXIMITY_TERARANGERTOWER_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp b/libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp index db527417df111..476d47415a13a 100644 --- a/libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp +++ b/libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.cpp @@ -13,10 +13,13 @@ along with this program. If not, see . */ +#include "AP_Proximity_config.h" + +#if AP_PROXIMITY_TERARANGERTOWEREVO_ENABLED + #include #include "AP_Proximity_TeraRangerTowerEvo.h" -#if HAL_PROXIMITY_ENABLED #include #include #include @@ -164,4 +167,4 @@ void AP_Proximity_TeraRangerTowerEvo::update_sector_data(int16_t angle_deg, uint _last_distance_received_ms = AP_HAL::millis(); } -#endif // HAL_PROXIMITY_ENABLED +#endif // AP_PROXIMITY_TERARANGERTOWEREVO_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.h b/libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.h index 4b12d3fab6e38..5804344636aba 100644 --- a/libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.h +++ b/libraries/AP_Proximity/AP_Proximity_TeraRangerTowerEvo.h @@ -1,8 +1,11 @@ #pragma once +#include "AP_Proximity_config.h" + +#if AP_PROXIMITY_TERARANGERTOWEREVO_ENABLED + #include "AP_Proximity.h" -#if HAL_PROXIMITY_ENABLED #include "AP_Proximity_Backend_Serial.h" #define PROXIMITY_TRTOWER_TIMEOUT_MS 300 // requests timeout after 0.3 seconds @@ -59,4 +62,4 @@ class AP_Proximity_TeraRangerTowerEvo : public AP_Proximity_Backend_Serial { // const uint8_t REFRESH_600_HZ[5] = { (uint8_t)0x00, (uint8_t)0x52, (uint8_t)0x03, (uint8_t)0x06, (uint8_t)0xDF}; }; -#endif // HAL_PROXIMITY_ENABLED +#endif // AP_PROXIMITY_TERARANGERTOWEREVO_ENABLED diff --git a/libraries/AP_Proximity/AP_Proximity_config.h b/libraries/AP_Proximity/AP_Proximity_config.h index c6ca9e029132a..e34f3dd3069d4 100644 --- a/libraries/AP_Proximity/AP_Proximity_config.h +++ b/libraries/AP_Proximity/AP_Proximity_config.h @@ -1,11 +1,57 @@ #pragma once #include +#include +#include #ifndef HAL_PROXIMITY_ENABLED #define HAL_PROXIMITY_ENABLED (!HAL_MINIMIZE_FEATURES && BOARD_FLASH_SIZE > 1024) #endif +#ifndef AP_PROXIMITY_BACKEND_DEFAULT_ENABLED +#define AP_PROXIMITY_BACKEND_DEFAULT_ENABLED HAL_PROXIMITY_ENABLED +#endif + +#ifndef AP_PROXIMITY_AIRSIMSITL_ENABLED +#define AP_PROXIMITY_AIRSIMSITL_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED && (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif + #ifndef AP_PROXIMITY_CYGBOT_ENABLED -#define AP_PROXIMITY_CYGBOT_ENABLED HAL_PROXIMITY_ENABLED +#define AP_PROXIMITY_CYGBOT_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_PROXIMITY_DRONECAN_ENABLED +#define AP_PROXIMITY_DRONECAN_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED && HAL_ENABLE_DRONECAN_DRIVERS +#endif + +#ifndef AP_PROXIMITY_LIGHTWARE_SF40C_ENABLED +#define AP_PROXIMITY_LIGHTWARE_SF40C_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED +#define AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_PROXIMITY_MAV_ENABLED +#define AP_PROXIMITY_MAV_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED && HAL_GCS_ENABLED +#endif + +#ifndef AP_PROXIMITY_RANGEFINDER_ENABLED +#define AP_PROXIMITY_RANGEFINDER_ENABLED AP_RANGEFINDER_ENABLED && AP_PROXIMITY_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_PROXIMITY_RPLIDARA2_ENABLED +#define AP_PROXIMITY_RPLIDARA2_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_PROXIMITY_SITL_ENABLED +#define AP_PROXIMITY_SITL_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED && (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif + +#ifndef AP_PROXIMITY_TERARANGERTOWER_ENABLED +#define AP_PROXIMITY_TERARANGERTOWER_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED +#endif + +#ifndef AP_PROXIMITY_TERARANGERTOWEREVO_ENABLED +#define AP_PROXIMITY_TERARANGERTOWEREVO_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED #endif From af5f702c361f53b2837243b3dfde60d4066b2b6a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 5 Apr 2023 22:59:30 +1000 Subject: [PATCH 21/24] GCS_MAVLink: add and use defines for all Proximity backends --- libraries/GCS_MAVLink/GCS_Common.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 94e26f7f8c19e..baee27709ceff 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -433,9 +433,11 @@ void GCS_MAVLINK::send_distance_sensor() AP_Proximity *proximity = AP_Proximity::get_singleton(); if (proximity != nullptr) { for (uint8_t i = 0; i < proximity->num_sensors(); i++) { +#if AP_PROXIMITY_RANGEFINDER_ENABLED if (proximity->get_type(i) == AP_Proximity::Type::RangeFinder) { filter_possible_proximity_sensors = true; } +#endif } } #endif From e5a0d1bf9c072d701537beba7088eb448153ca3a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 6 Apr 2023 23:18:18 +1000 Subject: [PATCH 22/24] Tools: add proximity backends to custom build server --- Tools/scripts/build_options.py | 12 +++++++++++- Tools/scripts/extract_features.py | 6 +++++- 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index b2ed18e10c3d5..4328277088770 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -38,7 +38,6 @@ def __init__(self, Feature('Safety', 'PARACHUTE', 'HAL_PARACHUTE_ENABLED', 'Enable Parachute', 0, None), Feature('Safety', 'FENCE', 'AP_FENCE_ENABLED', 'Enable Geofence', 2, None), - Feature('Safety', 'PROXIMITY', 'HAL_PROXIMITY_ENABLED', 'Enable Proximity', 0, None), Feature('Safety', 'AC_AVOID', 'AC_AVOID_ENABLED', 'Enable Avoidance', 0, 'FENCE'), Feature('Safety', 'AC_OAPATHPLANNER', 'AC_OAPATHPLANNER_ENABLED', 'Enable Object Avoidance Path Planner', 0, 'FENCE'), @@ -218,6 +217,17 @@ def __init__(self, Feature('Sensors', 'OPTICALFLOW_PIXART', 'AP_OPTICALFLOW_PIXART_ENABLED', 'Enable Optical flow PIXART Sensor', 0, "OPTICALFLOW"), # NOQA: E501 Feature('Sensors', 'OPTICALFLOW_UPFLOW', 'AP_OPTICALFLOW_UPFLOW_ENABLED', 'Enable Optical flow UPFLOW Sensor', 0, "OPTICALFLOW"), # NOQA: E501 + Feature('Proximity', 'PROXIMITY', 'HAL_PROXIMITY_ENABLED', 'Enable Proximity', 0, None), + Feature('Proximity', 'PROXIMITY_CYGBOT', 'AP_PROXIMITY_CYGBOT_ENABLED', 'Enable Cygbot D1 Proximity Sensors', 0, "PROXIMITY"), # noqa + Feature('Proximity', 'PROXIMITY_DRONECAN', 'AP_PROXIMITY_DRONECAN_ENABLED', 'Enable DroneCAN Proximity Sensors', 0, "PROXIMITY"), # noqa + Feature('Proximity', 'PROXIMITY_LIGHTWARE_SF40C', 'AP_PROXIMITY_LIGHTWARE_SF40C_ENABLED', 'Enable LightWare SF40C Proximity Sensors', 0, "PROXIMITY"), # noqa + Feature('Proximity', 'PROXIMITY_LIGHTWARE_SF45B', 'AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED', 'Enable LightWare SF45B Proximity Sensors', 0, "PROXIMITY"), # noqa + Feature('Proximity', 'PROXIMITY_MAV', 'AP_PROXIMITY_MAV_ENABLED', 'Enable MAVLink Proximity Sensors', 0, "PROXIMITY"), # noqa + Feature('Proximity', 'PROXIMITY_RANGEFINDER', 'AP_PROXIMITY_RANGEFINDER_ENABLED', 'Use RangeFinders as proximity sensors', 0, "PROXIMITY,RANGEFINDER"), # noqa + Feature('Proximity', 'PROXIMITY_RPLIDARA2', 'AP_PROXIMITY_RPLIDARA2_ENABLED', 'Enable RPLidarA2 Proximity Sensors', 0, "PROXIMITY"), # noqa + Feature('Proximity', 'PROXIMITY_TERRARANGERTOWER', 'AP_PROXIMITY_TERARANGERTOWER_ENABLED', 'Enable TerraRangerTower Proximity Sensors', 0, "PROXIMITY"), # noqa + Feature('Proximity', 'PROXIMITY_TERRARANGERTOWEREVO', 'AP_PROXIMITY_TERARANGERTOWEREVO_ENABLED', 'Enable TerraRangerTower Evo Proximity Sensors', 0, "PROXIMITY"), # noqa + Feature('Sensors', 'BMP085', 'AP_BARO_BMP085_ENABLED', 'Enable BMP085 Barometric Sensor', 1, None), Feature('Sensors', 'BMP280', 'AP_BARO_BMP280_ENABLED', 'Enable BMP280 Barometric Sensor', 1, None), Feature('Sensors', 'BMP388', 'AP_BARO_BMP388_ENABLED', 'Enable BMP388 Barometric Sensor', 1, None), diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py index cf6e64cc764a7..2f2a907ac3f5c 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -116,9 +116,13 @@ def __init__(self, filename, nm="arm-none-eabi-nm"): ('AP_CAMERA_{type}_ENABLED', 'AP_Camera_(?P.*)::trigger_pic',), ('HAL_RUNCAM_ENABLED', 'AP_RunCam::AP_RunCam',), + ('HAL_PROXIMITY_ENABLED', 'AP_Proximity::AP_Proximity',), + ('AP_PROXIMITY_{type}_ENABLED', 'AP_Proximity_(?P.*)::update',), + ('AP_PROXIMITY_CYGBOT_ENABLED', 'AP_Proximity_Cygbot_D1::update',), + ('AP_PROXIMITY_LIGHTWARE_{type}_ENABLED', 'AP_Proximity_LightWare(?P.*)::update',), + ('HAL_PARACHUTE_ENABLED', 'AP_Parachute::update',), ('AP_FENCE_ENABLED', r'AC_Fence::check\b',), - ('HAL_PROXIMITY_ENABLED', 'AP_Proximity::AP_Proximity',), ('AC_AVOID_ENABLED', 'AC_Avoid::AC_Avoid',), ('AC_OAPATHPLANNER_ENABLED', 'AP_OAPathPlanner::AP_OAPathPlanner',), From 8c326bc6fb5224df284acd1b2793d316836abc2f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 12 Apr 2023 17:41:20 +1000 Subject: [PATCH 23/24] Tools: correct extraction of handle_play_tune feature --- Tools/scripts/extract_features.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py index 2f2a907ac3f5c..3d39955e334f1 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -178,7 +178,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm"): ('AP_RC_CHANNEL_AUX_FUNCTION_STRINGS_ENABLED', r'RC_Channel::lookuptable',), - ('AP_NOTIFY_MAVLINK_PLAY_TUNE_SUPPORT_ENABLED', r'AP_Notify::play_tune'), + ('AP_NOTIFY_MAVLINK_PLAY_TUNE_SUPPORT_ENABLED', r'AP_Notify::handle_play_tune'), ('AP_NOTIFY_MAVLINK_LED_CONTROL_SUPPORT_ENABLED', r'AP_Notify::handle_led_control'), ('AP_NOTIFY_NCP5623_ENABLED', r'NCP5623::write'), ('AP_NOTIFY_PROFILED_ENABLED', r'ProfiLED::init_ports'), From 795bb19cb99b32973584cdd354a514b3c39847c2 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 14 Mar 2023 10:30:23 +1100 Subject: [PATCH 24/24] AP_BattMonitor: move defines around battery types for consistency --- libraries/AP_BattMonitor/AP_BattMonitor.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index c2880d24e6eb7..6e3dc4fa8b1ed 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -305,21 +305,21 @@ AP_BattMonitor::init() drivers[instance] = new AP_BattMonitor_SMBus_NeoDesign(*this, state[instance], _params[instance]); break; #endif - case Type::BEBOP: #if AP_BATTERY_BEBOP_ENABLED + case Type::BEBOP: drivers[instance] = new AP_BattMonitor_Bebop(*this, state[instance], _params[instance]); -#endif break; - case Type::UAVCAN_BatteryInfo: +#endif #if AP_BATTERY_UAVCAN_BATTERYINFO_ENABLED + case Type::UAVCAN_BatteryInfo: drivers[instance] = new AP_BattMonitor_DroneCAN(*this, state[instance], AP_BattMonitor_DroneCAN::UAVCAN_BATTERY_INFO, _params[instance]); -#endif break; - case Type::BLHeliESC: +#endif #if AP_BATTERY_ESC_ENABLED + case Type::BLHeliESC: drivers[instance] = new AP_BattMonitor_ESC(*this, state[instance], _params[instance]); -#endif break; +#endif #if AP_BATTERY_SUM_ENABLED case Type::Sum: drivers[instance] = new AP_BattMonitor_Sum(*this, state[instance], _params[instance], instance);