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') 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: 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..3d39955e334f1 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',), @@ -174,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'), 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); diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index d5db4eda2132e..0dfbb10f08f8a 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[] = { @@ -60,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; } @@ -69,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; } @@ -78,6 +83,8 @@ void AP_Camera::cam_mode_toggle() // take a picture void AP_Camera::take_picture() { + WITH_SEMAPHORE(_rsem); + if (primary == nullptr) { return; } @@ -88,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; } @@ -98,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; } @@ -108,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; } @@ -117,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; } @@ -170,6 +185,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; @@ -192,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; @@ -280,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; @@ -292,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; @@ -304,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; } @@ -312,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; @@ -324,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; } @@ -332,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; @@ -344,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) { @@ -359,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) { @@ -370,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; @@ -383,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; @@ -396,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; @@ -409,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; @@ -421,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; @@ -430,8 +479,23 @@ 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) +{ + WITH_SEMAPHORE(_rsem); + + 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) +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..946c82824d449 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 }; @@ -80,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); @@ -124,6 +140,15 @@ 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); +#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[]; @@ -150,11 +175,12 @@ 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(); + 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 bfd3006c1729f..4db6dbebfb074 100644 --- a/libraries/AP_Camera/AP_Camera_Backend.h +++ b/libraries/AP_Camera/AP_Camera_Backend.h @@ -80,7 +80,13 @@ 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) { return false; } +#endif protected: 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..5da821735436a --- /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) +{ + 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..3a6f77de6dd82 --- /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) 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 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', 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 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 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 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 diff --git a/libraries/AP_Scripting/examples/mount-poi.lua b/libraries/AP_Scripting/applets/mount-poi.lua similarity index 82% rename from libraries/AP_Scripting/examples/mount-poi.lua rename to libraries/AP_Scripting/applets/mount-poi.lua index 3c574ef96d5a0..a0eb1e16d166c 100644 --- a/libraries/AP_Scripting/examples/mount-poi.lua +++ b/libraries/AP_Scripting/applets/mount-poi.lua @@ -18,9 +18,9 @@ -- 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} local ALT_FRAME_ABSOLUTE = 0 local UPDATE_INTERVAL_MS = 100 @@ -29,15 +29,19 @@ 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") -- 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 @@ -81,13 +85,10 @@ 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 - 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 +109,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 @@ -116,9 +117,9 @@ 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(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 +134,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 +145,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,24 +163,23 @@ 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 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(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 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 diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index f869d375440ee..b3d2b90c0917b 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -976,17 +976,59 @@ function winch:relax() end function winch:healthy() end -- desc ----@class mount -mount = {} +---@class camera +camera = {} -- 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 +---@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 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 = {} -- desc ---@param instance integer 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 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() diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 254a5417e7062..d26217b2fa319 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -602,7 +602,21 @@ 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 +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 +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 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