From 8acac41163596f95b1efa6d369b2fd75f0d1f639 Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Wed, 12 Feb 2025 07:48:04 -0900 Subject: [PATCH 01/91] ark: pi6x: fix CDCACM_PRODUCTID (#24320) --- boards/ark/pi6x/nuttx-config/bootloader/defconfig | 2 +- boards/ark/pi6x/nuttx-config/nsh/defconfig | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/boards/ark/pi6x/nuttx-config/bootloader/defconfig b/boards/ark/pi6x/nuttx-config/bootloader/defconfig index 745251793b..bba1ad3f39 100644 --- a/boards/ark/pi6x/nuttx-config/bootloader/defconfig +++ b/boards/ark/pi6x/nuttx-config/bootloader/defconfig @@ -32,7 +32,7 @@ CONFIG_BOARD_LOOPSPERMSEC=95150 CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_CDCACM=y CONFIG_CDCACM_IFLOWCONTROL=y -CONFIG_CDCACM_PRODUCTID=0x0039 +CONFIG_CDCACM_PRODUCTID=0x003A CONFIG_CDCACM_PRODUCTSTR="ARK BL Pi6X.x" CONFIG_CDCACM_RXBUFSIZE=600 CONFIG_CDCACM_TXBUFSIZE=12000 diff --git a/boards/ark/pi6x/nuttx-config/nsh/defconfig b/boards/ark/pi6x/nuttx-config/nsh/defconfig index 0e944e53c9..57ff449302 100644 --- a/boards/ark/pi6x/nuttx-config/nsh/defconfig +++ b/boards/ark/pi6x/nuttx-config/nsh/defconfig @@ -76,7 +76,7 @@ CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_CDCACM=y CONFIG_CDCACM_IFLOWCONTROL=y -CONFIG_CDCACM_PRODUCTID=0x0039 +CONFIG_CDCACM_PRODUCTID=0x003A CONFIG_CDCACM_PRODUCTSTR="ARK Pi6X.x" CONFIG_CDCACM_RXBUFSIZE=600 CONFIG_CDCACM_TXBUFSIZE=12000 From 195961ae83190083526614f0fb923584f42fff04 Mon Sep 17 00:00:00 2001 From: Ramon Roche Date: Tue, 11 Feb 2025 19:47:29 -0800 Subject: [PATCH 02/91] ci: only push dev container on commit to main --- .github/workflows/dev_container.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/dev_container.yml b/.github/workflows/dev_container.yml index f64d29b684..b3f9542ead 100644 --- a/.github/workflows/dev_container.yml +++ b/.github/workflows/dev_container.yml @@ -96,6 +96,6 @@ jobs: platforms: | linux/amd64 provenance: mode=max - push: true + push: ${{ github.event_name != 'pull_request' }} cache-from: type=s3,blobs_prefix=cache/${{ github.repository }}/,manifests_prefix=cache/${{ github.repository }}/,region=${{ env.RUNS_ON_AWS_REGION }},bucket=${{ env.RUNS_ON_S3_BUCKET_CACHE }} cache-to: type=s3,blobs_prefix=cache/${{ github.repository }}/,manifests_prefix=cache/${{ github.repository }}/,region=${{ env.RUNS_ON_AWS_REGION }},bucket=${{ env.RUNS_ON_S3_BUCKET_CACHE }},mode=max From b77797b4903e2cb5ba460834f835c94ee76e32ae Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Wed, 12 Feb 2025 15:28:40 -0900 Subject: [PATCH 03/91] tools: px_uploader.py: change RuntimeErorr to printf warning for fw_maxsize greater than fw_maxsize (#24321) --- Tools/px_uploader.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 34f3be05ca..6d8c7c69dc 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -708,7 +708,7 @@ class uploader: # https://github.com/PX4/Firmware/blob/master/src/drivers/boards/common/stm32/board_mcu_version.c#L125-L144 if self.fw_maxsize > fw.property('image_maxsize') and not force: - raise RuntimeError(f"Board can accept larger flash images ({self.fw_maxsize} bytes) than board config ({fw.property('image_maxsize')} bytes). Please use the correct board configuration to avoid lacking critical functionality.") + print(f"WARNING: Board can accept larger flash images ({self.fw_maxsize} bytes) than board config ({fw.property('image_maxsize')} bytes)") else: # If we're still on bootloader v4 on a Pixhawk, we don't know if we # have the silicon errata and therefore need to flash px4_fmu-v2 From 96461cdd7d9f576e17f61f093dd110ec17a23610 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 13 Feb 2025 11:49:08 +0100 Subject: [PATCH 04/91] ci: ignore changes to comments or constants in versioned message check These can be changed without version increment --- Tools/ci/check_msg_versioning.sh | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/Tools/ci/check_msg_versioning.sh b/Tools/ci/check_msg_versioning.sh index c10d4819c2..37c593d277 100755 --- a/Tools/ci/check_msg_versioning.sh +++ b/Tools/ci/check_msg_versioning.sh @@ -26,6 +26,15 @@ do # - Incrementing the version # - An old .msg version exists # - A translation header exists and is included + + # Ignore changes to comments or constants + content_a=$(git show "${BASE_COMMIT}:${file}" | grep -o '^[^#]*' | grep -v =) + content_b=$(git show "${HEAD_COMMIT}:${file}" | grep -o '^[^#]*' | grep -v =) + if [ "${content_a}" == "${content_b}" ]; then + echo "No version update required for ${file}" + continue + fi + diff=$(git --no-pager diff --no-color --diff-filter=M "${BASE_COMMIT}"..."${HEAD_COMMIT}" -- "${file}") old_version=$(echo "${diff}" | sed -n 's/^-uint32 MESSAGE_VERSION = \([0-9]*\).*/\1/p') new_version=$(echo "${diff}" | sed -n 's/^+uint32 MESSAGE_VERSION = \([0-9]*\).*/\1/p') From a151d85a1ca2ed85459a4572dafee46c84dbe377 Mon Sep 17 00:00:00 2001 From: Alex Klimaj Date: Thu, 13 Feb 2025 16:29:21 -0700 Subject: [PATCH 05/91] ARK Cannodes disable mag bias estimator by default (#24327) --- boards/ark/can-gps/init/rc.board_defaults | 2 +- boards/ark/can-rtk-gps/init/rc.board_defaults | 2 +- boards/ark/cannode/init/rc.board_defaults | 1 + boards/ark/septentrio-gps/init/rc.board_defaults | 2 +- boards/ark/teseo-gps/init/rc.board_defaults | 2 +- 5 files changed, 5 insertions(+), 4 deletions(-) diff --git a/boards/ark/can-gps/init/rc.board_defaults b/boards/ark/can-gps/init/rc.board_defaults index 13e16a24f1..3d085fe527 100644 --- a/boards/ark/can-gps/init/rc.board_defaults +++ b/boards/ark/can-gps/init/rc.board_defaults @@ -4,7 +4,7 @@ #------------------------------------------------------------------------------ param set-default CBRK_IO_SAFETY 0 -param set-default MBE_ENABLE 1 +param set-default MBE_ENABLE 0 param set-default SENS_IMU_CLPNOTI 0 safety_button start diff --git a/boards/ark/can-rtk-gps/init/rc.board_defaults b/boards/ark/can-rtk-gps/init/rc.board_defaults index 99b30c747f..85d6a92063 100644 --- a/boards/ark/can-rtk-gps/init/rc.board_defaults +++ b/boards/ark/can-rtk-gps/init/rc.board_defaults @@ -7,7 +7,7 @@ param set-default CBRK_IO_SAFETY 0 param set-default CANNODE_SUB_MBD 1 param set-default CANNODE_SUB_RTCM 1 param set-default GPS_1_GNSS 63 -param set-default MBE_ENABLE 1 +param set-default MBE_ENABLE 0 param set-default SENS_IMU_CLPNOTI 0 safety_button start diff --git a/boards/ark/cannode/init/rc.board_defaults b/boards/ark/cannode/init/rc.board_defaults index 7d149ce5ad..9fe7db23e0 100644 --- a/boards/ark/cannode/init/rc.board_defaults +++ b/boards/ark/cannode/init/rc.board_defaults @@ -3,6 +3,7 @@ # board specific defaults #------------------------------------------------------------------------------ +param set-default MBE_ENABLE 0 param set-default SENS_IMU_CLPNOTI 0 pwm_out start diff --git a/boards/ark/septentrio-gps/init/rc.board_defaults b/boards/ark/septentrio-gps/init/rc.board_defaults index 0db1234ce9..6ae5883882 100644 --- a/boards/ark/septentrio-gps/init/rc.board_defaults +++ b/boards/ark/septentrio-gps/init/rc.board_defaults @@ -6,7 +6,7 @@ param set-default CBRK_IO_SAFETY 0 param set-default CANNODE_SUB_MBD 1 param set-default CANNODE_SUB_RTCM 1 -param set-default MBE_ENABLE 1 +param set-default MBE_ENABLE 0 param set-default SENS_IMU_CLPNOTI 0 safety_button start diff --git a/boards/ark/teseo-gps/init/rc.board_defaults b/boards/ark/teseo-gps/init/rc.board_defaults index 610b5b418f..c765117f40 100644 --- a/boards/ark/teseo-gps/init/rc.board_defaults +++ b/boards/ark/teseo-gps/init/rc.board_defaults @@ -6,7 +6,7 @@ param set-default CBRK_IO_SAFETY 0 param set-default CANNODE_SUB_MBD 1 param set-default CANNODE_SUB_RTCM 1 -param set-default MBE_ENABLE 1 +param set-default MBE_ENABLE 0 param set-default SENS_IMU_CLPNOTI 0 tone_alarm start From eb18edf5eb3c47d736a565d552b438bf3b715825 Mon Sep 17 00:00:00 2001 From: Eric Katzfey <53063038+katzfey@users.noreply.github.com> Date: Thu, 13 Feb 2025 21:49:09 -0800 Subject: [PATCH 06/91] Don't allow spacecraft module parameters for VOXL 2 builds (#24336) - VOXL 2 builds use DISABLE_PARAMS_MODULE_SCOPING for parameters. The new spacecraft module has duplicate symbols with the control_allocator module and so this kills the VOXL 2 build --- src/lib/parameters/CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/lib/parameters/CMakeLists.txt b/src/lib/parameters/CMakeLists.txt index b87e652cad..81b609080a 100644 --- a/src/lib/parameters/CMakeLists.txt +++ b/src/lib/parameters/CMakeLists.txt @@ -62,6 +62,10 @@ if(DISABLE_PARAMS_MODULE_SCOPING) # allow those timer configurations to be skipped. if ((${PX4_BOARD_NAME} MATCHES "MODALAI_VOXL2") AND (file_path MATCHES pwm_out)) message(STATUS "Skipping pwm file path ${file_path} for VOXL2") + # Spacecraft has duplicate parameter names which kills the VOXL 2 build. VOXL 2 does not + # support the spacecraft module so we skip adding the parameters. + elseif ((${PX4_BOARD_NAME} MATCHES "MODALAI_VOXL2") AND (file_path MATCHES spacecraft)) + message(STATUS "Skipping spacecraft file path ${file_path} for VOXL2") else() list(APPEND module_config_files "${file_path}") endif() From f2471861a38cc43bdd23e9642fffb86fddcf4c87 Mon Sep 17 00:00:00 2001 From: Alexander Lerach Date: Thu, 13 Feb 2025 10:13:26 +0100 Subject: [PATCH 07/91] Fix GPS RTCM instance selection --- src/drivers/gps/gps.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index aeeba755e3..ba6f24070f 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -547,8 +547,13 @@ void GPS::handleInjectDataTopic() for (int instance = 0; instance < _orb_inject_data_sub.size(); instance++) { const bool exists = _orb_inject_data_sub[instance].advertised(); - if (exists) { - if (_orb_inject_data_sub[instance].copy(&msg)) { + if (exists && _orb_inject_data_sub[instance].copy(&msg)) { + /* Don't select the own RTCM instance. In case it has a lower + * instance number, it will be selected and will be rejected + * later in the code, resulting in no RTCM injection at all. + */ + if (msg.device_id != get_device_id()) { + // Only use the message if it is up to date if ((hrt_absolute_time() - msg.timestamp) < 5_s) { // Remember that we already did a copy on this instance. already_copied = true; From 430be081315a905d10acccfb5953429c571a9bd3 Mon Sep 17 00:00:00 2001 From: Alexander Lerach Date: Fri, 14 Feb 2025 14:54:42 +0100 Subject: [PATCH 08/91] Add payload tx/rx timeouts to DDS * Add tx/rx timeouts * Code style & tx default timeout * Clarify TX/RX disable value --- src/modules/uxrce_dds_client/module.yaml | 24 +++ .../uxrce_dds_client/uxrce_dds_client.cpp | 158 ++++++++++++------ .../uxrce_dds_client/uxrce_dds_client.h | 20 ++- 3 files changed, 147 insertions(+), 55 deletions(-) diff --git a/src/modules/uxrce_dds_client/module.yaml b/src/modules/uxrce_dds_client/module.yaml index 4e5db21160..773475f1f5 100644 --- a/src/modules/uxrce_dds_client/module.yaml +++ b/src/modules/uxrce_dds_client/module.yaml @@ -105,3 +105,27 @@ parameters: category: System reboot_required: true default: 0 + + UXRCE_DDS_TX_TO: + description: + short: TX rate timeout configuration + long: | + Specifies after how many seconds without sending data the DDS connection is reestablished. + A value less than one disables the TX rate timeout. + type: int32 + category: System + reboot_required: true + default: 3 + unit: s + + UXRCE_DDS_RX_TO: + description: + short: RX rate timeout configuration + long: | + Specifies after how many seconds without receiving data the DDS connection is reestablished. + A value less than one disables the RX rate timeout. + type: int32 + category: System + reboot_required: true + default: -1 + unit: s diff --git a/src/modules/uxrce_dds_client/uxrce_dds_client.cpp b/src/modules/uxrce_dds_client/uxrce_dds_client.cpp index 339bedd7a9..b550912791 100644 --- a/src/modules/uxrce_dds_client/uxrce_dds_client.cpp +++ b/src/modules/uxrce_dds_client/uxrce_dds_client.cpp @@ -209,7 +209,7 @@ void UxrceddsClient::deinit() _comm = nullptr; } -bool UxrceddsClient::setup_session(uxrSession *session) +bool UxrceddsClient::setupSession(uxrSession *session) { _participant_config = static_cast(_param_uxrce_dds_ptcfg.get()); _synchronize_timestamps = (_param_uxrce_dds_synct.get() > 0); @@ -379,7 +379,7 @@ bool UxrceddsClient::setup_session(uxrSession *session) return true; } -void UxrceddsClient::delete_session(uxrSession *session) +void UxrceddsClient::deleteSession(uxrSession *session) { delete_repliers(); @@ -472,6 +472,20 @@ static void fillMessageFormatResponse(const message_format_request_s &message_fo message_format_response.timestamp = hrt_absolute_time(); } +void UxrceddsClient::calculateTxRxRate() +{ + const hrt_abstime now = hrt_absolute_time(); + + if (now - _last_status_update > 1_s) { + float dt = (now - _last_status_update) / 1e6f; + _last_payload_tx_rate = (_subs->num_payload_sent - _last_num_payload_sent) / dt; + _last_payload_rx_rate = (_pubs->num_payload_received - _last_num_payload_received) / dt; + _last_num_payload_sent = _subs->num_payload_sent; + _last_num_payload_received = _pubs->num_payload_received; + _last_status_update = now; + } +} + void UxrceddsClient::handleMessageFormatRequest() { message_format_request_s message_format_request; @@ -483,6 +497,87 @@ void UxrceddsClient::handleMessageFormatRequest() } } +void UxrceddsClient::checkConnectivity(uxrSession *session) +{ + // Reset TX zero counter, when data is sent + if (_last_payload_tx_rate > 0) { + _num_tx_rate_zero = 0; + } + + // Reset RX zero counter, when data is received + if (_last_payload_rx_rate > 0) { + _num_rx_rate_zero = 0; + } + + const hrt_abstime now = hrt_absolute_time(); + + // Start ping and tx/rx rate monitoring, unless we're actively sending & receiving payloads successfully + if ((_last_payload_tx_rate > 0) && (_last_payload_rx_rate > 0)) { + _connected = true; + _num_pings_missed = 0; + _last_ping = now; + + } else { + if (hrt_elapsed_time(&_last_ping) > 1_s) { + // Check payload tx rate + if (_last_payload_tx_rate == 0) { + _num_tx_rate_zero++; + } + + // Check payload rx rate + if (_last_payload_rx_rate == 0) { + _num_rx_rate_zero++; + } + + // Check ping + _last_ping = now; + + if (_had_ping_reply) { + _num_pings_missed = 0; + + } else { + ++_num_pings_missed; + } + + int timeout_ms = 1'000; // 1 second + uint8_t attempts = 1; + uxr_ping_agent_session(session, timeout_ms, attempts); + + _had_ping_reply = false; + } + + if (_num_pings_missed >= 3) { + PX4_ERR("No ping response, disconnecting"); + _connected = false; + } + + int32_t tx_timeout = _param_uxrce_dds_tx_to.get(); + int32_t rx_timeout = _param_uxrce_dds_rx_to.get(); + + if (tx_timeout > 0 && _num_tx_rate_zero >= tx_timeout) { + PX4_ERR("Payload TX rate zero for too long, disconnecting"); + _connected = false; + } + + if (rx_timeout > 0 && _num_rx_rate_zero >= rx_timeout) { + PX4_ERR("Payload RX rate zero for too long, disconnecting"); + _connected = false; + } + } +} + +void UxrceddsClient::resetConnectivityCounters() +{ + _last_status_update = hrt_absolute_time(); + _last_ping = hrt_absolute_time(); + _had_ping_reply = false; + _num_pings_missed = 0; + _last_num_payload_sent = 0; + _last_num_payload_received = 0; + _num_tx_rate_zero = 0; + _num_rx_rate_zero = 0; +} + void UxrceddsClient::syncSystemClock(uxrSession *session) { struct timespec ts = {}; @@ -535,8 +630,8 @@ void UxrceddsClient::run() continue; } - if (!setup_session(&session)) { - delete_session(&session); + if (!setupSession(&session)) { + deleteSession(&session); px4_usleep(1'000'000); PX4_ERR("session setup failed, will retry now"); continue; @@ -552,13 +647,8 @@ void UxrceddsClient::run() } hrt_abstime last_sync_session = 0; - hrt_abstime last_status_update = hrt_absolute_time(); - hrt_abstime last_ping = hrt_absolute_time(); - int num_pings_missed = 0; - bool had_ping_reply = false; - uint32_t last_num_payload_sent{}; - uint32_t last_num_payload_received{}; int poll_error_counter = 0; + resetConnectivityCounters(); _subs->init(); _subs_initialized = true; @@ -629,55 +719,19 @@ void UxrceddsClient::run() // Check for a ping response /* PONG_IN_SESSION_STATUS */ if (session.on_pong_flag == 1) { - had_ping_reply = true; + _had_ping_reply = true; } - const hrt_abstime now = hrt_absolute_time(); + // Calculate the payload tx/rx rate for connectivity monitoring + calculateTxRxRate(); - if (now - last_status_update > 1_s) { - float dt = (now - last_status_update) / 1e6f; - _last_payload_tx_rate = (_subs->num_payload_sent - last_num_payload_sent) / dt; - _last_payload_rx_rate = (_pubs->num_payload_received - last_num_payload_received) / dt; - last_num_payload_sent = _subs->num_payload_sent; - last_num_payload_received = _pubs->num_payload_received; - last_status_update = now; - } - - // Handle ping, unless we're actively sending & receiving payloads successfully - if ((_last_payload_tx_rate > 0) && (_last_payload_rx_rate > 0)) { - _connected = true; - num_pings_missed = 0; - last_ping = now; - - } else { - if (hrt_elapsed_time(&last_ping) > 1_s) { - last_ping = now; - - if (had_ping_reply) { - num_pings_missed = 0; - - } else { - ++num_pings_missed; - } - - int timeout_ms = 1'000; // 1 second - uint8_t attempts = 1; - uxr_ping_agent_session(&session, timeout_ms, attempts); - - had_ping_reply = false; - } - - if (num_pings_missed >= 3) { - PX4_INFO("No ping response, disconnecting"); - _connected = false; - } - } + // Check if there is still connectivity with the agent + checkConnectivity(&session); perf_end(_loop_perf); - } - delete_session(&session); + deleteSession(&session); } } diff --git a/src/modules/uxrce_dds_client/uxrce_dds_client.h b/src/modules/uxrce_dds_client/uxrce_dds_client.h index 2a75d73e4a..823e242f9d 100644 --- a/src/modules/uxrce_dds_client/uxrce_dds_client.h +++ b/src/modules/uxrce_dds_client/uxrce_dds_client.h @@ -120,13 +120,17 @@ private: bool init(); void deinit(); - bool setup_session(uxrSession *session); - void delete_session(uxrSession *session); + bool setupSession(uxrSession *session); + void deleteSession(uxrSession *session); bool setBaudrate(int fd, unsigned baud); void handleMessageFormatRequest(); + void calculateTxRxRate(); + void checkConnectivity(uxrSession *session); + void resetConnectivityCounters(); + uORB::Publication _message_format_response_pub{ORB_ID(message_format_response)}; uORB::Subscription _message_format_request_sub{ORB_ID(message_format_request)}; @@ -179,6 +183,14 @@ private: uxrCommunication *_comm{nullptr}; int _fd{-1}; + hrt_abstime _last_status_update; + hrt_abstime _last_ping; + bool _had_ping_reply{false}; + int _num_pings_missed{0}; + int32_t _num_tx_rate_zero{0}; + int32_t _num_rx_rate_zero{0}; + uint32_t _last_num_payload_sent{0}; + uint32_t _last_num_payload_received{0}; int _last_payload_tx_rate{}; ///< in B/s int _last_payload_rx_rate{}; ///< in B/s @@ -197,6 +209,8 @@ private: (ParamInt) _param_uxrce_key, (ParamInt) _param_uxrce_dds_ptcfg, (ParamInt) _param_uxrce_dds_syncc, - (ParamInt) _param_uxrce_dds_synct + (ParamInt) _param_uxrce_dds_synct, + (ParamInt) _param_uxrce_dds_tx_to, + (ParamInt) _param_uxrce_dds_rx_to ) }; From e3fd50667d1b4cd272544258ec9952550f0b2236 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 11 Feb 2025 11:56:20 +0100 Subject: [PATCH 09/91] Mag cal: automatically disable internal mags if external ones are available --- src/lib/sensor_calibration/Magnetometer.hpp | 1 + src/modules/commander/mag_calibration.cpp | 14 ++++++++++++++ .../VehicleMagnetometer.cpp | 18 ++++++++++++++++++ 3 files changed, 33 insertions(+) diff --git a/src/lib/sensor_calibration/Magnetometer.hpp b/src/lib/sensor_calibration/Magnetometer.hpp index 8d328e7a7d..5f9dd72d43 100644 --- a/src/lib/sensor_calibration/Magnetometer.hpp +++ b/src/lib/sensor_calibration/Magnetometer.hpp @@ -85,6 +85,7 @@ public: int8_t calibration_index() const { return _calibration_index; } uint32_t device_id() const { return _device_id; } bool enabled() const { return (_priority > 0); } + void disable() { _priority = 0; } bool external() const { return _external; } const matrix::Vector3f &offset() const { return _offset; } const int32_t &priority() const { return _priority; } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index a72f0fcadb..7aeadeab55 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -903,6 +903,15 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma bool param_save = false; bool failed = true; + bool external_mag_available = false; + + for (unsigned cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { + if (worker_data.calibration[cur_mag].external() && worker_data.calibration[cur_mag].enabled()) { + external_mag_available = true; + break; + } + } + for (unsigned cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { calibration::Magnetometer ¤t_cal = worker_data.calibration[cur_mag]; @@ -921,6 +930,11 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma current_cal.set_offdiagonal(offdiag[cur_mag]); } + if (external_mag_available && !current_cal.external()) { + // automatically disable the internal mags as they should not be used for navigation + current_cal.disable(); + } + current_cal.PrintStatus(); if (current_cal.ParametersSave(cur_mag, true)) { diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index df90ffdf90..a0ba14bcfd 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -209,6 +209,19 @@ void VehicleMagnetometer::UpdateMagBiasEstimate() if (_magnetometer_bias_estimate_sub.copy(&mag_bias_est)) { bool parameters_notify = false; + bool external_mag_available = false; + + for (unsigned mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) { + if (_calibration[mag_index].external() + && _calibration[mag_index].enabled() + && mag_bias_est.valid[mag_index] + && mag_bias_est.stable[mag_index]) { + + external_mag_available = true; + break; + } + } + for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) { if (mag_bias_est.valid[mag_index] && (mag_bias_est.timestamp > _last_calibration_update)) { @@ -228,6 +241,11 @@ void VehicleMagnetometer::UpdateMagBiasEstimate() const Vector3f offset = _calibration[mag_index].BiasCorrectedSensorOffset(_calibration_estimator_bias[mag_index]); if (_calibration[mag_index].set_offset(offset)) { + if (external_mag_available && !_calibration[mag_index].external()) { + // automatically disable the internal mags as they should not be used for navigation + _calibration[mag_index].disable(); + } + // save parameters with preferred calibration slot to current sensor index _calibration[mag_index].ParametersSave(mag_index); From fe5c88789568903b9a7cb175421ae203b8cfc32f Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 14 Feb 2025 15:45:39 +0100 Subject: [PATCH 10/91] mag check: do not require mag 0 sys_has_mag defines the required number of mags; if one mag is needed, it can be any instance, not necessarily mag 0 --- .../HealthAndArmingChecks/checks/magnetometerCheck.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp index 6716a8e06a..ac35a61803 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp @@ -47,9 +47,6 @@ void MagnetometerChecks::checkAndReport(const Context &context, Report &reporter int num_enabled_and_valid_calibration = 0; for (int instance = 0; instance < _sensor_mag_sub.size(); instance++) { - bool is_mag_fault = false; - const bool is_required = instance == 0 || isMagRequired(instance, is_mag_fault); - const bool exists = _sensor_mag_sub[instance].advertised(); bool is_valid = false; bool is_calibration_valid = false; @@ -80,7 +77,9 @@ void MagnetometerChecks::checkAndReport(const Context &context, Report &reporter } // Do not raise errors if a mag is not required - if (!is_required) { + bool is_mag_fault = false; + + if (!isMagRequired(instance, is_mag_fault)) { continue; } From 61d595dc64c32da36d29a2c4409dae2d618f8077 Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Thu, 13 Feb 2025 11:24:29 +0100 Subject: [PATCH 11/91] reset dist_to_ground_lock if dist_to_bottom is not valid. this avoids a reset to a previous dist_to_ground after a switch back to a valid measurement --- .../tasks/ManualAltitude/FlightTaskManualAltitude.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 7ea9289cb6..ed7676296f 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -129,6 +129,7 @@ void FlightTaskManualAltitude::_updateAltitudeLock() } else { _position_setpoint(2) = _position(2); + _dist_to_ground_lock = NAN; } } From fd5bb9e69c0d68fb7d030b52dd5bfc62ec7209a8 Mon Sep 17 00:00:00 2001 From: alexklimaj Date: Fri, 14 Feb 2025 09:49:36 -0700 Subject: [PATCH 12/91] uavcannode rangefinder: only publish reading too far or too close if less than or greater than. Not equal to. --- src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp b/src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp index 129bd32c21..2ad053d9a4 100644 --- a/src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp +++ b/src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp @@ -100,10 +100,10 @@ public: } // reading_type - if (dist.current_distance >= dist.max_distance) { + if (dist.current_distance > dist.max_distance) { range_sensor.reading_type = uavcan::equipment::range_sensor::Measurement::READING_TYPE_TOO_FAR; - } else if (dist.current_distance <= dist.min_distance) { + } else if (dist.current_distance < dist.min_distance) { range_sensor.reading_type = uavcan::equipment::range_sensor::Measurement::READING_TYPE_TOO_CLOSE; } else if (dist.signal_quality != 0) { From b0eb63958793b56e8d244d2095b6f2616b474d7e Mon Sep 17 00:00:00 2001 From: Eric Katzfey <53063038+katzfey@users.noreply.github.com> Date: Fri, 14 Feb 2025 11:33:38 -0800 Subject: [PATCH 13/91] voxl2_io: Updated to latest version from ModalAI fork - Updated to the latest version of the voxl2_io driver from the ModalAI fork. - Moved to the platform independent Serial driver - Added voxl2_io driver to the SLPI DSP build --- boards/modalai/voxl2-slpi/default.px4board | 1 + src/drivers/voxl2_io/CMakeLists.txt | 3 - src/drivers/voxl2_io/Kconfig | 2 +- src/drivers/voxl2_io/module.yaml | 13 +- src/drivers/voxl2_io/voxl2_io.cpp | 1044 +++++++----------- src/drivers/voxl2_io/voxl2_io.hpp | 85 +- src/drivers/voxl2_io/voxl2_io_packet.c | 26 + src/drivers/voxl2_io/voxl2_io_packet.h | 3 + src/drivers/voxl2_io/voxl2_io_packet_types.h | 1 + src/drivers/voxl2_io/voxl2_io_params.c | 53 +- src/drivers/voxl2_io/voxl2_io_serial.cpp | 191 ---- src/drivers/voxl2_io/voxl2_io_serial.hpp | 69 -- 12 files changed, 510 insertions(+), 981 deletions(-) delete mode 100644 src/drivers/voxl2_io/voxl2_io_serial.cpp delete mode 100644 src/drivers/voxl2_io/voxl2_io_serial.hpp diff --git a/boards/modalai/voxl2-slpi/default.px4board b/boards/modalai/voxl2-slpi/default.px4board index db58d4e28a..1aec34124a 100644 --- a/boards/modalai/voxl2-slpi/default.px4board +++ b/boards/modalai/voxl2-slpi/default.px4board @@ -13,6 +13,7 @@ CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y CONFIG_DRIVERS_QSHELL_QURT=y CONFIG_DRIVERS_RC_CRSF_RC=y +CONFIG_DRIVERS_VOXL2_IO=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_EKF2=y diff --git a/src/drivers/voxl2_io/CMakeLists.txt b/src/drivers/voxl2_io/CMakeLists.txt index 57a5f4af21..b6e3d8a3c2 100644 --- a/src/drivers/voxl2_io/CMakeLists.txt +++ b/src/drivers/voxl2_io/CMakeLists.txt @@ -37,15 +37,12 @@ px4_add_module( SRCS voxl2_io_crc16.c voxl2_io_crc16.h - voxl2_io_serial.cpp - voxl2_io_serial.hpp voxl2_io_packet.c voxl2_io_packet.h voxl2_io_packet_types.h voxl2_io.cpp voxl2_io.hpp DEPENDS - rc px4_work_queue mixer_module MODULE_CONFIG diff --git a/src/drivers/voxl2_io/Kconfig b/src/drivers/voxl2_io/Kconfig index 622cad470d..15964deeeb 100644 --- a/src/drivers/voxl2_io/Kconfig +++ b/src/drivers/voxl2_io/Kconfig @@ -2,4 +2,4 @@ menuconfig DRIVERS_VOXL2_IO bool "voxl2_io" default n ---help--- - Enable support for voxl2_io + Enable support for voxl2_io \ No newline at end of file diff --git a/src/drivers/voxl2_io/module.yaml b/src/drivers/voxl2_io/module.yaml index 021305e955..17feaba8c5 100644 --- a/src/drivers/voxl2_io/module.yaml +++ b/src/drivers/voxl2_io/module.yaml @@ -1,12 +1,13 @@ module_name: VOXL2 IO Output actuator_output: - config_parameters: - - param: 'VOXL2_IO_MIN' - label: 'PWM min value' - - param: 'VOXL2_IO_MAX' - label: 'PWM max value' +# config_parameters: +# - param: 'VOXL2_IO_MIN' +# label: 'PWM min value' +# - param: 'VOXL2_IO_MAX' +# label: 'PWM max value' output_groups: - param_prefix: VOXL2_IO group_label: 'PWMs' channel_label: 'PWM Channel' - num_channels: 4 + num_channels: 8 + diff --git a/src/drivers/voxl2_io/voxl2_io.cpp b/src/drivers/voxl2_io/voxl2_io.cpp index 352b474906..90adfb8422 100644 --- a/src/drivers/voxl2_io/voxl2_io.cpp +++ b/src/drivers/voxl2_io/voxl2_io.cpp @@ -33,8 +33,6 @@ #include "voxl2_io.hpp" -#include - Voxl2IO::Voxl2IO() : OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(VOXL2_IO_DEFAULT_PORT)), @@ -43,69 +41,81 @@ Voxl2IO::Voxl2IO() : _output_update_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": output update interval")) { _mixing_output.setMaxNumOutputs(VOXL2_IO_OUTPUT_CHANNELS); - _uart_port = new Voxl2IoSerial(); - voxl2_io_packet_init(&_sbus_packet); + voxl2_io_packet_init(&_voxl2_io_packet); + + //set low rate scheduling interval to 200hz so that RC can be updated even if all actuators are disabled + //otherwise, the default low rate scheduling interval is 300ms and RC packets are delayed and lost + _mixing_output.setLowrateSchedulingInterval(5_ms); } Voxl2IO::~Voxl2IO() { - /* make sure servos are off */ - stop_all_pwms(); - - if (_uart_port) { - _uart_port->uart_close(); - _uart_port = nullptr; - } + _uart_port.close(); perf_free(_cycle_perf); perf_free(_output_update_perf); } - int Voxl2IO::init() { + PX4_INFO("VOXL2_IO: Driver starting"); + int ret = PX4_OK; - /* Open serial port in this thread */ - if (!_uart_port->is_open()) { - if (_uart_port->uart_open((const char *)_device, _parameters.baud_rate) == PX4_OK) { - /* Send PWM config to M0065... pwm_min and pwm_max */ - PX4_INFO("Opened UART connection to M0065 device on port %s", _device); - - } else { - PX4_ERR("Failed opening device"); - return PX4_ERROR; - } - } - - /* Verify connectivity and protocol version number */ - if (get_version_info() < 0) { - PX4_ERR("Failed to detect voxl2_io protocol version."); - return PX4_ERROR; - - } else { - if (_version_info.sw_version == VOXL2_IO_SW_PROTOCOL_VERSION - && _version_info.hw_version == VOXL2_IO_HW_PROTOCOL_VERSION) { - PX4_INFO("Detected M0065 protocol version. SW: %u HW: %u", _version_info.sw_version, _version_info.hw_version); - - } else { - PX4_ERR("Detected incorrect M0065 protocol version. SW: %u HW: %u", _version_info.sw_version, _version_info.hw_version); - return PX4_ERROR; - } - } - - /* Getting initial parameter values */ + // Getting initial parameter values ret = update_params(); if (ret != OK) { + PX4_ERR("VOXL2_IO: Failed to update params during init"); return ret; } - /* Send PWM MIN/MAX to M0065 */ - update_pwm_config(); + // Print initial param values + print_params(); - ScheduleOnInterval(_current_update_interval); - // ScheduleNow(); + PX4_INFO("VOXL2_IO: "); + + // Open serial port + if (!_uart_port.isOpen()) { + PX4_INFO("VOXL2_IO: Opening UART ESC device %s, baud rate %" PRIi32, _device, _parameters.baud_rate); + + // Configure UART port + if (! _uart_port.setPort(_device)) { + PX4_ERR("Error configuring serial device on port %s", _device); + return -1; + } + + if (! _uart_port.setBaudrate(_parameters.baud_rate)) { + PX4_ERR("Error setting baudrate to %d on %s", (int) _parameters.baud_rate, _device); + return -1; + } + + // Open the UART. If this is successful then the UART is ready to use. + if (! _uart_port.open()) { + PX4_ERR("Error opening serial device %s", _device); + return -1; + } + } + + // Detect M0065 board + ret = get_version_info(); + + if (ret != 0) { + PX4_ERR("VOXL2_IO: Could not detect the board"); + PX4_ERR("VOXL2_IO: Driver initialization failed. Exiting"); + + if (_uart_port.open()) { + PX4_INFO("VOXL2_IO: Closing uart port"); + _uart_port.close(); + } + + return -1; + } + + //ScheduleOnInterval(_current_update_interval); + ScheduleNow(); + + PX4_INFO("VOXL2_IO: Driver initialization succeeded"); return ret; } @@ -117,11 +127,10 @@ int Voxl2IO::update_params() ret = load_params(&_parameters); if (ret == PX4_OK) { - _mixing_output.setAllDisarmedValues(VOXL2_IO_MIXER_DISARMED); + _mixing_output.setAllDisarmedValues(_parameters.pwm_dis); _mixing_output.setAllFailsafeValues(VOXL2_IO_MIXER_FAILSAFE); - _mixing_output.setAllMinValues(VOXL2_IO_MIXER_MIN); - _mixing_output.setAllMaxValues(VOXL2_IO_MIXER_MAX); - _pwm_fullscale = _parameters.pwm_max - _parameters.pwm_min; + _mixing_output.setAllMinValues(_parameters.pwm_min); + _mixing_output.setAllMaxValues(_parameters.pwm_max); } return ret; @@ -130,196 +139,159 @@ int Voxl2IO::update_params() int Voxl2IO::load_params(voxl2_io_params_t *params) { int ret = PX4_OK; - int32_t max = params->pwm_max; - int32_t min = params->pwm_min; // initialize out for (int i = 0; i < VOXL2_IO_OUTPUT_CHANNELS; i++) { params->function_map[i] = (int)OutputFunction::Disabled; } - /* UART config, PWM mode, and RC protocol*/ + // UART config, PWM mode, and RC protocol param_get(param_find("VOXL2_IO_BAUD"), ¶ms->baud_rate); - param_get(param_find("RC_INPUT_PROTO"), ¶ms->param_rc_input_proto); + //param_get(param_find("RC_INPUT_PROTO"), ¶ms->param_rc_input_proto); - /* PWM min, max, and failsafe values*/ + // PWM min, max, and failsafe values param_get(param_find("VOXL2_IO_MIN"), ¶ms->pwm_min); param_get(param_find("VOXL2_IO_MAX"), ¶ms->pwm_max); + param_get(param_find("VOXL2_IO_DIS"), ¶ms->pwm_dis); + param_get(param_find("VOXL2_IO_CMIN"), ¶ms->pwm_cal_min); + param_get(param_find("VOXL2_IO_CMAX"), ¶ms->pwm_cal_max); - /* PWM output functions */ + // PWM output functions + //0: disabled, 1: constant min, 2: constant max + //101-112: motors, 201-208: servos, 402: RC Roll, 403: RC Pitch, 404: RC Throttle, + //405: RC Yaw, 406: RC Flaps, 407-412: RC AUX 1-6, 420-422: Gimbal RPY param_get(param_find("VOXL2_IO_FUNC1"), ¶ms->function_map[0]); param_get(param_find("VOXL2_IO_FUNC2"), ¶ms->function_map[1]); param_get(param_find("VOXL2_IO_FUNC3"), ¶ms->function_map[2]); param_get(param_find("VOXL2_IO_FUNC4"), ¶ms->function_map[3]); + param_get(param_find("VOXL2_IO_FUNC5"), ¶ms->function_map[4]); + param_get(param_find("VOXL2_IO_FUNC6"), ¶ms->function_map[5]); + param_get(param_find("VOXL2_IO_FUNC7"), ¶ms->function_map[6]); + param_get(param_find("VOXL2_IO_FUNC8"), ¶ms->function_map[7]); - /* Validate PWM min and max values */ + // Validate PWM min and max values if (params->pwm_min > params->pwm_max) { - PX4_ERR("Invalid parameter VOXL2_IO_MIN. Please verify parameters."); + PX4_ERR("VOXL2_IO: Invalid parameter VOXL2_IO_MIN. Please verify parameters."); params->pwm_min = 0; ret = PX4_ERROR; } - if (ret == PX4_OK && _uart_port->is_open() && (max != params->pwm_max || min != params->pwm_min)) { - PX4_INFO("Updating PWM params load_params"); - update_pwm_config(); - } - return ret; } -void Voxl2IO::update_pwm_config() -{ - Command cmd; - uint8_t data[VOXL2_IO_BOARD_CONFIG_SIZE] = {static_cast((_parameters.pwm_min & 0xFF00) >> 8), static_cast(_parameters.pwm_min & 0xFF), - static_cast((_parameters.pwm_max & 0xFF00) >> 8), static_cast(_parameters.pwm_max & 0xFF) - }; - cmd.len = voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_CONFIG_BOARD_REQUEST, data, VOXL2_IO_BOARD_CONFIG_SIZE, cmd.buf, - sizeof(cmd.buf)); - - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("Failed to send config packet"); - - } else { - _bytes_sent += cmd.len; - _packets_sent++; - } -} - int Voxl2IO::get_version_info() { - int res = 0 ; - int header = -1 ; - int info_packet = -1; - int read_retries = 3; - int read_succeeded = 0; + PX4_INFO("VOXL2_IO: Detecting M0065 board..."); + voxl2_io_packet_init(&_voxl2_io_packet); + Command cmd; + cmd.len = voxl2_io_create_extended_version_request_packet(0, cmd.buf, sizeof(cmd.buf)); - /* Request protocol version info from M0065 */ - cmd.len = voxl2_io_create_version_request_packet(0, cmd.buf, VOXL2_IO_VERSION_INFO_SIZE); + int retries_left = _board_detect_retries; + bool got_response = false; - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("Failed to send version info packet"); + while ((got_response == false) && (retries_left > 0)) { + retries_left--; + + //send the version request command to the board + if (_uart_port.write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("VOXL2_IO: Could not write version request packet to UART port"); + return -1; + } - } else { _bytes_sent += cmd.len; _packets_sent++; - } - /* Read response */ - px4_usleep(10000); - memset(&_read_buf, 0x00, READ_BUF_SIZE); - res = _uart_port->uart_read(_read_buf, READ_BUF_SIZE); + hrt_abstime t_request = hrt_absolute_time(); + hrt_abstime t_timeout = 50000; //50ms timeout for version info response - while (read_retries) { - if (res) { - /* Get index of packer header */ - for (int index = 0; index < READ_BUF_SIZE; ++index) { - if (_read_buf[index] == VOXL2_IO_PACKET_TYPE_VERSION_RESPONSE) { - info_packet = index; - break; - } - if (_read_buf[index] == VOXL2_IO_PACKET_HEADER) { - header = index; + //wait for the response to come back + while ((!got_response) && (hrt_elapsed_time(&t_request) < t_timeout)) { + px4_usleep(500); //sleep a bit while waiting for the board to respond + + int nread = _uart_port.read(_read_buf, sizeof(_read_buf)); + + for (int i = 0; i < nread; i++) { + int16_t parse_ret = voxl2_io_packet_process_char(_read_buf[i], &_voxl2_io_packet); + + if (parse_ret > 0) { + hrt_abstime response_time = hrt_elapsed_time(&t_request); + //PX4_INFO("got packet of length %i",ret); + _packets_received++; + uint8_t packet_type = voxl2_io_packet_get_type(&_voxl2_io_packet); + uint8_t packet_size = voxl2_io_packet_get_size(&_voxl2_io_packet); + + if (packet_type == VOXL2_IO_PACKET_TYPE_VERSION_EXT_RESPONSE && packet_size == sizeof(VOXL2_IO_EXTENDED_VERSION_INFO)) { + VOXL2_IO_EXTENDED_VERSION_INFO ver; + memcpy(&ver, _voxl2_io_packet.buffer, packet_size); + + PX4_INFO("VOXL2_IO: \tVOXL2_IO ID: %i", ver.id); + PX4_INFO("VOXL2_IO: \tBoard Type : %i: %s", ver.hw_version, board_id_to_name(ver.hw_version).c_str()); + + uint8_t *u = &ver.unique_id[0]; + PX4_INFO("VOXL2_IO: \tUnique ID : 0x%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X", + u[11], u[10], u[9], u[8], u[7], u[6], u[5], u[4], u[3], u[2], u[1], u[0]); + + PX4_INFO("VOXL2_IO: \tFirmware : version %4d, hash %.12s", ver.sw_version, ver.firmware_git_version); + PX4_INFO("VOXL2_IO: \tBootloader : version %4d, hash %.12s", ver.bootloader_version, ver.bootloader_git_version); + PX4_INFO("VOXL2_IO: \tReply time : %uus", (uint32_t)response_time); + + //we requested response from ID 0, so it should match + if (ver.id != 0) { + PX4_ERR("VOXL2_IO: Invalid id: %d", ver.id); + } + + //check HW (board version) + else if (ver.hw_version != VOXL2_IO_HW_VERSION) { + PX4_ERR("VOXL2_IO: Invalid HW version : %d (expected %d)", ver.hw_version, VOXL2_IO_HW_VERSION); + return -1; + } + + //check firmware version running on the board + else if (ver.sw_version != VOXL2_IO_SW_VERSION) { + PX4_ERR("VOXL2_IO: Invalid FW version : %d (expected %d)", ver.sw_version, VOXL2_IO_SW_VERSION); + return -1; + + } else { + got_response = true; + memcpy(&_version_info, &ver, sizeof(_version_info)); //store the version info only if it is valid + } + } } } - /* Try again in a bit if packet header not present yet... */ - if (header == -1 || info_packet == -1) { - if (_debug && header == -1) { PX4_ERR("Failed to find voxl2_io packet header, trying again... retries left: %i", read_retries); } - - if (_debug && info_packet == -1) { PX4_ERR("Failed to find version info packet header, trying again... retries left: %i", read_retries); } - - read_retries--; - flush_uart_rx(); - - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("Failed to send version info packet"); - - } else { - _bytes_sent += cmd.len; - _packets_sent++; - px4_usleep(2000); - } - - continue; - } - - /* Check if we got a valid packet...*/ - if (parse_response(&_read_buf[header], (uint8_t)VOXL2_IO_VERSION_INFO_SIZE)) { - if (_debug) { - PX4_ERR("Error parsing version info packet"); - PX4_INFO_RAW("[%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x]\n", - _read_buf[header + 0], _read_buf[header + 1], _read_buf[header + 2], _read_buf[header + 3], _read_buf[header + 4], - _read_buf[header + 5], - _read_buf[header + 6], _read_buf[header + 7], _read_buf[header + 8], _read_buf[header + 9], _read_buf[header + 10], - _read_buf[header + 11], - _read_buf[header + 12], _read_buf[header + 13], _read_buf[header + 14], _read_buf[header + 15], _read_buf[header + 16], - _read_buf[header + 17], - _read_buf[header + 18], _read_buf[header + 19], _read_buf[header + 20], _read_buf[header + 21], _read_buf[header + 22], - _read_buf[header + 23], - _read_buf[header + 24], _read_buf[header + 25], _read_buf[header + 26], _read_buf[header + 27], _read_buf[header + 28], - _read_buf[header + 29] - ); - } - - read_retries--; - flush_uart_rx(); - - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("Failed to send version info packet"); - - } else { - _bytes_sent += cmd.len; - _packets_sent++; - px4_usleep(2000); - } - - break; - - } else { - memcpy(&_version_info, &_read_buf[header], sizeof(VOXL2_IO_VERSION_INFO)); - read_succeeded = 1; + //break out of the loop waiting for a response + if (got_response) { break; } + } - } else { - read_retries--; - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("Failed to send version info packet"); - - } else { - _bytes_sent += cmd.len; - _packets_sent++; - px4_usleep(2000); - } + if (!got_response) { + PX4_ERR("VOXL2_IO: Board version info response timeout (%d retries left)", retries_left); } } - if (! read_succeeded) { - return -EIO; - } - - return 0; + return (got_response == true ? 0 : -1); } bool Voxl2IO::updateOutputs(bool stop_motors, uint16_t outputs[input_rc_s::RC_INPUT_MAX_CHANNELS], unsigned num_outputs, unsigned num_control_groups_updated) { - /* Stop Mixer while ESCs are being calibrated */ + // Stop Mixer while ESCs are being calibrated if (_outputs_disabled) { return 0; } + //PX4_INFO("VOXL2_IO: Mixer output: %u, %u, %u, %u", outputs[0], outputs[1], outputs[2], outputs[3]); + //in Run() we call _mixing_output.update(), which calls MixingOutput::limitAndUpdateOutputs which calls _interface.updateOutputs (this function) //So, if Run() is blocked by a custom command, this function will not be called until Run is running again - int16_t _rate_req[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; - uint8_t _led_req[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; - int32_t _fb_idx = -1; + uint32_t output_cmds[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0, 0, 0, 0, 0}; if (num_outputs != VOXL2_IO_OUTPUT_CHANNELS) { - PX4_ERR("Num outputs != VOXL2_IO_OUTPUT_CHANNELS!"); + PX4_ERR("VOXL2_IO: Num outputs != VOXL2_IO_OUTPUT_CHANNELS!"); return false; } @@ -333,43 +305,20 @@ bool Voxl2IO::updateOutputs(bool stop_motors, uint16_t outputs[input_rc_s::RC_IN _pwm_on = true; } - if (!_pwm_on || stop_motors) { - _rate_req[i] = 0; + //Do we even need this condition? mixer should handle stopping motors anyway by sending the disable command, right? + if (0) { //(!_pwm_on || stop_motors) { + output_cmds[i] = _parameters.pwm_dis * MIXER_OUTPUT_TO_CMD_SCALE; //0; //convert to ns } else { - _rate_req[i] = outputs[i]; + output_cmds[i] = ((uint32_t)outputs[i]) * MIXER_OUTPUT_TO_CMD_SCALE; //convert to ns } - - _pwm_values[i] = _rate_req[i]; } Command cmd; - cmd.len = voxl2_io_create_pwm_packet4_fb(_rate_req[0], _rate_req[1], _rate_req[2], _rate_req[3], - _led_req[0], _led_req[1], _led_req[2], _led_req[3], - _fb_idx, cmd.buf, sizeof(cmd.buf)); + cmd.len = voxl2_io_create_hires_pwm_packet(output_cmds, VOXL2_IO_OUTPUT_CHANNELS, cmd.buf, sizeof(cmd.buf)); - if (_pwm_on && _debug) { - PX4_INFO("Mixer outputs"); - PX4_INFO("[%u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u]", - outputs[0], outputs[1], outputs[2], outputs[3], outputs[4], outputs[5], - outputs[6], outputs[7], outputs[8], outputs[9], outputs[10], outputs[11], - outputs[12], outputs[13], outputs[14], outputs[15], outputs[16], outputs[17] - ); - - // Debug messages for PWM 400Hz values sent to M0065 - uint16_t tics_1 = (_parameters.pwm_min + (_pwm_fullscale * ((double)outputs[0] / VOXL2_IO_MIXER_MAX))) * VOXL2_IO_TICS; - PX4_INFO("\tPWM CH1: %hu::%uus::%u tics", outputs[0], tics_1 / 24, tics_1); - uint16_t tics_2 = (_parameters.pwm_min + (_pwm_fullscale * ((double)outputs[1] / VOXL2_IO_MIXER_MAX))) * VOXL2_IO_TICS; - PX4_INFO("\tPWM CH2: %u::%uus::%u tics", outputs[1], tics_2 / 24, tics_2); - uint16_t tics_3 = (_parameters.pwm_min + (_pwm_fullscale * ((double)outputs[2] / VOXL2_IO_MIXER_MAX))) * VOXL2_IO_TICS; - PX4_INFO("\tPWM CH3: %u::%uus::%u tics", outputs[2], tics_3 / 24, tics_3); - uint16_t tics_4 = (_parameters.pwm_min + (_pwm_fullscale * ((double)outputs[3] / VOXL2_IO_MIXER_MAX))) * VOXL2_IO_TICS; - PX4_INFO("\tPWM CH4: %u::%uus::%u tics", outputs[3], tics_4 / 24, tics_4); - PX4_INFO(""); - } - - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("Failed to send packet"); + if (_uart_port.write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("VOXL2_IO: Failed to send packet"); return false; } else { @@ -377,40 +326,39 @@ bool Voxl2IO::updateOutputs(bool stop_motors, uint16_t outputs[input_rc_s::RC_IN _packets_sent++; } + //if (_pwm_on && _debug){ + if (_debug) { + PX4_INFO("VOXL2_IO: Mixer outputs: [%u, %u, %u, %u, %u, %u, %u, %u]", + outputs[0], outputs[1], outputs[2], outputs[3], + outputs[4], outputs[5], outputs[6], outputs[7]); + } + perf_count(_output_update_perf); return true; } -int Voxl2IO::flush_uart_rx() -{ - while (_uart_port->uart_read(_read_buf, sizeof(_read_buf)) > 0) {} - - return 0; -} - static bool valid_port(int port) { - if (port == 2 || port == 6 || port == 7) { return true; } + if (port == 2 || port == 6 || port == 7) { + return true; + } return false; } +/* int Voxl2IO::parse_response(uint8_t *buf, uint8_t len) { for (int i = 0; i < len; i++) { - int16_t ret = voxl2_io_packet_process_char(buf[i], &_sbus_packet); + int16_t ret = voxl2_io_packet_process_char(buf[i], &_voxl2_io_packet); if (ret > 0) { - uint8_t packet_type = voxl2_io_packet_get_type(&_sbus_packet); - uint8_t packet_size = voxl2_io_packet_get_size(&_sbus_packet); + uint8_t packet_type = voxl2_io_packet_get_type(&_voxl2_io_packet); + uint8_t packet_size = voxl2_io_packet_get_size(&_voxl2_io_packet); if (packet_type == VOXL2_IO_PACKET_TYPE_RC_DATA_RAW && packet_size == VOXL2_IO_SBUS_FRAME_SIZE) { return 0; - - } else if (packet_type == VOXL2_IO_PACKET_TYPE_VERSION_RESPONSE && packet_size == sizeof(VOXL2_IO_VERSION_INFO)) { - return 0; - } else { return -1; } @@ -418,36 +366,31 @@ int Voxl2IO::parse_response(uint8_t *buf, uint8_t len) } else { //parser error switch (ret) { case VOXL2_IO_ERROR_BAD_CHECKSUM: - if (_pwm_on && _debug) { PX4_WARN("BAD packet checksum"); } - + if(_pwm_on && _debug) PX4_WARN("VOXL2_IO: BAD packet checksum"); break; case VOXL2_IO_ERROR_BAD_LENGTH: - if (_pwm_on && _debug) { PX4_WARN("BAD packet length"); } - + if(_pwm_on && _debug) PX4_WARN("VOXL2_IO: BAD packet length"); break; case VOXL2_IO_ERROR_BAD_HEADER: - if (_pwm_on && _debug) { PX4_WARN("BAD packet header"); } - + if(_pwm_on && _debug) PX4_WARN("VOXL2_IO: BAD packet header"); break; case VOXL2_IO_NO_PACKET: - // if(_pwm_on) PX4_WARN("NO packet"); + // if(_pwm_on) PX4_WARN("VOXL2_IO: NO packet"); break; default: - if (_pwm_on && _debug) { PX4_WARN("Unknown error: %i", ret); } - + if(_pwm_on && _debug) PX4_WARN("VOXL2_IO: Unknown error: %i", ret); break; } - - return ret; } } return 0; } +*/ void Voxl2IO::fill_rc_in(uint16_t raw_rc_count_local, uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS], @@ -455,6 +398,8 @@ void Voxl2IO::fill_rc_in(uint16_t raw_rc_count_local, unsigned frame_drops, int rssi, input_rc_s &input_rc) { // fill rc_in struct for publishing + memset(&input_rc, 0, sizeof(input_rc)); //zero out the struct first + input_rc.channel_count = raw_rc_count_local; if (input_rc.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) { @@ -474,9 +419,9 @@ void Voxl2IO::fill_rc_in(uint16_t raw_rc_count_local, _raw_rc_values[i] = UINT16_MAX; } - input_rc.timestamp = now; + input_rc.timestamp = now; input_rc.timestamp_last_signal = input_rc.timestamp; - input_rc.rc_ppm_frame_length = 0; + input_rc.rc_ppm_frame_length = 0; /* fake rssi if no value was provided */ if (rssi == -1) { @@ -494,131 +439,98 @@ void Voxl2IO::fill_rc_in(uint16_t raw_rc_count_local, _sbus_frame_drops++; } - input_rc.rc_failsafe = failsafe; - input_rc.rc_lost = input_rc.rc_failsafe; - input_rc.rc_lost_frame_count = _sbus_frame_drops; + input_rc.rssi_dbm = 0.0f; + input_rc.rc_failsafe = failsafe; + input_rc.rc_lost = input_rc.rc_failsafe; + input_rc.rc_lost_frame_count = _sbus_frame_drops; input_rc.rc_total_frame_count = ++_sbus_total_frames; } -int Voxl2IO::receive_sbus() + +int Voxl2IO::parse_sbus_packet(uint8_t *raw_data, uint32_t data_len) { - int res = 0; - int header = -1; - int read_retries = 3; - int read_succeeded = 0; - voxl2_io_packet_init(&_sbus_packet); + input_rc_s input_rc; + uint16_t num_values; + bool sbus_failsafe = false; + bool sbus_frame_drop = false; + uint16_t max_channels = sizeof(_raw_rc_values) / sizeof(_raw_rc_values[0]); + hrt_abstime t_now = hrt_absolute_time(); - while (read_retries) { - memset(&_read_buf, 0x00, READ_BUF_SIZE); - res = _uart_port->uart_read(_read_buf, READ_BUF_SIZE); - if (res) { - /* Get index of packer header */ - for (int index = 0; index < READ_BUF_SIZE; ++index) { - if (_read_buf[index] == VOXL2_IO_PACKET_HEADER) { - header = index; - break; - } - } + bool rc_updated = sbus_parse(t_now, raw_data, data_len, _raw_rc_values, &num_values, + &sbus_failsafe, &sbus_frame_drop, &_sbus_frame_drops, max_channels); - /* Try again in a bit if packet header not present yet... */ - if (header == -1) { - if (_debug) { PX4_ERR("Failed to find SBUS packet header, trying again... retries left: %i", read_retries); } + if (rc_updated) { + //if (_pwm_on && _debug){ + if (_debug) { + //PX4_INFO("VOXL2_IO: Decoded packet, header pos: %i", header); + PX4_INFO("VOXL2_IO: [%u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u]", + _raw_rc_values[0], _raw_rc_values[1], _raw_rc_values[2], + _raw_rc_values[3], _raw_rc_values[4], _raw_rc_values[5], + _raw_rc_values[6], _raw_rc_values[7], _raw_rc_values[8], + _raw_rc_values[9], _raw_rc_values[10], _raw_rc_values[11], + _raw_rc_values[12], _raw_rc_values[13], _raw_rc_values[14], + _raw_rc_values[15], _raw_rc_values[16], _raw_rc_values[17] + ); - read_retries--; - continue; - } - - /* Check if we got a valid packet...*/ - if (parse_response(&_read_buf[header], (uint8_t)VOXL2_IO_SBUS_FRAME_SIZE)) { - if (_pwm_on && _debug) { - PX4_ERR("Error parsing QC RAW SBUS packet"); - PX4_INFO_RAW("[%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x]\n", - _read_buf[header + 0], _read_buf[header + 1], _read_buf[header + 2], _read_buf[header + 3], _read_buf[header + 4], - _read_buf[header + 5], - _read_buf[header + 6], _read_buf[header + 7], _read_buf[header + 8], _read_buf[header + 9], _read_buf[header + 10], - _read_buf[header + 11], - _read_buf[header + 12], _read_buf[header + 13], _read_buf[header + 14], _read_buf[header + 15], _read_buf[header + 16], - _read_buf[header + 17], - _read_buf[header + 18], _read_buf[header + 19], _read_buf[header + 20], _read_buf[header + 21], _read_buf[header + 22], - _read_buf[header + 23], - _read_buf[header + 24], _read_buf[header + 25], _read_buf[header + 26], _read_buf[header + 27], _read_buf[header + 28], - _read_buf[header + 29] - ); - } - - read_retries--; - break; - } - - input_rc_s input_rc; - uint16_t num_values; - bool sbus_failsafe = false; - bool sbus_frame_drop = false; - uint16_t max_channels = sizeof(_raw_rc_values) / sizeof(_raw_rc_values[0]); - hrt_abstime now = hrt_absolute_time(); - bool rc_updated = sbus_parse(now, &_read_buf[header + SBUS_PAYLOAD], SBUS_FRAME_SIZE, _raw_rc_values, &num_values, - &sbus_failsafe, &sbus_frame_drop, &_sbus_frame_drops, max_channels); - - if (rc_updated) { - if (_pwm_on && _debug) { - PX4_INFO("Decoded packet, header pos: %i", header); - PX4_INFO("[%u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u, %u]", - _raw_rc_values[0], _raw_rc_values[1], _raw_rc_values[2], - _raw_rc_values[3], _raw_rc_values[4], _raw_rc_values[5], - _raw_rc_values[6], _raw_rc_values[7], _raw_rc_values[8], - _raw_rc_values[9], _raw_rc_values[10], _raw_rc_values[11], - _raw_rc_values[12], _raw_rc_values[13], _raw_rc_values[14], - _raw_rc_values[15], _raw_rc_values[16], _raw_rc_values[17] - ); - } - - input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS; - fill_rc_in(num_values, _raw_rc_values, now, sbus_frame_drop, sbus_failsafe, _sbus_frame_drops, -1, input_rc); - - if (!input_rc.rc_lost && !input_rc.rc_failsafe) { - _rc_last_valid = input_rc.timestamp; - } - - input_rc.timestamp_last_signal = _rc_last_valid; - _rc_pub.publish(input_rc); - - _bytes_received += res; - _packets_received++; - read_succeeded = 1; - break; - - } else if (_pwm_on && _debug) { - PX4_ERR("Failed to decode SBUS packet, header pos: %i", header); - - if (sbus_frame_drop) { - PX4_WARN("SBUS frame dropped"); - } - - PX4_ERR("[%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x]", - _read_buf[header + 0], _read_buf[header + 1], _read_buf[header + 2], _read_buf[header + 3], _read_buf[header + 4], - _read_buf[header + 5], - _read_buf[header + 6], _read_buf[header + 7], _read_buf[header + 8], _read_buf[header + 9], _read_buf[header + 10], - _read_buf[header + 11], - _read_buf[header + 12], _read_buf[header + 13], _read_buf[header + 14], _read_buf[header + 15], _read_buf[header + 16], - _read_buf[header + 17], - _read_buf[header + 18], _read_buf[header + 19], _read_buf[header + 20], _read_buf[header + 21], _read_buf[header + 22], - _read_buf[header + 23], - _read_buf[header + 24], _read_buf[header + 25], _read_buf[header + 26], _read_buf[header + 27], _read_buf[header + 28], - _read_buf[header + 29] - ); + if (sbus_frame_drop) { + PX4_WARN("VOXL2_IO: SBUS frame dropped"); } } - read_retries--; + input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS; + fill_rc_in(num_values, _raw_rc_values, t_now, sbus_frame_drop, sbus_failsafe, _sbus_frame_drops, -1, input_rc); + + if (!input_rc.rc_lost && !input_rc.rc_failsafe) { + _rc_last_valid_time = input_rc.timestamp; + } + + input_rc.timestamp_last_signal = _rc_last_valid_time; + _rc_pub.publish(input_rc); + + if (_rc_mode == RC_MODE::SCAN) { + PX4_INFO("VOXL2_IO: Detected VOXL2 IO SBUS RC"); + _rc_mode = RC_MODE::SBUS; + } } - if (! read_succeeded) { - _new_packet = false; - return -EIO; + return 0; +} + + +int Voxl2IO::receive_uart_packets() +{ + int nread = _uart_port.read(_read_buf, READ_BUF_SIZE); + + if (nread > 0) { + if (_debug) { + PX4_INFO("VOXL2_IO: receive_uart_packets read %d bytes", nread); + } + + _bytes_received += nread; + + for (int i = 0; i < nread; i++) { + int16_t parse_ret = voxl2_io_packet_process_char(_read_buf[i], &_voxl2_io_packet); + + if (parse_ret > 0) { + _packets_received++; + + uint8_t packet_type = voxl2_io_packet_get_type(&_voxl2_io_packet); + uint8_t packet_size = voxl2_io_packet_get_size(&_voxl2_io_packet); + + if (packet_type == VOXL2_IO_PACKET_TYPE_RC_DATA_RAW && packet_size == VOXL2_IO_SBUS_FRAME_SIZE) { + + //parse SBUS packet only if configured to do so + if ((_rc_mode == RC_MODE::SCAN) || (_rc_mode == RC_MODE::SBUS)) { + parse_sbus_packet(&_voxl2_io_packet.buffer[SBUS_PAYLOAD], SBUS_FRAME_SIZE); + } + } + + //parse other packets (future use) + } + } } - _new_packet = true; return 0; } @@ -635,26 +547,14 @@ void Voxl2IO::Run() perf_begin(_cycle_perf); - /* Handle RC */ - if (_rc_mode == RC_MODE::SCAN) { - if (receive_sbus() == PX4_OK) { - PX4_INFO("Found M0065 SBUS RC."); - _rc_mode = RC_MODE::SBUS; - } // Add more cases here for other protocols in the future.. - - } else if (_rc_mode == RC_MODE::SBUS) { - receive_sbus(); - } - - /* Only update outputs if we have new values from RC */ - if (_new_packet || _rc_mode == RC_MODE::EXTERNAL) { - _mixing_output.update(); //calls MixingOutput::limitAndUpdateOutputs which calls updateOutputs in this module - _new_packet = false; - } + _mixing_output.update(); //calls MixingOutput::limitAndUpdateOutputs which calls updateOutputs in this module /* update PWM status if armed or if disarmed PWM values are set */ _pwm_on = _mixing_output.armed().armed; + //receive packets from voxl_io board + receive_uart_packets(); + /* check for parameter updates */ if (!_pwm_on && _parameter_update_sub.updated()) { /* clear update */ @@ -665,48 +565,6 @@ void Voxl2IO::Run() update_params(); } - /* Don't process commands if pwm on */ - if (!_pwm_on) { - if (_current_cmd.valid()) { - PX4_INFO("sending %d commands with delay %dus", _current_cmd.repeats, _current_cmd.repeat_delay_us); - flush_uart_rx(); - - do { - PX4_INFO("CMDs left %d", _current_cmd.repeats); - - if (_uart_port->uart_write(_current_cmd.buf, _current_cmd.len) == _current_cmd.len) { - if (_current_cmd.repeats == 0) { - _current_cmd.clear(); - } - - - } else { - _bytes_sent += _current_cmd.len; - _packets_sent++; - - if (_current_cmd.retries == 0) { - _current_cmd.clear(); - PX4_ERR("Failed to send command, errno: %i", errno); - - } else { - _current_cmd.retries--; - PX4_ERR("Failed to send command, errno: %i", errno); - } - } - - px4_usleep(_current_cmd.repeat_delay_us); - } while (_current_cmd.repeats-- > 0); - - } else { - Command *new_cmd = _pending_cmd.load(); - - if (new_cmd) { - _current_cmd = *new_cmd; - _pending_cmd.store(nullptr); - } - } - } - /* check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread) */ _mixing_output.updateSubscriptions(true); perf_end(_cycle_perf); @@ -728,17 +586,17 @@ int Voxl2IO::task_spawn(int argc, char *argv[]) while ((ch = px4_getopt(argc - 1, argv, "vdep:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'v': - PX4_INFO("Verbose mode enabled"); + PX4_INFO("VOXL2_IO: Verbose mode enabled"); get_instance()->_debug = true; break; case 'd': - PX4_INFO("M0065 PWM outputs disabled"); + PX4_INFO("VOXL2_IO: M0065 PWM outputs disabled"); get_instance()->_outputs_disabled = true; break; case 'e': - PX4_INFO("M0065 using external RC"); + PX4_INFO("VOXL2_IO: M0065 using external RC"); get_instance()->_rc_mode = RC_MODE::EXTERNAL; break; @@ -747,7 +605,7 @@ int Voxl2IO::task_spawn(int argc, char *argv[]) snprintf(get_instance()->_device, 2, "%s", myoptarg); } else { - PX4_ERR("Bad UART port number: %s (must be 2, 6, or 7).", myoptarg); + PX4_ERR("VOXL2_IO: Bad UART port number: %s (must be 2, 6, or 7).", myoptarg); _object.store(nullptr); _task_id = -1; return PX4_ERROR; @@ -756,7 +614,7 @@ int Voxl2IO::task_spawn(int argc, char *argv[]) break; default: - print_usage("Unknown command, parsing flags"); + print_usage("VOXL2_IO: Unknown command, parsing flags"); break; } } @@ -766,7 +624,7 @@ int Voxl2IO::task_spawn(int argc, char *argv[]) } } else { - PX4_ERR("alloc failed"); + PX4_ERR("VOXL2_IO: alloc failed"); } _object.store(nullptr); @@ -776,280 +634,131 @@ int Voxl2IO::task_spawn(int argc, char *argv[]) } -bool Voxl2IO::stop_all_pwms() -{ - int16_t _rate_req[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; - int16_t _led_req[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; - uint8_t _fb_idx = 0; - - Command cmd; - cmd.len = voxl2_io_create_pwm_packet4_fb(_rate_req[0], _rate_req[1], _rate_req[2], _rate_req[3], - _led_req[0], _led_req[1], _led_req[2], _led_req[3], - _fb_idx, cmd.buf, sizeof(cmd.buf)); - - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("Failed to send packet"); - return false; - - } else { - _bytes_sent += cmd.len; - _packets_sent++; - } - - return true; -} - -int Voxl2IO::send_cmd_thread_safe(Command *cmd) -{ - cmd->id = _cmd_id++; - _pending_cmd.store(cmd); - - /* wait until main thread processed it */ - while (_pending_cmd.load()) { - px4_usleep(1000); - } - - return 0; -} - int Voxl2IO::calibrate_escs() { - /* Disable outputs so Mixer isn't being a PITA while we calibrate */ + // Disable outputs so Mixer isn't being a PITA while we calibrate _outputs_disabled = true; Command cmd; - int32_t fb_idx = -1; - uint8_t data[VOXL2_IO_ESC_CAL_SIZE] {0}; - cmd.len = voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_TUNE_CONFIG, data, VOXL2_IO_ESC_CAL_SIZE, cmd.buf, - sizeof(cmd.buf)); - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("ESC Calibration failed: Failed to send PWM OFF packet"); - _outputs_disabled = false; - return -1; + PX4_INFO("VOXL2_IO: PWM ESC calibration is starting!"); + + // Give user 10 seconds to plug in PWM cable for ESCs + PX4_INFO("VOXL2_IO: MIN PWM used for ESC Calibration : %" PRId32, _parameters.pwm_cal_min); + PX4_INFO("VOXL2_IO: MAX PWM used for ESC Calibration : %" PRId32, _parameters.pwm_cal_max); + PX4_INFO("VOXL2_IO:"); + PX4_INFO("VOXL2_IO: Connect your ESCs! (Calibration will start in ~10 seconds)"); + + px4_usleep(10000000); + + uint32_t max_pwm_cmds[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0, 0, 0, 0, 0}; + uint32_t min_pwm_cmds[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0, 0, 0, 0, 0}; + + for (int idx = 0; idx < VOXL2_IO_OUTPUT_CHANNELS; idx++) { + //only send out calibration pulse if the actuator is a motor + if ((_parameters.function_map[idx] >= 101) && (_parameters.function_map[idx] <= 112)) { + max_pwm_cmds[idx] = _parameters.pwm_cal_max * MIXER_OUTPUT_TO_CMD_SCALE; + min_pwm_cmds[idx] = _parameters.pwm_cal_min * MIXER_OUTPUT_TO_CMD_SCALE; + } } - /* Give user 10 seconds to plug in PWM cable for ESCs */ - PX4_INFO("Disconnected and reconnect your ESCs! (Calibration will start in ~10 seconds)"); - hrt_abstime start_cal = hrt_absolute_time(); - - while (hrt_elapsed_time(&start_cal) < 10000000) { - continue; + if (_debug) { + PX4_INFO("VOXL2_IO: Scaled max pwms: %u %u %u %u %u %u %u %u", + max_pwm_cmds[0], max_pwm_cmds[1], max_pwm_cmds[2], max_pwm_cmds[3], + max_pwm_cmds[4], max_pwm_cmds[5], max_pwm_cmds[6], max_pwm_cmds[7]); } - /* PWM MAX 3 seconds */ - PX4_INFO("Writing PWM MAX for 3 seconds!"); - int16_t max_pwm[4] {VOXL2_IO_MIXER_MAX, VOXL2_IO_MIXER_MAX, VOXL2_IO_MIXER_MAX, VOXL2_IO_MIXER_MAX}; + hrt_abstime start; - if (_debug) { PX4_INFO("%i %i %i %i", max_pwm[0], max_pwm[1], max_pwm[2], max_pwm[3]); } + // Send PWM max every 2.5ms for 5 seconds + PX4_INFO("VOXL2_IO: Sending PWM MAX (%dus) for 5 seconds!", _parameters.pwm_cal_max); + cmd.len = voxl2_io_create_hires_pwm_packet(max_pwm_cmds, VOXL2_IO_OUTPUT_CHANNELS, cmd.buf, sizeof(cmd.buf)); + start = hrt_absolute_time(); - int16_t led_cmd[4] {0, 0, 0, 0}; - cmd.len = voxl2_io_create_pwm_packet4_fb(max_pwm[0], max_pwm[1], max_pwm[2], max_pwm[3], - led_cmd[0], led_cmd[1], led_cmd[2], led_cmd[3], - fb_idx, cmd.buf, sizeof(cmd.buf)); + while (hrt_elapsed_time(&start) < 5000000) { + if (_uart_port.write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("VOXL2_IO: ESC Calibration failed: Failed to send PWM MAX packet"); + _outputs_disabled = false; + return -1; + } - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("ESC Calibration failed: Failed to send PWM MAX packet"); - _outputs_disabled = false; - return -1; - - } else { - cmd.clear(); + px4_usleep(2500); } - hrt_abstime start_pwm_max = hrt_absolute_time(); + // Send PWM min every 2.5ms for 5 seconds + PX4_INFO("VOXL2_IO: Sending PWM MIN (%dus) for 5 seconds!", _parameters.pwm_cal_min); - while (hrt_elapsed_time(&start_pwm_max) < 3000000) { - continue; + if (_debug) { + PX4_INFO("VOXL2_IO: Scaled min pwms: %u %u %u %u %u %u %u %u", + min_pwm_cmds[0], min_pwm_cmds[1], min_pwm_cmds[2], min_pwm_cmds[3], + min_pwm_cmds[4], min_pwm_cmds[5], min_pwm_cmds[6], min_pwm_cmds[7]); } - /* PWM MIN 4 seconds */ - PX4_INFO("Writing PWM MIN for 4 seconds!"); - int16_t min_pwm[4] {VOXL2_IO_MIXER_MIN, VOXL2_IO_MIXER_MIN, VOXL2_IO_MIXER_MIN, VOXL2_IO_MIXER_MIN}; + cmd.len = voxl2_io_create_hires_pwm_packet(min_pwm_cmds, VOXL2_IO_OUTPUT_CHANNELS, cmd.buf, sizeof(cmd.buf)); - if (_debug) { PX4_INFO("%i %i %i %i", min_pwm[0], min_pwm[1], min_pwm[2], min_pwm[3]); } - cmd.len = voxl2_io_create_pwm_packet4_fb(min_pwm[0], min_pwm[1], min_pwm[2], min_pwm[3], - led_cmd[0], led_cmd[1], led_cmd[2], led_cmd[3], - fb_idx, cmd.buf, sizeof(cmd.buf)); + start = hrt_absolute_time(); - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("ESC Calibration failed: Failed to send PWM MIN packet"); - _outputs_disabled = false; - return -1; + while (hrt_elapsed_time(&start) < 5000000) { + if (_uart_port.write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("VOXL2_IO: ESC Calibration failed: Failed to send PWM MIN packet"); + _outputs_disabled = false; + return -1; + } + + px4_usleep(2500); } - hrt_abstime start_pwm_min = hrt_absolute_time(); - - while (hrt_elapsed_time(&start_pwm_min) < 4000000) { - continue; - } - - PX4_INFO("ESC Calibration complete"); + PX4_INFO("VOXL2_IO: Waiting 5 seconds to finish the calibration (no PWM output)"); + px4_usleep(5000000); + PX4_INFO("VOXL2_IO: ESC Calibration complete"); _outputs_disabled = false; return 0; } int Voxl2IO::custom_command(int argc, char *argv[]) { - int myoptind = 0; - int ch; - const char *myoptarg = nullptr; - - Command cmd; - uint8_t output_channel = 0xF; - int16_t rate = 0; - - uint32_t repeat_count = 100; - uint32_t repeat_delay_us = 10000; - const char *verb = argv[argc - 1]; - if ((strcmp(verb, "pwm")) == 0 && argc < 3) { - return print_usage("pwm command: missing args"); - - } else if (argc < 1) { + if (argc < 1) { return print_usage("unknown command: missing args"); } - PX4_INFO("Executing the following command: %s", verb); + PX4_INFO("VOXL2_IO: Executing the following command: %s", verb); - /* start the FMU if not running */ + /* start if not running */ if (!strcmp(verb, "start")) { if (!is_running()) { return Voxl2IO::task_spawn(argc, argv); } - } - if (!strcmp(verb, "status")) { - if (!is_running()) { - PX4_INFO("Not running"); - return -1; - } - - return get_instance()->print_status(); + PX4_INFO("VOXL2_IO: Already running"); + return 0; } if (!is_running()) { - PX4_INFO("Not running"); + PX4_INFO("VOXL2_IO: Not running"); return -1; } + if (!strcmp(verb, "status")) { + return get_instance()->print_status(); + } + + if (!strcmp(verb, "calibrate_escs")) { if (get_instance()->_outputs_disabled) { - PX4_WARN("Can't calibrate ESCs while outputs are disabled."); + PX4_WARN("VOXL2_IO: Can't calibrate ESCs while outputs are disabled."); return -1; } return get_instance()->calibrate_escs(); } - while ((ch = px4_getopt(argc, argv, "c:n:t:r:p:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'c': - output_channel = atoi(myoptarg); - - if (output_channel > VOXL2_IO_OUTPUT_CHANNELS - 1) { - char reason[50]; - sprintf(reason, "Bad channel value: %d. Must be 0-%d.", output_channel, VOXL2_IO_OUTPUT_CHANNELS - 1); - print_usage(reason); - return 0; - } - - break; - - case 'n': - repeat_count = atoi(myoptarg); - - if (repeat_count < 1) { - print_usage("bad repeat_count"); - return 0; - } - - break; - - case 't': - repeat_delay_us = atoi(myoptarg); - - if (repeat_delay_us < 1) { - print_usage("bad repeat delay"); - return 0; - } - - break; - - case 'r': - rate = atoi(myoptarg); - break; - - case 'p': - if (valid_port(atoi(myoptarg))) { - snprintf(get_instance()->_device, 2, "%s", myoptarg); - - } else { - PX4_ERR("Bad UART port number: %s (must be 2, 6, or 7).", myoptarg); - return 0; - } - - break; - - default: - print_usage("Unknown command, parsing flags"); - return 0; - } - } - - if (!strcmp(verb, "pwm")) { - PX4_INFO("Output channel: %i", output_channel); - PX4_INFO("Repeat count: %i", repeat_count); - PX4_INFO("Repeat delay (us): %i", repeat_delay_us); - PX4_INFO("Rate: %i", rate); - - if (output_channel < VOXL2_IO_OUTPUT_CHANNELS) { - PX4_INFO("Request PWM for Output Channel: %i - PWM: %i", output_channel, rate); - int16_t rate_req[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; - uint8_t id_fb = 0; - - if (output_channel == - 0xFF) { //WARNING: this condition is not possible due to check 'if (esc_id < MODAL_IO_OUTPUT_CHANNELS)'. - rate_req[0] = rate; - rate_req[1] = rate; - rate_req[2] = rate; - rate_req[3] = rate; - - } else { - rate_req[output_channel] = rate; - id_fb = output_channel; - } - - cmd.len = voxl2_io_create_pwm_packet4_fb(rate_req[0], rate_req[1], rate_req[2], rate_req[3], - 0, 0, 0, 0, - id_fb, cmd.buf, sizeof(cmd.buf)); - - cmd.response = false; - cmd.repeats = repeat_count; - cmd.resp_delay_us = 1000; - cmd.repeat_delay_us = repeat_delay_us; - cmd.print_feedback = false; - - PX4_INFO("feedback id debug: %i", id_fb); - PX4_INFO("Sending UART M0065 power command %i", rate); - - if (get_instance()->_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("Failed to send packet: stop PWMs"); - return -1; - - } else { - get_instance()->_bytes_sent += cmd.len; - get_instance()->_packets_sent++; - } - - } else { - print_usage("Invalid Output Channel, use 0-3"); - return 0; - } + if (!strcmp(verb, "enable_debug")) { + get_instance()->_debug = true; } return print_usage("unknown custom command"); @@ -1057,22 +766,20 @@ int Voxl2IO::custom_command(int argc, char *argv[]) int Voxl2IO::print_status() { - PX4_INFO("Max update rate: %u Hz", 1000000 / _current_update_interval); - PX4_INFO("PWM Rate: 400 Hz"); // Only support 400 Hz for now - PX4_INFO("Outputs on: %s", _pwm_on ? "yes" : "no"); - PX4_INFO("FW version: v%u.%u", _version_info.sw_version, _version_info.hw_version); - PX4_INFO("RC Type: SBUS"); // Only support SBUS through M0065 for now - PX4_INFO("RC Connected: %s", hrt_absolute_time() - _rc_last_valid > 500000 ? "no" : "yes"); - PX4_INFO("RC Packets Received: %" PRIu16, _sbus_total_frames); - PX4_INFO("UART port: %s", _device); - PX4_INFO("UART open: %s", _uart_port->is_open() ? "yes" : "no"); - PX4_INFO("Packets sent: %" PRIu32, _packets_sent); - PX4_INFO(""); - PX4_INFO("Params: VOXL2_IO_BAUD: %" PRId32, _parameters.baud_rate); - PX4_INFO("Params: VOXL2_IO_FUNC1: %" PRId32, _parameters.function_map[0]); - PX4_INFO("Params: VOXL2_IO_FUNC2: %" PRId32, _parameters.function_map[1]); - PX4_INFO("Params: VOXL2_IO_FUNC3: %" PRId32, _parameters.function_map[2]); - PX4_INFO("Params: VOXL2_IO_FUNC4: %" PRId32, _parameters.function_map[3]); + //PX4_INFO("VOXL2_IO: Max update rate: %u Hz", 1000000/_current_update_interval); + //PX4_INFO("VOXL2_IO: PWM Rate: 400 Hz"); // Only support 400 Hz for now + PX4_INFO("VOXL2_IO: Outputs on : %s", _pwm_on ? "yes" : "no"); + PX4_INFO("VOXL2_IO: SW version : %u", _version_info.sw_version); + PX4_INFO("VOXL2_IO: HW version : %u: %s", _version_info.hw_version, + board_id_to_name(_version_info.hw_version).c_str()); + PX4_INFO("VOXL2_IO: RC Type : SBUS"); // Only support SBUS through M0065 for now + PX4_INFO("VOXL2_IO: RC Connected : %s", hrt_absolute_time() - _rc_last_valid_time > 500000 ? "no" : "yes"); + PX4_INFO("VOXL2_IO: RC Packets Rxd : %" PRIu16, _sbus_total_frames); + PX4_INFO("VOXL2_IO: UART port : %s", _device); + PX4_INFO("VOXL2_IO: UART open : %s", _uart_port.open() ? "yes" : "no"); + PX4_INFO("VOXL2_IO: Packets sent : %" PRIu32, _packets_sent); + PX4_INFO("VOXL2_IO: "); + print_params(); perf_print_counter(_cycle_perf); perf_print_counter(_output_update_perf); @@ -1103,17 +810,48 @@ px4io driver is used for main ones. PRINT_MODULE_USAGE_PARAM_FLAG('e', "Disable RC", false); PRINT_MODULE_USAGE_PARAM_INT('p', 2, 2, 7, "UART port", false); PRINT_MODULE_USAGE_COMMAND_DESCR("calibrate_escs", "Calibrate ESCs min/max range"); - PRINT_MODULE_USAGE_COMMAND_DESCR("calibrate_escs", "Calibrate ESCs min/max range"); - PRINT_MODULE_USAGE_COMMAND_DESCR("pwm", "Open-Loop PWM test control request"); - PRINT_MODULE_USAGE_PARAM_INT('c', 0, 0, 3, "PWM OUTPUT Channel, 0-3", false); - PRINT_MODULE_USAGE_PARAM_INT('r', 0, 0, 800, "Duty Cycle value, 0 to 800", false); - PRINT_MODULE_USAGE_PARAM_INT('n', 100, 0, 1<<31, "Command repeat count, 0 to INT_MAX", false); - PRINT_MODULE_USAGE_PARAM_INT('t', 10000, 0, 1<<31, "Delay between repeated commands (microseconds), 0 to INT_MAX", false); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; } +std::string Voxl2IO::board_id_to_name(int board_id) +{ + switch(board_id){ + case 31: return std::string("ModalAi 4-in-1 ESC V2 RevB (M0049)"); + case 32: return std::string("Blheli32 4-in-1 ESC Type A (Tmotor F55A PRO F051)"); + case 33: return std::string("Blheli32 4-in-1 ESC Type B (Tmotor F55A PRO G071)"); + case 34: return std::string("ModalAi 4-in-1 ESC (M0117-1)"); + case 35: return std::string("ModalAi I/O Expander (M0065)"); + case 36: return std::string("ModalAi 4-in-1 ESC (M0117-3)"); + case 37: return std::string("ModalAi 4-in-1 ESC (M0134-1)"); + case 38: return std::string("ModalAi 4-in-1 ESC (M0134-3)"); + case 39: return std::string("ModalAi 4-in-1 ESC (M0129-1)"); + case 40: return std::string("ModalAi 4-in-1 ESC (M0129-3)"); + case 41: return std::string("ModalAi 4-in-1 ESC (M0134-6)"); + case 42: return std::string("ModalAi 4-in-1 ESC (M0138-1)"); + default: return std::string("Unknown Board"); + } +} + +void Voxl2IO::print_params() +{ + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_BAUD : %" PRId32, _parameters.baud_rate); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC1 : %" PRId32, _parameters.function_map[0]); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC2 : %" PRId32, _parameters.function_map[1]); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC3 : %" PRId32, _parameters.function_map[2]); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC4 : %" PRId32, _parameters.function_map[3]); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC5 : %" PRId32, _parameters.function_map[4]); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC6 : %" PRId32, _parameters.function_map[5]); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC7 : %" PRId32, _parameters.function_map[6]); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_FUNC8 : %" PRId32, _parameters.function_map[7]); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_DIS : %" PRId32, _parameters.pwm_dis); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_MIN : %" PRId32, _parameters.pwm_min); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_MAX : %" PRId32, _parameters.pwm_max); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_CMIN : %" PRId32, _parameters.pwm_cal_min); + PX4_INFO("VOXL2_IO: Params: VOXL2_IO_CMAX : %" PRId32, _parameters.pwm_cal_max); +} + extern "C" __EXPORT int voxl2_io_main(int argc, char *argv[]); int voxl2_io_main(int argc, char *argv[]) diff --git a/src/drivers/voxl2_io/voxl2_io.hpp b/src/drivers/voxl2_io/voxl2_io.hpp index 6ac46251bc..b3458456a0 100644 --- a/src/drivers/voxl2_io/voxl2_io.hpp +++ b/src/drivers/voxl2_io/voxl2_io.hpp @@ -35,6 +35,7 @@ #include #include +#include #include @@ -49,16 +50,17 @@ #include #include #include +#include #include #include #include #include -#include "voxl2_io_serial.hpp" - #include "voxl2_io_packet.h" #include "voxl2_io_packet_types.h" +using namespace device; + using namespace time_literals; class Voxl2IO final : public ModuleBase, public OutputModuleInterface @@ -85,8 +87,8 @@ public: virtual int init(); - void update_pwm_config(); int get_version_info(); + void print_params(); struct Command { uint16_t id = 0; @@ -105,8 +107,8 @@ public: void clear() { len = 0; } }; - int send_cmd_thread_safe(Command *cmd); - int receive_sbus(); + int receive_uart_packets(); + int parse_sbus_packet(uint8_t *raw_data, uint32_t data_len); void fill_rc_in(uint16_t raw_rc_count_local, uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS], @@ -114,53 +116,42 @@ public: unsigned frame_drops, int rssi, input_rc_s &input_rc); private: void Run() override; - bool stop_all_pwms(); /* PWM Parameters */ - static constexpr uint32_t VOXL2_IO_CONFIG = 0; // Default to off - static constexpr uint32_t VOXL2_IO_BOARD_CONFIG_SIZE = 4; // PWM_MIN, PWM_MAX, 4 bytes - static constexpr uint32_t VOXL2_IO_ESC_CAL_SIZE = 1; - static constexpr uint32_t VOXL2_IO_DEFAULT_BAUD = 921600; - static constexpr uint16_t VOXL2_IO_OUTPUT_CHANNELS = 4; - static constexpr uint16_t VOXL2_IO_OUTPUT_DISABLED = 0; - - static constexpr uint32_t VOXL2_IO_WRITE_WAIT_US = 200; - static constexpr uint32_t VOXL2_IO_DISCONNECT_TIMEOUT_US = 500000; - - static constexpr uint16_t DISARMED_VALUE = 0; - - static constexpr uint16_t VOXL2_IO_MIXER_MIN = 0; - static constexpr uint16_t VOXL2_IO_MIXER_MAX = 800; - static constexpr uint16_t VOXL2_IO_MIXER_FAILSAFE = 0; - static constexpr uint16_t VOXL2_IO_MIXER_DISARMED = 0; - - static constexpr int32_t VOXL2_IO_DEFAULT_MIN = 1000; - static constexpr int32_t VOXL2_IO_DEFAULT_MAX = 2000; - static constexpr int32_t VOXL2_IO_DEFAULT_FAILSAFE = 900; - static constexpr int32_t VOXL2_IO_TICS = 24; // 24 tics per us on M0065 timer clks + static constexpr uint32_t VOXL2_IO_DEFAULT_BAUD = 921600; + static constexpr uint16_t VOXL2_IO_OUTPUT_CHANNELS = 8; + static constexpr uint16_t VOXL2_IO_MIXER_FAILSAFE = + UINT16_MAX; //this will tell mixer to use the default failsafe depending on the function /* SBUS */ static constexpr uint16_t VOXL2_IO_SBUS_FRAME_SIZE = 30; static constexpr uint16_t SBUS_PAYLOAD = 3; + //packet packing function accepts pulse width in nanoseconds + //assuming the mixer outputs in microseconds, need to convert + static constexpr uint32_t MIXER_OUTPUT_TO_CMD_SCALE = 1000; //standard pwm + //static constexpr uint32_t MIXER_OUTPUT_TO_CMD_SCALE = 125; //oneshot125 + /* M0065 version info */ - static constexpr uint16_t VOXL2_IO_VERSION_INFO_SIZE = 6; - static constexpr uint16_t VOXL2_IO_SW_PROTOCOL_VERSION = 1; - static constexpr uint16_t VOXL2_IO_HW_PROTOCOL_VERSION = 35; - VOXL2_IO_VERSION_INFO _version_info; + static constexpr uint16_t VOXL2_IO_SW_VERSION = 2; + static constexpr uint16_t VOXL2_IO_HW_VERSION = 35; //this is the version of the HW + int _board_detect_retries{3}; + VOXL2_IO_EXTENDED_VERSION_INFO _version_info; /* Module update interval */ static constexpr unsigned _current_update_interval{4000}; // 250 Hz typedef struct { - int32_t config{VOXL2_IO_CONFIG}; int32_t baud_rate{VOXL2_IO_DEFAULT_BAUD}; - int32_t pwm_min{VOXL2_IO_DEFAULT_MIN}; - int32_t pwm_max{VOXL2_IO_DEFAULT_MAX}; - int32_t pwm_failsafe{VOXL2_IO_DEFAULT_FAILSAFE}; - int32_t param_rc_input_proto{0}; + int32_t pwm_min{0}; + int32_t pwm_max{0}; + int32_t pwm_dis{0}; + int32_t pwm_cal_min{0}; + int32_t pwm_cal_max{0}; + //int32_t pwm_failsafe{VOXL2_IO_DEFAULT_FAILSAFE}; + //int32_t param_rc_input_proto{0}; int32_t param_rc_rssi_pwm_chan{0}; - int32_t function_map[VOXL2_IO_OUTPUT_CHANNELS] {0, 0, 0, 0}; + int32_t function_map[VOXL2_IO_OUTPUT_CHANNELS] {0, 0, 0, 0, 0, 0, 0, 0}; int32_t verbose_logging{0}; } voxl2_io_params_t; voxl2_io_params_t _parameters; @@ -179,20 +170,18 @@ private: SCAN } _rc_mode{RC_MODE::SCAN}; - /* QUP7, VOXL2 J19, /dev/slpi-uart-7*/ char _device[10] {VOXL2_IO_DEFAULT_PORT}; - Voxl2IoSerial *_uart_port; + Serial _uart_port{}; /* Mixer output */ MixingOutput _mixing_output; /* RC input */ - VOXL2_IOPacket _sbus_packet; - uint64_t _rc_last_valid; + VOXL2_IOPacket _voxl2_io_packet; + uint64_t _rc_last_valid_time; uint16_t _raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {UINT16_MAX}; - unsigned _sbus_frame_drops{0}; - uint16_t _sbus_total_frames{0}; - bool _new_packet{false}; + unsigned int _sbus_frame_drops{0}; + uint32_t _sbus_total_frames{0}; /* Publications */ uORB::PublicationMulti _rc_pub{ORB_ID(input_rc)}; @@ -201,17 +190,12 @@ private: uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; bool _pwm_on{false}; - int32_t _pwm_fullscale{0}; - int16_t _pwm_values[VOXL2_IO_OUTPUT_CHANNELS] = {0, 0, 0, 0}; bool _outputs_disabled{false}; perf_counter_t _cycle_perf; perf_counter_t _output_update_perf; bool _debug{false}; - uint16_t _cmd_id{0}; - Command _current_cmd; - px4::atomic _pending_cmd{nullptr}; static const uint8_t READ_BUF_SIZE = 128; uint8_t _read_buf[READ_BUF_SIZE]; @@ -220,9 +204,8 @@ private: uint32_t _packets_sent{0}; uint32_t _packets_received{0}; - int parse_response(uint8_t *buf, uint8_t len); int load_params(voxl2_io_params_t *params); int update_params(); - int flush_uart_rx(); int calibrate_escs(); + std::string board_id_to_name(int board_id); }; diff --git a/src/drivers/voxl2_io/voxl2_io_packet.c b/src/drivers/voxl2_io/voxl2_io_packet.c index 3de26acbfe..99340fa974 100644 --- a/src/drivers/voxl2_io/voxl2_io_packet.c +++ b/src/drivers/voxl2_io/voxl2_io_packet.c @@ -150,6 +150,32 @@ int32_t voxl2_io_create_rpm_packet4_fb(int16_t rpm0, int16_t rpm1, int16_t rpm2, return voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_RPM_CMD, (uint8_t *) & (data[0]), 10, out, out_size); } +typedef struct { + uint8_t command_type; + //uint8_t channel_offset; + uint16_t vals[8]; //could be 1,2,4,6,8 +} __attribute__((__packed__)) pwm_hires_cmd_t; + +pwm_hires_cmd_t hires_cmd; + +int32_t voxl2_io_create_hires_pwm_packet(uint32_t *pwm_val_ns, uint32_t cmd_cnt, uint8_t *out, uint16_t out_size) +{ + if (cmd_cnt > 8) { + return -1; + } + + hires_cmd.command_type = 0; + //hires_cmd.channel_offset = 0; + + //resolution of commands in the packet is 0.05us = 50ns + for (uint32_t idx = 0; idx < cmd_cnt; idx++) { + hires_cmd.vals[idx] = pwm_val_ns[idx] / 50; + } + + return voxl2_io_create_packet(VOXL2_IO_PACKET_TYPE_PWM_HIRES_CMD, (uint8_t *) &hires_cmd, (cmd_cnt * 2 + 1), out, + out_size); +} + int32_t voxl2_io_create_packet(uint8_t type, uint8_t *data, uint16_t size, uint8_t *out, uint16_t out_size) { uint16_t packet_size = size + 5; diff --git a/src/drivers/voxl2_io/voxl2_io_packet.h b/src/drivers/voxl2_io/voxl2_io_packet.h index 7e6237017f..1c4d242814 100644 --- a/src/drivers/voxl2_io/voxl2_io_packet.h +++ b/src/drivers/voxl2_io/voxl2_io_packet.h @@ -213,6 +213,9 @@ int32_t voxl2_io_create_rpm_packet4(int16_t rpm0, int16_t rpm1, int16_t rpm2, in uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3, uint8_t *out, uint16_t out_size); +int32_t voxl2_io_create_hires_pwm_packet(uint32_t *pwm_val_ns, uint32_t cmd_cnt, uint8_t *out, uint16_t out_size); + + // Create a packet for sending closed-loop RPM command and LED command to 4 ESCs, also request feedback from one ESC (with id=fb_id) // Return value is the length of generated packet (if positive), otherwise error code int32_t voxl2_io_create_rpm_packet4_fb(int16_t rpm0, int16_t rpm1, int16_t rpm2, int16_t rpm3, diff --git a/src/drivers/voxl2_io/voxl2_io_packet_types.h b/src/drivers/voxl2_io/voxl2_io_packet_types.h index 150982eca4..12766fb0ef 100644 --- a/src/drivers/voxl2_io/voxl2_io_packet_types.h +++ b/src/drivers/voxl2_io/voxl2_io_packet_types.h @@ -46,6 +46,7 @@ #define VOXL2_IO_PACKET_TYPE_SOUND_CMD 3 #define VOXL2_IO_PACKET_TYPE_STEP_CMD 4 #define VOXL2_IO_PACKET_TYPE_LED_CMD 5 +#define VOXL2_IO_PACKET_TYPE_PWM_HIRES_CMD 6 #define VOXL2_IO_PACKET_TYPE_RESET_CMD 10 #define VOXL2_IO_PACKET_TYPE_SET_ID_CMD 11 #define VOXL2_IO_PACKET_TYPE_SET_DIR_CMD 12 diff --git a/src/drivers/voxl2_io/voxl2_io_params.c b/src/drivers/voxl2_io/voxl2_io_params.c index 704cca989e..ac99723b8d 100644 --- a/src/drivers/voxl2_io/voxl2_io_params.c +++ b/src/drivers/voxl2_io/voxl2_io_params.c @@ -32,34 +32,73 @@ ****************************************************************************/ /** - * UART M0065 baud rate + * VOXL2_IO UART baud rate * - * Default rate is 921600, which is used for communicating with M0065. + * Default rate is 921600, which is used for communicating with VOXL2_IO board * * @group VOXL2 IO * @unit bit/s */ PARAM_DEFINE_INT32(VOXL2_IO_BAUD, 921600); + /** - * M0065 PWM Min + * VOXL2_IO Disabled PWM * - * Minimum duration (microseconds) for M0065 PWM + * Pulse duration in disabled state (microseconds) for VOXL2_IO board * * @min 0 * @max 2000 * @group VOXL2 IO * @unit us */ -PARAM_DEFINE_INT32(VOXL2_IO_MIN, 1000); +PARAM_DEFINE_INT32(VOXL2_IO_DIS, 1000); /** - * M0065 PWM Max + * VOXL2_IO Min PWM * - * Maximum duration (microseconds) for M0065 PWM + * Minimum duration (microseconds) for VOXL2_IO board + * + * @min 0 + * @max 2000 + * @group VOXL2 IO + * @unit us + */ + +PARAM_DEFINE_INT32(VOXL2_IO_MIN, 1100); + +/** + * VOXL2_IO Max PWM + * + * Maximum duration (microseconds) for VOXL2_IO board * @min 0 * @max 2000 * @group VOXL2 IO * @unit us */ PARAM_DEFINE_INT32(VOXL2_IO_MAX, 2000); + + +/** + * VOXL2_IO Calibration Min PWM + * + * Minimum duration (microseconds) for VOXL2_IO board + * + * @min 0 + * @max 2000 + * @group VOXL2 IO + * @unit us + */ + +PARAM_DEFINE_INT32(VOXL2_IO_CMIN, 1050); + +/** + * VOXL2_IO Calibration Max PWM + * + * Maximum duration (microseconds) for VOXL2_IO board + * @min 0 + * @max 2000 + * @group VOXL2 IO + * @unit us + */ +PARAM_DEFINE_INT32(VOXL2_IO_CMAX, 2000); diff --git a/src/drivers/voxl2_io/voxl2_io_serial.cpp b/src/drivers/voxl2_io/voxl2_io_serial.cpp deleted file mode 100644 index cee0923ece..0000000000 --- a/src/drivers/voxl2_io/voxl2_io_serial.cpp +++ /dev/null @@ -1,191 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "string.h" -#include "voxl2_io_serial.hpp" - -Voxl2IoSerial::Voxl2IoSerial() -{ -} - -Voxl2IoSerial::~Voxl2IoSerial() -{ - if (_uart_fd >= 0) { - uart_close(); - } -} - -int Voxl2IoSerial::uart_open(const char *dev, speed_t speed) -{ - if (_uart_fd >= 0) { - PX4_ERR("Port in use: %s (%i)", dev, errno); - return -1; - } - - /* Open UART */ -#ifdef __PX4_QURT - _uart_fd = qurt_uart_open(dev, speed); -#else - _uart_fd = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK); -#endif - - if (_uart_fd < 0) { - PX4_ERR("Error opening port: %s (%i)", dev, errno); - return -1; - } - -#ifndef __PX4_QURT - /* Back up the original UART configuration to restore it after exit */ - int termios_state; - - if ((termios_state = tcgetattr(_uart_fd, &_orig_cfg)) < 0) { - PX4_ERR("Error configuring port: tcgetattr %s: %d", dev, termios_state); - uart_close(); - return -1; - } - - /* Fill the struct for the new configuration */ - tcgetattr(_uart_fd, &_cfg); - - /* Disable output post-processing */ - _cfg.c_oflag &= ~OPOST; - - _cfg.c_cflag |= (CLOCAL | CREAD); /* ignore modem controls */ - _cfg.c_cflag &= ~CSIZE; - _cfg.c_cflag |= CS8; /* 8-bit characters */ - _cfg.c_cflag &= ~PARENB; /* no parity bit */ - _cfg.c_cflag &= ~CSTOPB; /* only need 1 stop bit */ - _cfg.c_cflag &= ~CRTSCTS; /* no hardware flowcontrol */ - - /* setup for non-canonical mode */ - _cfg.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON); - _cfg.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN); - - if (cfsetispeed(&_cfg, speed) < 0 || cfsetospeed(&_cfg, speed) < 0) { - PX4_ERR("Error configuring port: %s: %d (cfsetispeed, cfsetospeed)", dev, termios_state); - uart_close(); - return -1; - } - - if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &_cfg)) < 0) { - PX4_ERR("Error configuring port: %s (tcsetattr)", dev); - uart_close(); - return -1; - } - -#endif - - _speed = speed; - - return 0; -} - -int Voxl2IoSerial::uart_set_baud(speed_t speed) -{ -#ifndef __PX4_QURT - - if (_uart_fd < 0) { - return -1; - } - - if (cfsetispeed(&_cfg, speed) < 0) { - return -1; - } - - if (tcsetattr(_uart_fd, TCSANOW, &_cfg) < 0) { - return -1; - } - - _speed = speed; - - return 0; -#endif - - return -1; -} - -int Voxl2IoSerial::uart_close() -{ -#ifndef __PX4_QURT - - if (_uart_fd < 0) { - PX4_ERR("invalid state for closing"); - return -1; - } - - if (tcsetattr(_uart_fd, TCSANOW, &_orig_cfg)) { - PX4_ERR("failed restoring uart to original state"); - } - - if (close(_uart_fd)) { - PX4_ERR("error closing uart"); - } - -#endif - - _uart_fd = -1; - - return 0; -} - -int Voxl2IoSerial::uart_write(FAR void *buf, size_t len) -{ - if (_uart_fd < 0 || buf == NULL) { - PX4_ERR("invalid state for writing or buffer"); - return -1; - } - -#ifdef __PX4_QURT - return qurt_uart_write(_uart_fd, (const char *) buf, len); -#else - return write(_uart_fd, buf, len); -#endif -} - -int Voxl2IoSerial::uart_read(FAR void *buf, size_t len) -{ - if (_uart_fd < 0 || buf == NULL) { - PX4_ERR("invalid state for reading or buffer"); - return -1; - } - -#ifdef __PX4_QURT -#define ASYNC_UART_READ_WAIT_US 2000 - // The UART read on SLPI is via an asynchronous service so specify a timeout - // for the return. The driver will poll periodically until the read comes in - // so this may block for a while. However, it will timeout if no read comes in. - return qurt_uart_read(_uart_fd, (char *) buf, len, ASYNC_UART_READ_WAIT_US); -#else - return read(_uart_fd, buf, len); -#endif -} diff --git a/src/drivers/voxl2_io/voxl2_io_serial.hpp b/src/drivers/voxl2_io/voxl2_io_serial.hpp deleted file mode 100644 index 638bdef288..0000000000 --- a/src/drivers/voxl2_io/voxl2_io_serial.hpp +++ /dev/null @@ -1,69 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -#include -#include -#include -#include - -#ifdef __PX4_QURT -#include -#define FAR -#endif - -class Voxl2IoSerial -{ -public: - Voxl2IoSerial(); - virtual ~Voxl2IoSerial(); - - int uart_open(const char *dev, speed_t speed); - int uart_set_baud(speed_t speed); - int uart_close(); - int uart_write(FAR void *buf, size_t len); - int uart_read(FAR void *buf, size_t len); - bool is_open() { return _uart_fd >= 0; }; - int uart_get_baud() {return _speed; } - -private: - int _uart_fd = -1; - -#if ! defined(__PX4_QURT) - struct termios _orig_cfg; - struct termios _cfg; -#endif - - int _speed = -1; -}; From 5ffa69ff54f338d09b1304930ae00883859c0560 Mon Sep 17 00:00:00 2001 From: dirksavage88 Date: Fri, 14 Feb 2025 15:16:17 -0500 Subject: [PATCH 14/91] fix newline in module.yaml Signed-off-by: dirksavage88 --- src/drivers/voxl2_io/module.yaml | 1 - 1 file changed, 1 deletion(-) diff --git a/src/drivers/voxl2_io/module.yaml b/src/drivers/voxl2_io/module.yaml index 17feaba8c5..86dc48ca58 100644 --- a/src/drivers/voxl2_io/module.yaml +++ b/src/drivers/voxl2_io/module.yaml @@ -10,4 +10,3 @@ actuator_output: group_label: 'PWMs' channel_label: 'PWM Channel' num_channels: 8 - From 01549a58326c47cf562dc190f7fa6eaf966516cc Mon Sep 17 00:00:00 2001 From: dirksavage88 Date: Fri, 14 Feb 2025 15:24:34 -0500 Subject: [PATCH 15/91] fix to kconfig newline Signed-off-by: dirksavage88 --- src/drivers/voxl2_io/Kconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/voxl2_io/Kconfig b/src/drivers/voxl2_io/Kconfig index 15964deeeb..622cad470d 100644 --- a/src/drivers/voxl2_io/Kconfig +++ b/src/drivers/voxl2_io/Kconfig @@ -2,4 +2,4 @@ menuconfig DRIVERS_VOXL2_IO bool "voxl2_io" default n ---help--- - Enable support for voxl2_io \ No newline at end of file + Enable support for voxl2_io From 024dd701fb17089b9a2d2ccce1abbc6e3aa5b23b Mon Sep 17 00:00:00 2001 From: Ramon Roche Date: Mon, 17 Feb 2025 11:11:37 -0800 Subject: [PATCH 16/91] readme: update maintainers information (#24305) * creates new maintainers file per oss best practices * adds Pedro as Space maintainer --- MAINTAINERS.md | 42 ++++++++++++++++++++++++++++++++++++++++++ README.md | 25 ++----------------------- 2 files changed, 44 insertions(+), 23 deletions(-) create mode 100644 MAINTAINERS.md diff --git a/MAINTAINERS.md b/MAINTAINERS.md new file mode 100644 index 0000000000..426852e7b2 --- /dev/null +++ b/MAINTAINERS.md @@ -0,0 +1,42 @@ +Maintainers +=========== + +See [the documentation on Maintainers](https://docs.px4.io/main/en/contribute/maintainers.html) to learn about the role of the maintainers and the process to become one. + +**Active Maintainers** + +| Name | Sector | GitHub | Chat | email +|-------------------------|--------|--------|------|---------------- +| Lorenz Meier | Founder | [LorenzMeier][LorenzMeier] | | +| Daniel Agar | Architecture | [dagar][dagar] | daniel_agar | +| Beat Küng | Architecture | [bkueng][bkueng] | beatkueng | +| Ramón Roche | CI / Testing | [mrpollo][mrpollo] | rroche | +| Mathieu Bresciani | State Estimation | [bresch][bresch] | mbresch | +| Paul Riseborough | State Estimation | [priseborough][priseborough] | | +| David Sidrane | RTOS / NuttX | [davids5][davids5] | david_s5 | +| Jayoung Lim | Simulation | [Jaeyoung-Lim][Jaeyoung-Lim] | jaeyounglim. | +| Beniamino Pozzan | ROS 2 | [beniaminopozzan][beniaminopozzan] | beniaminopozzan | +| Matthias Grob | Multirotor | [MaEtUgR][MaEtUgR] | maetugr | +| Silvan Fuhrer | Fixed-Wing / VTOL | [sfuhrer][sfuhrer] | sfuhrer | +| Christian Friedrich | Rover | [chfriedrich98][chfriedrich98] | christian982564 | +| Pedro Roque | Spacecraft | [Pedro-Roque][Pedro-Roque] | .pedroroque | + + +**Documentation Maintainers** + +| Name | GitHub | Chat | email +|------|--------|------|---------------------- +| Hamish Willee | [hamishwillee][hamishwillee] | hamishwillee | + +**Release Managers** + +| Name | GitHub | Chat | email +|------|--------|------|---------------------- +| Ramón Roche | [mrpollo][mrpollo] | rroche | +| Daniel Agar | [dagar][dagar] | daniel_agar | + +**Retired Maintainers** + +| Name | GitHub | Chat | email +|------|--------|------|---------------------- +| | | | diff --git a/README.md b/README.md index aef9e8f8a5..dac25f0ea1 100644 --- a/README.md +++ b/README.md @@ -44,30 +44,9 @@ The PX4 Dev Team syncs up on a [weekly dev call](https://docs.px4.io/main/en/con ## Maintenance Team -Note: This is the source of truth for the active maintainers of PX4 ecosystem. +See the latest list of maintainers on [MAINTAINERS](MAINTAINERS.md) file at the root of the project. -| Sector | Maintainer | -|---|---| -| Founder | [Lorenz Meier](https://github.com/LorenzMeier) | -| Architecture | [Daniel Agar](https://github.com/dagar) / [Beat Küng](https://github.com/bkueng)| -| State Estimation | [Mathieu Bresciani](https://github.com/bresch) / [Paul Riseborough](https://github.com/priseborough) | -| OS/NuttX | [David Sidrane](https://github.com/davids5) | -| Drivers | [Daniel Agar](https://github.com/dagar) | -| Simulation | [Jaeyoung Lim](https://github.com/Jaeyoung-Lim) | -| ROS2 | [Beniamino Pozzan](https://github.com/beniaminopozzan) | -| Community QnA Call | [Ramon Roche](https://github.com/mrpollo) | -| [Documentation](https://docs.px4.io/main/en/) | [Hamish Willee](https://github.com/hamishwillee) | - -| Vehicle Type | Maintainer | -|---|---| -| Multirotor | [Matthias Grob](https://github.com/MaEtUgR) | -| Fixed Wing | [Thomas Stastny](https://github.com/tstastny) | -| Hybrid VTOL | [Silvan Fuhrer](https://github.com/sfuhrer) | -| Rover | [Christian Friedrich](https://github.com/chfriedrich98) | -| Boat | x | - - -See also [maintainers list](https://px4.io/community/maintainers/) (px4.io) and the [contributors list](https://github.com/PX4/PX4-Autopilot/graphs/contributors) (Github). However it may be not up to date. +For the latest stats on contributors please see the latest stats for the Dronecode ecosystem in our project dashboard under [LFX Insights](https://insights.lfx.linuxfoundation.org/foundation/dronecode). For information on how to update your profile and affiliations please see the following support link on how to [Complete Your LFX Profile](https://docs.linuxfoundation.org/lfx/my-profile/complete-your-lfx-profile). Dronecode publishes a yearly snapshot of contributions and achievements on its [website under the Reports section](https://dronecode.org). ## Supported Hardware From e12c3c00a4e658a0206735eb7402afbbb0242b20 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 17 Feb 2025 14:35:15 -0500 Subject: [PATCH 17/91] control_allocator: param update avoid temporary - this is a harmless workaround for a GCC warning (-Wdangling-pointer) false positive --- .../ActuatorEffectivenessControlSurfaces.cpp | 6 +----- .../ActuatorEffectivenessControlSurfaces.hpp | 2 +- .../ActuatorEffectivenessHelicopter.cpp | 7 +++---- .../ActuatorEffectivenessHelicopter.hpp | 2 +- .../ActuatorEffectivenessHelicopterCoaxial.cpp | 7 +++---- .../ActuatorEffectivenessHelicopterCoaxial.hpp | 2 +- 6 files changed, 10 insertions(+), 16 deletions(-) diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp index 23dee70563..46fea5f33e 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp @@ -69,15 +69,11 @@ void ActuatorEffectivenessControlSurfaces::updateParams() { ModuleParams::updateParams(); - int32_t count = 0; - - if (param_get(_count_handle, &count) != 0) { + if (param_get(_count_handle, &_count) != PX4_OK) { PX4_ERR("param_get failed"); return; } - _count = count; - for (int i = 0; i < _count; i++) { param_get(_param_handles[i].type, (int32_t *)&_params[i].type); diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp index 7047363cc8..5e64e5a738 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp @@ -107,7 +107,7 @@ private: param_t _count_handle; Params _params[MAX_COUNT] {}; - int _count{0}; + int32_t _count{0}; SlewRate _flaps_setpoint_with_slewrate; SlewRate _spoilers_setpoint_with_slewrate; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp index 643b218c47..beacc7007b 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp @@ -73,14 +73,13 @@ void ActuatorEffectivenessHelicopter::updateParams() { ModuleParams::updateParams(); - int32_t count = 0; - - if (param_get(_param_handles.num_swash_plate_servos, &count) != 0) { + if (param_get(_param_handles.num_swash_plate_servos, &_geometry.num_swash_plate_servos) != PX4_OK) { PX4_ERR("param_get failed"); return; } - _geometry.num_swash_plate_servos = math::constrain((int)count, 3, NUM_SWASH_PLATE_SERVOS_MAX); + _geometry.num_swash_plate_servos = math::constrain(_geometry.num_swash_plate_servos, + (int32_t)3, (int32_t)NUM_SWASH_PLATE_SERVOS_MAX); for (int i = 0; i < _geometry.num_swash_plate_servos; ++i) { float angle_deg{}; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp index 93083fa066..669bb725e6 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp @@ -58,7 +58,7 @@ public: struct Geometry { SwashPlateGeometry swash_plate_servos[NUM_SWASH_PLATE_SERVOS_MAX]; - int num_swash_plate_servos{0}; + int32_t num_swash_plate_servos{0}; float throttle_curve[NUM_CURVE_POINTS]; float pitch_curve[NUM_CURVE_POINTS]; float yaw_collective_pitch_scale; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp index 0c06f5963f..40e956df99 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp @@ -60,14 +60,13 @@ void ActuatorEffectivenessHelicopterCoaxial::updateParams() { ModuleParams::updateParams(); - int32_t count = 0; - - if (param_get(_param_handles.num_swash_plate_servos, &count) != 0) { + if (param_get(_param_handles.num_swash_plate_servos, &_geometry.num_swash_plate_servos) != PX4_OK) { PX4_ERR("param_get failed"); return; } - _geometry.num_swash_plate_servos = math::constrain((int)count, 2, NUM_SWASH_PLATE_SERVOS_MAX); + _geometry.num_swash_plate_servos = math::constrain(_geometry.num_swash_plate_servos, + (int32_t)2, (int32_t)NUM_SWASH_PLATE_SERVOS_MAX); for (int i = 0; i < _geometry.num_swash_plate_servos; ++i) { float angle_deg{}; diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp index a507aee2dd..e97c31c0da 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp @@ -55,7 +55,7 @@ public: struct Geometry { SwashPlateGeometry swash_plate_servos[NUM_SWASH_PLATE_SERVOS_MAX]; - int num_swash_plate_servos{0}; + int32_t num_swash_plate_servos{0}; float spoolup_time; }; From 46609d5e6cc1af75c015a9cba09d196beda010cb Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Mon, 17 Feb 2025 18:55:24 -0900 Subject: [PATCH 18/91] voxl: document enable_debug command (#24353) --- src/drivers/voxl2_io/voxl2_io.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/voxl2_io/voxl2_io.cpp b/src/drivers/voxl2_io/voxl2_io.cpp index 90adfb8422..3461c99a4b 100644 --- a/src/drivers/voxl2_io/voxl2_io.cpp +++ b/src/drivers/voxl2_io/voxl2_io.cpp @@ -810,6 +810,7 @@ px4io driver is used for main ones. PRINT_MODULE_USAGE_PARAM_FLAG('e', "Disable RC", false); PRINT_MODULE_USAGE_PARAM_INT('p', 2, 2, 7, "UART port", false); PRINT_MODULE_USAGE_COMMAND_DESCR("calibrate_escs", "Calibrate ESCs min/max range"); + PRINT_MODULE_USAGE_COMMAND_DESCR("enable_debug", "Enables driver debugging"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; From 38de04a53a66441a65664bfbdbf4b6ce850e4882 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 17 Feb 2025 23:06:24 -0500 Subject: [PATCH 19/91] Tools/setup/ubuntu.sh: add curl for NXP mr-canhubk3 build (#24346) --- Tools/setup/ubuntu.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/setup/ubuntu.sh b/Tools/setup/ubuntu.sh index 434b2a81cd..e641a82da3 100755 --- a/Tools/setup/ubuntu.sh +++ b/Tools/setup/ubuntu.sh @@ -116,6 +116,7 @@ if [[ $INSTALL_NUTTX == "true" ]]; then binutils-dev \ bison \ build-essential \ + curl \ flex \ g++-multilib \ gcc-arm-none-eabi \ From 90b229070050af72352b3e8b7add2ecc5ac45d04 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 18 Feb 2025 00:26:06 -0500 Subject: [PATCH 20/91] Tools: docker_run.sh fix entrypoint as user - modern docker can specify the user at runtime (--user) --- Tools/docker_run.sh | 2 +- Tools/setup/docker-entrypoint.sh | 16 +--------------- 2 files changed, 2 insertions(+), 16 deletions(-) diff --git a/Tools/docker_run.sh b/Tools/docker_run.sh index c78b9cc7e1..46888cd066 100755 --- a/Tools/docker_run.sh +++ b/Tools/docker_run.sh @@ -47,6 +47,7 @@ CCACHE_DIR=${HOME}/.ccache mkdir -p "${CCACHE_DIR}" docker run -it --rm -w "${SRC_DIR}" \ + --user="$(id -u):$(id -g)" \ --env=AWS_ACCESS_KEY_ID \ --env=AWS_SECRET_ACCESS_KEY \ --env=BRANCH_NAME \ @@ -54,7 +55,6 @@ docker run -it --rm -w "${SRC_DIR}" \ --env=CI \ --env=CODECOV_TOKEN \ --env=COVERALLS_REPO_TOKEN \ - --env=LOCAL_USER_ID="$(id -u)" \ --env=PX4_ASAN \ --env=PX4_MSAN \ --env=PX4_TSAN \ diff --git a/Tools/setup/docker-entrypoint.sh b/Tools/setup/docker-entrypoint.sh index 1094587c1c..0412558902 100755 --- a/Tools/setup/docker-entrypoint.sh +++ b/Tools/setup/docker-entrypoint.sh @@ -1,6 +1,5 @@ #!/bin/bash -echo "[docker-entrypoint.sh] Starting entrypoint" # Start virtual X server in the background # - DISPLAY default is :99, set in dockerfile # - Users can override with `-e DISPLAY=` in `docker run` command to avoid @@ -17,17 +16,4 @@ if [ -n "${ROS_DISTRO}" ]; then source "/opt/ros/$ROS_DISTRO/setup.bash" fi -echo "[docker-entrypoint.sh] Working Directory: ${pwd}" - -# Use the LOCAL_USER_ID if passed in at runtime -if [ -n "${LOCAL_USER_ID}" ]; then - echo "[docker-entrypoint.sh] Starting with: $LOCAL_USER_ID:user" - # modify existing user's id - usermod -u $LOCAL_USER_ID user - - # run as user - # exec gosu user "$@" - exec "$@" -else - exec "$@" -fi +exec "$@" From d2cbe10243551b860ba06e745bf6dd9fb1d22dd7 Mon Sep 17 00:00:00 2001 From: Marco Hauswirth <58551738+haumarco@users.noreply.github.com> Date: Tue, 18 Feb 2025 13:23:10 +0100 Subject: [PATCH 21/91] Clean up temperature msg fields (#24272) * remove temp field from airspeed.msg, adjust temp selection * temp-sensor hirarchy: airspeed, ext. baro, default value * directly use diff-press or baro temp in true-airspeed calc * improve clarity * add enum for temperature source in VehicleAirData.msg --- msg/Airspeed.msg | 2 - msg/VehicleAirData.msg | 4 +- src/drivers/telemetry/hott/messages.cpp | 2 +- src/drivers/uavcan/sensors/airspeed.cpp | 1 - .../airspeed_selector_main.cpp | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 1 - src/modules/mavlink/streams/HIGHRES_IMU.hpp | 2 +- src/modules/mavlink/streams/HIGH_LATENCY2.hpp | 13 +++++- src/modules/sensors/sensors.cpp | 24 +---------- src/modules/sensors/sensors.hpp | 6 +-- .../vehicle_air_data/VehicleAirData.cpp | 40 +++++++++++-------- .../vehicle_air_data/VehicleAirData.hpp | 10 +++-- src/modules/simulation/simulator_sih/sih.cpp | 1 - 13 files changed, 50 insertions(+), 58 deletions(-) diff --git a/msg/Airspeed.msg b/msg/Airspeed.msg index aaed7b72ca..bc673cf0aa 100644 --- a/msg/Airspeed.msg +++ b/msg/Airspeed.msg @@ -5,6 +5,4 @@ float32 indicated_airspeed_m_s # indicated airspeed in m/s float32 true_airspeed_m_s # true filtered airspeed in m/s -float32 air_temperature_celsius # air temperature in degrees Celsius, -1000 if unknown - float32 confidence # confidence value from 0 to 1 for this sensor diff --git a/msg/VehicleAirData.msg b/msg/VehicleAirData.msg index 59ca5e5c8d..41e5358ae8 100644 --- a/msg/VehicleAirData.msg +++ b/msg/VehicleAirData.msg @@ -6,10 +6,10 @@ uint64 timestamp_sample # the timestamp of the raw data (microseconds) uint32 baro_device_id # unique device ID for the selected barometer float32 baro_alt_meter # Altitude above MSL calculated from temperature compensated baro sensor data using an ISA corrected for sea level pressure SENS_BARO_QNH. -float32 baro_temp_celcius # Temperature in degrees Celsius float32 baro_pressure_pa # Absolute pressure in Pascals +float32 ambient_temperature # Abient temperature in degrees Celsius +uint8 temperature_source # Source of temperature data: 0: Default Temperature (15°C), 1: External Baro, 2: Airspeed float32 rho # air density -float32 eas2tas # equivalent airspeed to true airspeed conversion factor uint8 calibration_count # Calibration changed counter. Monotonically increases whenever calibration changes. diff --git a/src/drivers/telemetry/hott/messages.cpp b/src/drivers/telemetry/hott/messages.cpp index 3be05d87a3..364d5005a7 100644 --- a/src/drivers/telemetry/hott/messages.cpp +++ b/src/drivers/telemetry/hott/messages.cpp @@ -150,7 +150,7 @@ build_eam_response(uint8_t *buffer, size_t *size) msg.eam_sensor_id = EAM_SENSOR_ID; msg.sensor_text_id = EAM_SENSOR_TEXT_ID; - msg.temperature1 = (uint8_t)(airdata.baro_temp_celcius + 20); + msg.temperature1 = (uint8_t)(airdata.ambient_temperature + 20); msg.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG; msg.main_voltage_L = (uint8_t)(battery.voltage_v * 10); diff --git a/src/drivers/uavcan/sensors/airspeed.cpp b/src/drivers/uavcan/sensors/airspeed.cpp index e62b29d5ea..2127817fd0 100644 --- a/src/drivers/uavcan/sensors/airspeed.cpp +++ b/src/drivers/uavcan/sensors/airspeed.cpp @@ -104,7 +104,6 @@ UavcanAirspeedBridge::ias_sub_cb(const report.timestamp = hrt_absolute_time(); report.indicated_airspeed_m_s = msg.indicated_airspeed; report.true_airspeed_m_s = _last_tas_m_s; - report.air_temperature_celsius = _last_outside_air_temp_k + atmosphere::kAbsoluteNullCelsius; publish(msg.getSrcNodeID().get(), &report); } diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index f8012529e1..03b9d48574 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -388,7 +388,7 @@ AirspeedModule::Run() input_data.airspeed_indicated_raw = airspeed_raw.indicated_airspeed_m_s; input_data.airspeed_true_raw = airspeed_raw.true_airspeed_m_s; input_data.airspeed_timestamp = airspeed_raw.timestamp; - input_data.air_temperature_celsius = airspeed_raw.air_temperature_celsius; + input_data.air_temperature_celsius = _vehicle_air_data.ambient_temperature; if (_in_takeoff_situation) { // set flag to false if either speed is above stall speed, diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index aa8b50d01a..00c0b9ec93 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2669,7 +2669,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) airspeed.timestamp_sample = timestamp_sample; airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f; airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f; - airspeed.air_temperature_celsius = 15.f; airspeed.timestamp = hrt_absolute_time(); _airspeed_pub.publish(airspeed); } diff --git a/src/modules/mavlink/streams/HIGHRES_IMU.hpp b/src/modules/mavlink/streams/HIGHRES_IMU.hpp index a6682213bf..7a3d96cb38 100644 --- a/src/modules/mavlink/streams/HIGHRES_IMU.hpp +++ b/src/modules/mavlink/streams/HIGHRES_IMU.hpp @@ -193,7 +193,7 @@ private: msg.abs_pressure = air_data.baro_pressure_pa; msg.diff_pressure = differential_pressure.differential_pressure_pa; msg.pressure_alt = air_data.baro_alt_meter; - msg.temperature = air_data.baro_temp_celcius; + msg.temperature = air_data.ambient_temperature; msg.fields_updated = fields_updated; mavlink_msg_highres_imu_send_struct(_mavlink->get_channel(), &msg); diff --git a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp index 6aca9f7d98..8623950e02 100644 --- a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp +++ b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp @@ -57,6 +57,7 @@ #include #include #include +#include #include @@ -521,6 +522,7 @@ private: update_gps(); update_vehicle_status(); update_wind(); + update_vehicle_air_data(); } void update_airspeed() @@ -529,7 +531,6 @@ private: if (_airspeed_sub.update(&airspeed)) { _airspeed.add_value(airspeed.indicated_airspeed_m_s, _update_rate_filtered); - _temperature.add_value(airspeed.air_temperature_celsius, _update_rate_filtered); } } @@ -610,6 +611,15 @@ private: } } + void update_vehicle_air_data() + { + vehicle_air_data_s air_data; + + if (_vehicle_air_data_sub.update(&air_data)) { + _temperature.add_value(air_data.ambient_temperature, _update_rate_filtered); + } + } + void set_default_values(mavlink_high_latency2_t &msg) const { msg.airspeed = 0; @@ -659,6 +669,7 @@ private: uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)}; uORB::Subscription _wind_sub{ORB_ID(wind)}; uORB::Subscription _health_report_sub{ORB_ID(health_report)}; + uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; SimpleAnalyzer _airspeed; SimpleAnalyzer _airspeed_sp; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 26cc3329da..97ceb6db21 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -278,32 +278,15 @@ void Sensors::diff_pres_poll() vehicle_air_data_s air_data{}; _vehicle_air_data_sub.copy(&air_data); - - float air_temperature_celsius = NAN; - - // assume anything outside of a (generous) operating range of -40C to 125C is invalid - if (PX4_ISFINITE(diff_pres.temperature) && (diff_pres.temperature >= -40.f) && (diff_pres.temperature <= 125.f)) { - - air_temperature_celsius = diff_pres.temperature; - - } else { - // differential pressure temperature invalid, check barometer - if ((air_data.timestamp != 0) && PX4_ISFINITE(air_data.baro_temp_celcius) - && (air_data.baro_temp_celcius >= -40.f) && (air_data.baro_temp_celcius <= 125.f)) { - - // TODO: review PCB_TEMP_ESTIMATE_DEG, ignore for external baro - air_temperature_celsius = air_data.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG; - } - } + const float temperature = air_data.ambient_temperature; // push raw data into validator - float airspeed_input[3] { diff_pres.differential_pressure_pa, air_temperature_celsius, 0.0f }; + float airspeed_input[3] { diff_pres.differential_pressure_pa, 0.0f, 0.0f }; _airspeed_validator.put(diff_pres.timestamp_sample, airspeed_input, diff_pres.error_count, 100); // TODO: real priority? // accumulate average for publication _diff_pres_timestamp_sum += diff_pres.timestamp_sample; _diff_pres_pressure_sum += diff_pres.differential_pressure_pa; - _diff_pres_temperature_sum += air_temperature_celsius; _baro_pressure_sum += air_data.baro_pressure_pa; _diff_pres_count++; @@ -313,12 +296,10 @@ void Sensors::diff_pres_poll() const uint64_t timestamp_sample = _diff_pres_timestamp_sum / _diff_pres_count; const float differential_pressure_pa = _diff_pres_pressure_sum / _diff_pres_count - _parameters.diff_pres_offset_pa; const float baro_pressure_pa = _baro_pressure_sum / _diff_pres_count; - const float temperature = _diff_pres_temperature_sum / _diff_pres_count; // reset _diff_pres_timestamp_sum = 0; _diff_pres_pressure_sum = 0; - _diff_pres_temperature_sum = 0; _baro_pressure_sum = 0; _diff_pres_count = 0; @@ -354,7 +335,6 @@ void Sensors::diff_pres_poll() airspeed.timestamp_sample = timestamp_sample; airspeed.indicated_airspeed_m_s = indicated_airspeed_m_s; airspeed.true_airspeed_m_s = true_airspeed_m_s; - airspeed.air_temperature_celsius = temperature; airspeed.confidence = _airspeed_validator.confidence(hrt_absolute_time()); airspeed.timestamp = hrt_absolute_time(); _airspeed_pub.publish(airspeed); diff --git a/src/modules/sensors/sensors.hpp b/src/modules/sensors/sensors.hpp index 911005249c..3a8bcd50bc 100644 --- a/src/modules/sensors/sensors.hpp +++ b/src/modules/sensors/sensors.hpp @@ -90,11 +90,7 @@ using namespace sensors; using namespace time_literals; -/** - * HACK - true temperature is much less than indicated temperature in baro, - * subtract 5 degrees in an attempt to account for the electrical upheating of the PCB - */ -#define PCB_TEMP_ESTIMATE_DEG 5.0f + class Sensors : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp index 263a56090f..a12634055f 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp @@ -45,6 +45,9 @@ using namespace matrix; using namespace atmosphere; static constexpr uint32_t SENSOR_TIMEOUT{300_ms}; +static constexpr float DEFAULT_TEMPERATURE_CELSIUS = 15.f; +static constexpr float TEMPERATURE_MIN_CELSIUS = -60.f; +static constexpr float TEMPERATURE_MAX_CELSIUS = 60.f; VehicleAirData::VehicleAirData() : ModuleParams(nullptr), @@ -77,21 +80,23 @@ void VehicleAirData::Stop() } } -void VehicleAirData::AirTemperatureUpdate() +float VehicleAirData::AirTemperatureUpdate(const float temperature_baro, TemperatureSource &source, + const hrt_abstime time_now_us) { + // use the temperature from the differential pressure sensor if available + // otherwise use the temperature from the external barometer + // Temperature measurements from internal baros are not used as typically not representative for ambient temperature + float temperature = source == TemperatureSource::EXTERNAL_BARO ? temperature_baro : DEFAULT_TEMPERATURE_CELSIUS; differential_pressure_s differential_pressure; - static constexpr float temperature_min_celsius = -20.f; - static constexpr float temperature_max_celsius = 35.f; - - // update air temperature if data from differential pressure sensor is finite and not exactly 0 - // limit the range to max 35°C to limt the error due to heated up airspeed sensors prior flight - if (_differential_pressure_sub.update(&differential_pressure) && PX4_ISFINITE(differential_pressure.temperature) - && fabsf(differential_pressure.temperature) > FLT_EPSILON) { - - _air_temperature_celsius = math::constrain(differential_pressure.temperature, temperature_min_celsius, - temperature_max_celsius); + if (_differential_pressure_sub.copy(&differential_pressure) + && time_now_us - differential_pressure.timestamp_sample < 1_s + && PX4_ISFINITE(differential_pressure.temperature)) { + temperature = differential_pressure.temperature; + source = TemperatureSource::AIRSPEED; } + + return math::constrain(temperature, TEMPERATURE_MIN_CELSIUS, TEMPERATURE_MAX_CELSIUS); } bool VehicleAirData::ParametersUpdate(bool force) @@ -139,8 +144,6 @@ void VehicleAirData::Run() const bool parameter_update = ParametersUpdate(); - AirTemperatureUpdate(); - estimator_status_flags_s estimator_status_flags; const bool estimator_status_flags_updated = _estimator_status_flags_sub.update(&estimator_status_flags); @@ -272,23 +275,26 @@ void VehicleAirData::Run() if (publish) { const float pressure_pa = _data_sum[instance] / _data_sum_count[instance]; - const float temperature = _temperature_sum[instance] / _data_sum_count[instance]; + const float temperature_baro = _temperature_sum[instance] / _data_sum_count[instance]; + TemperatureSource temperature_source = _calibration[instance].external() ? TemperatureSource::EXTERNAL_BARO : + TemperatureSource::DEFAULT_TEMP; + const float ambient_temperature = AirTemperatureUpdate(temperature_baro, temperature_source, time_now_us); const float pressure_sealevel_pa = _param_sens_baro_qnh.get() * 100.f; const float altitude = getAltitudeFromPressure(pressure_pa, pressure_sealevel_pa); // calculate air density - const float air_density = getDensityFromPressureAndTemp(pressure_pa, temperature); + const float air_density = getDensityFromPressureAndTemp(pressure_pa, ambient_temperature); // populate vehicle_air_data with and publish vehicle_air_data_s out{}; out.timestamp_sample = timestamp_sample; out.baro_device_id = _calibration[instance].device_id(); out.baro_alt_meter = altitude; - out.baro_temp_celcius = temperature; + out.ambient_temperature = ambient_temperature; + out.temperature_source = static_cast(temperature_source); out.baro_pressure_pa = pressure_pa; out.rho = air_density; - out.eas2tas = sqrtf(kAirDensitySeaLevelStandardAtmos / math::max(air_density, FLT_EPSILON)); out.calibration_count = _calibration[instance].calibration_count(); out.timestamp = hrt_absolute_time(); diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp index a0bdb4007d..1e6555fcdd 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp @@ -73,9 +73,15 @@ public: void PrintStatus(); private: + enum TemperatureSource { + DEFAULT_TEMP = 0, + EXTERNAL_BARO = 1, + AIRSPEED = 2, + }; + void Run() override; - void AirTemperatureUpdate(); + float AirTemperatureUpdate(const float temperature_baro, TemperatureSource &source, const hrt_abstime time_now_us); void CheckFailover(const hrt_abstime &time_now_us); bool ParametersUpdate(bool force = false); void UpdateStatus(); @@ -124,8 +130,6 @@ private: int8_t _selected_sensor_sub_index{-1}; - float _air_temperature_celsius{20.f}; // initialize with typical 20degC ambient temperature - bool _last_status_baro_fault{false}; DEFINE_PARAMETERS( diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index 07c24fe642..2ebf936823 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -556,7 +556,6 @@ void Sih::send_airspeed(const hrt_abstime &time_now_us) // regardless of vehicle type, body frame, etc this holds as long as wind=0 airspeed.true_airspeed_m_s = fmaxf(0.1f, _v_E.norm() + generate_wgn() * 0.2f); airspeed.indicated_airspeed_m_s = airspeed.true_airspeed_m_s * sqrtf(_wing_l.get_rho() / RHO); - airspeed.air_temperature_celsius = NAN; airspeed.confidence = 0.7f; airspeed.timestamp = hrt_absolute_time(); _airspeed_pub.publish(airspeed); From 1f2dba68d28b6a819656568d4f34f949f09f385f Mon Sep 17 00:00:00 2001 From: Silvan Date: Thu, 26 Dec 2024 21:13:51 +0100 Subject: [PATCH 22/91] remove avoidance library and logic Signed-off-by: Silvan --- Makefile | 2 +- .../1014_gazebo-classic_iris_obs_avoid | 2 - .../init.d-posix/airframes/4006_gz_px4vision | 1 - .../init.d/airframes/4016_holybro_px4vision | 1 - .../airframes/4020_holybro_px4vision_v1_5 | 1 - msg/versioned/VehicleStatus.msg | 3 - src/lib/CMakeLists.txt | 1 - src/lib/avoidance/CMakeLists.txt | 39 --- src/lib/avoidance/ObstacleAvoidance.cpp | 271 ------------------ src/lib/avoidance/ObstacleAvoidance.hpp | 152 ---------- src/lib/avoidance/ObstacleAvoidanceTest.cpp | 242 ---------------- src/lib/avoidance/ObstacleAvoidance_dummy.hpp | 93 ------ src/modules/commander/Commander.cpp | 26 -- src/modules/commander/Commander.hpp | 3 - .../checks/systemCheck.cpp | 21 -- src/modules/commander/commander_params.c | 8 - .../tasks/Auto/CMakeLists.txt | 2 +- .../tasks/Auto/FlightTaskAuto.cpp | 17 -- .../tasks/Auto/FlightTaskAuto.hpp | 10 - .../FlightTaskManualPosition.hpp | 2 +- src/modules/logger/params.c | 2 +- 21 files changed, 4 insertions(+), 895 deletions(-) delete mode 100644 src/lib/avoidance/CMakeLists.txt delete mode 100644 src/lib/avoidance/ObstacleAvoidance.cpp delete mode 100644 src/lib/avoidance/ObstacleAvoidance.hpp delete mode 100644 src/lib/avoidance/ObstacleAvoidanceTest.cpp delete mode 100644 src/lib/avoidance/ObstacleAvoidance_dummy.hpp diff --git a/Makefile b/Makefile index f549e4c8ce..c65989ffe7 100644 --- a/Makefile +++ b/Makefile @@ -404,7 +404,7 @@ check_newlines: # Testing # -------------------------------------------------------------------- -.PHONY: tests tests_coverage tests_mission tests_mission_coverage tests_offboard tests_avoidance +.PHONY: tests tests_coverage tests_mission tests_mission_coverage tests_offboard .PHONY: rostest python_coverage tests: diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid b/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid index 8221969873..e3c25ec777 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid @@ -28,5 +28,3 @@ param set-default PWM_MAIN_FUNC1 101 param set-default PWM_MAIN_FUNC2 102 param set-default PWM_MAIN_FUNC3 103 param set-default PWM_MAIN_FUNC4 104 - -param set-default COM_OBS_AVOID 1 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision b/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision index 65d290e0cc..967051b042 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision @@ -19,7 +19,6 @@ param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 # Commander Parameters -param set-default COM_OBS_AVOID 0 param set-default COM_DISARM_LAND 0.5 # EKF2 parameters diff --git a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision index d60de46e7b..9b86703c31 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision +++ b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision @@ -12,7 +12,6 @@ . ${R}etc/init.d/rc.mc_defaults # Commander Parameters -param set-default COM_OBS_AVOID 1 param set-default COM_DISARM_LAND 0.5 # EKF2 parameters diff --git a/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 b/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 index c9c3e5843a..e63a9eef01 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 +++ b/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 @@ -12,7 +12,6 @@ . ${R}etc/init.d/rc.mc_defaults # Commander Parameters -param set-default COM_OBS_AVOID 1 param set-default COM_DISARM_LAND 0.5 # EKF2 parameters diff --git a/msg/versioned/VehicleStatus.msg b/msg/versioned/VehicleStatus.msg index a347dfce3d..062c6f0765 100644 --- a/msg/versioned/VehicleStatus.msg +++ b/msg/versioned/VehicleStatus.msg @@ -131,9 +131,6 @@ bool open_drone_id_system_healthy bool parachute_system_present bool parachute_system_healthy -bool avoidance_system_required # Set to true if avoidance system is enabled via COM_OBS_AVOID parameter -bool avoidance_system_valid # Status of the obstacle avoidance system - bool rc_calibration_in_progress bool calibration_enabled diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 1075ad4cd6..5fd36f546f 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -34,7 +34,6 @@ add_subdirectory(adsb EXCLUDE_FROM_ALL) add_subdirectory(airspeed EXCLUDE_FROM_ALL) add_subdirectory(atmosphere EXCLUDE_FROM_ALL) -add_subdirectory(avoidance EXCLUDE_FROM_ALL) add_subdirectory(battery EXCLUDE_FROM_ALL) add_subdirectory(bezier EXCLUDE_FROM_ALL) add_subdirectory(button EXCLUDE_FROM_ALL) diff --git a/src/lib/avoidance/CMakeLists.txt b/src/lib/avoidance/CMakeLists.txt deleted file mode 100644 index c1da2a76dc..0000000000 --- a/src/lib/avoidance/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -############################################################################ -# -# Copyright (c) 2020 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -px4_add_library(avoidance - ObstacleAvoidance.cpp -) -target_link_libraries(avoidance PUBLIC hysteresis bezier) - -px4_add_functional_gtest(SRC ObstacleAvoidanceTest.cpp LINKLIBS avoidance) diff --git a/src/lib/avoidance/ObstacleAvoidance.cpp b/src/lib/avoidance/ObstacleAvoidance.cpp deleted file mode 100644 index f818f068a0..0000000000 --- a/src/lib/avoidance/ObstacleAvoidance.cpp +++ /dev/null @@ -1,271 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file ObstacleAvoidance.cpp - */ - -#include "ObstacleAvoidance.hpp" -#include "bezier/BezierN.hpp" - -using namespace matrix; -using namespace time_literals; - -/** Timeout in us for trajectory data to get considered invalid */ -static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500_ms; -/** If Flighttask fails, keep 0.5 seconds the current setpoint before going into failsafe land */ -static constexpr uint64_t TIME_BEFORE_FAILSAFE = 500_ms; -static constexpr uint64_t Z_PROGRESS_TIMEOUT_US = 2_s; - -ObstacleAvoidance::ObstacleAvoidance(ModuleParams *parent) : - ModuleParams(parent) -{ - _desired_waypoint = empty_trajectory_waypoint; - _failsafe_position.setNaN(); - _avoidance_point_not_valid_hysteresis.set_hysteresis_time_from(false, TIME_BEFORE_FAILSAFE); - _no_progress_z_hysteresis.set_hysteresis_time_from(false, Z_PROGRESS_TIMEOUT_US); - -} - -void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel_sp, float &yaw_sp, - float &yaw_speed_sp) -{ - _sub_vehicle_status.update(); - _sub_vehicle_trajectory_waypoint.update(); - _sub_vehicle_trajectory_bezier.update(); - - const auto &wp_msg = _sub_vehicle_trajectory_waypoint.get(); - const auto &bezier_msg = _sub_vehicle_trajectory_bezier.get(); - - const bool wp_msg_timeout = hrt_elapsed_time((hrt_abstime *)&wp_msg.timestamp) > TRAJECTORY_STREAM_TIMEOUT_US; - const bool bezier_msg_timeout = hrt_elapsed_time((hrt_abstime *)&bezier_msg.timestamp) > hrt_abstime( - bezier_msg.control_points[bezier_msg.bezier_order - 1].delta * 1e6f); - const bool avoidance_data_timeout = wp_msg_timeout && bezier_msg_timeout; - - const bool avoidance_point_valid = wp_msg.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid; - const bool avoidance_bezier_valid = bezier_msg.bezier_order > 0; - - _avoidance_point_not_valid_hysteresis.set_state_and_update(!avoidance_point_valid - && !avoidance_bezier_valid, hrt_absolute_time()); - - const bool avoidance_invalid = (avoidance_data_timeout || _avoidance_point_not_valid_hysteresis.get_state()); - - if ((_sub_vehicle_status.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) && avoidance_invalid) { - // if in nav_state LOITER and avoidance isn't healthy, don't inject setpoints from avoidance system - return; - } - - if (avoidance_invalid) { - if (_avoidance_activated) { - // Invalid point received: deactivate - PX4_WARN("Obstacle Avoidance system failed, loitering"); - _publishVehicleCmdDoLoiter(); - _avoidance_activated = false; - } - - if (!_failsafe_position.isAllFinite()) { - // save vehicle position when entering failsafe - _failsafe_position = _position; - } - - pos_sp = _failsafe_position; - vel_sp.setNaN(); - yaw_sp = NAN; - yaw_speed_sp = NAN; - - // Do nothing further - wait until activation - return; - - } else if (!_avoidance_activated) { - // First setpoint has been received: activate - PX4_INFO("Obstacle Avoidance system activated"); - _failsafe_position.setNaN(); - _avoidance_activated = true; - } - - if (avoidance_point_valid && !wp_msg_timeout) { - const auto &point0 = wp_msg.waypoints[vehicle_trajectory_waypoint_s::POINT_0]; - pos_sp = Vector3f(point0.position); - vel_sp = Vector3f(point0.velocity); - - if (!_ext_yaw_active) { - // inject yaw setpoints only if weathervane isn't active - yaw_sp = point0.yaw; - yaw_speed_sp = point0.yaw_speed; - } - - } else if (avoidance_bezier_valid && !bezier_msg_timeout) { - float yaw = NAN, yaw_speed = NAN; - _generateBezierSetpoints(pos_sp, vel_sp, yaw, yaw_speed); - - if (!_ext_yaw_active) { - // inject yaw setpoints only if weathervane isn't active - yaw_sp = yaw; - yaw_speed_sp = yaw_speed; - } - } -} - -void ObstacleAvoidance::_generateBezierSetpoints(matrix::Vector3f &position, matrix::Vector3f &velocity, - float &yaw, float &yaw_velocity) -{ - const auto &msg = _sub_vehicle_trajectory_bezier.get(); - int bezier_order = msg.bezier_order; - matrix::Vector3f bezier_points[bezier_order]; - float bezier_yaws[bezier_order]; - - for (int i = 0; i < bezier_order; i++) { - bezier_points[i] = Vector3f(msg.control_points[i].position); - bezier_yaws[i] = msg.control_points[i].yaw; - } - - const float duration_s = msg.control_points[bezier_order - 1].delta; - const hrt_abstime now = hrt_absolute_time(); - const hrt_abstime start = msg.timestamp; - const hrt_abstime end = start + hrt_abstime(duration_s * 1e6f); - - float T = NAN; - - if (bezier::calculateT(start, end, now, T) && - bezier::calculateBezierPosVel(bezier_points, bezier_order, T, position, velocity) && - bezier::calculateBezierYaw(bezier_yaws, bezier_order, T, yaw, yaw_velocity) - ) { - // translate bezier velocities T [0;1] into real velocities m/s - yaw_velocity /= duration_s; - velocity /= duration_s; - - } else { - PX4_WARN("Obstacle Avoidance system failed, bad trajectory"); - } -} - - -void ObstacleAvoidance::updateAvoidanceDesiredWaypoints(const Vector3f &curr_wp, const float curr_yaw, - const float curr_yawspeed, const Vector3f &next_wp, const float next_yaw, const float next_yawspeed, - const bool ext_yaw_active, const int wp_type) -{ - _desired_waypoint.timestamp = hrt_absolute_time(); - _desired_waypoint.type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS; - _curr_wp = curr_wp; - _ext_yaw_active = ext_yaw_active; - - curr_wp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].position); - Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].velocity); - Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].acceleration); - - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw = curr_yaw; - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw_speed = curr_yawspeed; - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].point_valid = true; - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].type = wp_type; - - next_wp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].position); - Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].velocity); - Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].acceleration); - - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw = next_yaw; - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw_speed = next_yawspeed; - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].point_valid = true; -} - -void ObstacleAvoidance::updateAvoidanceDesiredSetpoints(const Vector3f &pos_sp, const Vector3f &vel_sp, const int type) -{ - pos_sp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position); - vel_sp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity); - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].type = type; - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid = true; - - _pub_traj_wp_avoidance_desired.publish(_desired_waypoint); - - _desired_waypoint = empty_trajectory_waypoint; -} - -void ObstacleAvoidance::checkAvoidanceProgress(const Vector3f &pos, const Vector3f &prev_wp, - float target_acceptance_radius, const Vector2f &closest_pt) -{ - _position = pos; - position_controller_status_s pos_control_status = {}; - pos_control_status.timestamp = hrt_absolute_time(); - - // vector from previous triplet to current target - Vector2f prev_to_target = Vector2f(_curr_wp - prev_wp); - // vector from previous triplet to the vehicle projected position on the line previous-target triplet - Vector2f prev_to_closest_pt = closest_pt - Vector2f(prev_wp); - // fraction of the previous-tagerget line that has been flown - const float prev_curr_travelled = prev_to_closest_pt.length() / prev_to_target.length(); - - Vector2f pos_to_target = Vector2f(_curr_wp - _position); - - if (prev_curr_travelled > 1.0f) { - // if the vehicle projected position on the line previous-target is past the target waypoint, - // increase the target acceptance radius such that navigator will update the triplets - pos_control_status.acceptance_radius = pos_to_target.length() + 0.5f; - } - - const float pos_to_target_z = fabsf(_curr_wp(2) - _position(2)); - - bool no_progress_z = (pos_to_target_z > _prev_pos_to_target_z); - _no_progress_z_hysteresis.set_state_and_update(no_progress_z, hrt_absolute_time()); - - if (pos_to_target.length() < target_acceptance_radius && pos_to_target_z > _param_nav_mc_alt_rad.get() - && _no_progress_z_hysteresis.get_state()) { - // vehicle above or below the target waypoint - pos_control_status.altitude_acceptance = pos_to_target_z + 0.5f; - } - - _prev_pos_to_target_z = pos_to_target_z; - - // do not check for waypoints yaw acceptance in navigator - pos_control_status.yaw_acceptance = NAN; - - _pub_pos_control_status.publish(pos_control_status); -} - -void ObstacleAvoidance::_publishVehicleCmdDoLoiter() -{ - vehicle_command_s command{}; - command.timestamp = hrt_absolute_time(); - command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE; - command.param1 = (float)1; // base mode - command.param3 = (float)0; // sub mode - command.target_system = 1; - command.target_component = 1; - command.source_system = 1; - command.source_component = 1; - command.confirmation = false; - command.from_external = false; - command.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO; - command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - - // publish the vehicle command - _pub_vehicle_command.publish(command); -} diff --git a/src/lib/avoidance/ObstacleAvoidance.hpp b/src/lib/avoidance/ObstacleAvoidance.hpp deleted file mode 100644 index 8a27ffbbf1..0000000000 --- a/src/lib/avoidance/ObstacleAvoidance.hpp +++ /dev/null @@ -1,152 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file ObstacleAvoidance.hpp - * This class is used to inject the setpoints of an obstacle avoidance system - * into the FlightTasks - * - * @author Martina Rivizzigno - */ - -#pragma once - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include - -const vehicle_trajectory_waypoint_s empty_trajectory_waypoint = {0, 0, {0, 0, 0, 0, 0, 0, 0}, - { {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}}, - {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}}, - {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}}, - {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}}, - {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}} - } -}; - -class ObstacleAvoidance : public ModuleParams -{ -public: - ObstacleAvoidance(ModuleParams *parent); - ~ObstacleAvoidance() = default; - - /** - * Inject setpoints from obstacle avoidance system into FlightTasks. - * @param pos_sp, position setpoint - * @param vel_sp, velocity setpoint - * @param yaw_sp, yaw setpoint - * @param yaw_speed_sp, yaw speed setpoint - */ - void injectAvoidanceSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, float &yaw_sp, float &yaw_speed_sp); - - /** - * Updates the desired waypoints to send to the obstacle avoidance system. These messages don't have any direct impact on the flight. - * @param curr_wp, current position triplet - * @param curr_yaw, current yaw triplet - * @param curr_yawspeed, current yaw speed triplet - * @param next_wp, next position triplet - * @param next_yaw, next yaw triplet - * @param next_yawspeed, next yaw speed triplet - * @param wp_type, current triplet type - */ - void updateAvoidanceDesiredWaypoints(const matrix::Vector3f &curr_wp, const float curr_yaw, const float curr_yawspeed, - const matrix::Vector3f &next_wp, const float next_yaw, const float next_yawspeed, const bool ext_yaw_active, - const int wp_type); - /** - * Updates the desired setpoints to send to the obstacle avoidance system. - * @param pos_sp, desired position setpoint computed by the active FlightTask - * @param vel_sp, desired velocity setpoint computed by the active FlightTask - * @param type, current triplet type - */ - void updateAvoidanceDesiredSetpoints(const matrix::Vector3f &pos_sp, const matrix::Vector3f &vel_sp, const int type); - - /** - * Checks the vehicle progress between previous and current position waypoint of the triplet. - * @param pos, vehicle position - * @param prev_wp, previous position triplet - * @param target_acceptance_radius, current position triplet xy acceptance radius - * @param closest_pt, closest point to the vehicle on the line previous-current position triplet - */ - void checkAvoidanceProgress(const matrix::Vector3f &pos, const matrix::Vector3f &prev_wp, - float target_acceptance_radius, const matrix::Vector2f &closest_pt); - -protected: - - uORB::SubscriptionData _sub_vehicle_trajectory_bezier{ORB_ID(vehicle_trajectory_bezier)}; /**< vehicle trajectory waypoint subscription */ - uORB::SubscriptionData _sub_vehicle_trajectory_waypoint{ORB_ID(vehicle_trajectory_waypoint)}; /**< vehicle trajectory waypoint subscription */ - uORB::SubscriptionData _sub_vehicle_status{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ - - vehicle_trajectory_waypoint_s _desired_waypoint{}; /**< desired vehicle trajectory waypoint to be sent to OA */ - - uORB::Publication _pub_traj_wp_avoidance_desired{ORB_ID(vehicle_trajectory_waypoint_desired)}; /**< trajectory waypoint desired publication */ - uORB::Publication _pub_pos_control_status{ORB_ID(position_controller_status)}; /**< position controller status publication */ - uORB::Publication _pub_vehicle_command{ORB_ID(vehicle_command)}; /**< vehicle command do publication */ - - matrix::Vector3f _curr_wp = {}; /**< current position triplet */ - matrix::Vector3f _position = {}; /**< current vehicle position */ - matrix::Vector3f _failsafe_position = {}; /**< vehicle position when entered in failsafe */ - - bool _avoidance_activated{false}; /**< true after the first avoidance setpoint is received */ - - systemlib::Hysteresis _avoidance_point_not_valid_hysteresis{false}; /**< becomes true if the companion doesn't start sending valid setpoints */ - systemlib::Hysteresis _no_progress_z_hysteresis{false}; /**< becomes true if the vehicle is not making progress towards the z component of the goal */ - - float _prev_pos_to_target_z = -1.f; /**< z distance to the goal */ - - bool _ext_yaw_active = false; /**< true, if external yaw handling is active */ - - /** - * Publishes vehicle command. - */ - void _publishVehicleCmdDoLoiter(); - void _generateBezierSetpoints(matrix::Vector3f &position, matrix::Vector3f &velocity, float &yaw, float &yaw_velocity); - - DEFINE_PARAMETERS( - (ParamFloat) _param_nav_mc_alt_rad /**< Acceptance radius for multicopter altitude */ - ); - -}; diff --git a/src/lib/avoidance/ObstacleAvoidanceTest.cpp b/src/lib/avoidance/ObstacleAvoidanceTest.cpp deleted file mode 100644 index 008a8758a9..0000000000 --- a/src/lib/avoidance/ObstacleAvoidanceTest.cpp +++ /dev/null @@ -1,242 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include -#include -#include - - -using namespace matrix; -// to run: make tests TESTFILTER=ObstacleAvoidance - -class ObstacleAvoidanceTest : public ::testing::Test -{ -public: - Vector3f pos_sp; - Vector3f vel_sp; - float yaw_sp = 0.123f; - float yaw_speed_sp = NAN; - void SetUp() override - - { - param_control_autosave(false); - param_reset_all(); - pos_sp = Vector3f(1.f, 1.2f, 0.1f); - vel_sp = Vector3f(0.3f, 0.4f, 0.1f); - } -}; - -class TestObstacleAvoidance : public ::ObstacleAvoidance -{ -public: - TestObstacleAvoidance() : ObstacleAvoidance(nullptr) {} - void paramsChanged() {ObstacleAvoidance::updateParamsImpl();} - void test_setPosition(Vector3f &pos) {_position = pos;} -}; - -TEST_F(ObstacleAvoidanceTest, instantiation) { ObstacleAvoidance oa(nullptr); } - -TEST_F(ObstacleAvoidanceTest, oa_enabled_healthy) -{ - // GIVEN: the flight controller setpoints from FlightTaskAuto and a vehicle_trajectory_waypoint message coming - // from offboard - TestObstacleAvoidance oa; - - vehicle_trajectory_waypoint_s message = empty_trajectory_waypoint; - message.timestamp = hrt_absolute_time(); - message.type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS; - message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[0] = 2.6f; - message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[1] = 2.4f; - message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[2] = 2.7f; - message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw = 0.23f; - message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid = true; - - // GIVEN: and we publish the vehicle_trajectory_waypoint message and vehicle status message - uORB::Publication vehicle_trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)}; - vehicle_trajectory_waypoint_pub.publish(message); - - vehicle_status_s vehicle_status{}; - vehicle_status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; - uORB::Publication vehicle_status_pub{ORB_ID(vehicle_status)}; - vehicle_status_pub.publish(vehicle_status); - - // WHEN: we inject the vehicle_trajectory_waypoint in the interface - oa.injectAvoidanceSetpoints(pos_sp, vel_sp, yaw_sp, yaw_speed_sp); - - // THEN: the setpoints should be injected - EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[0], pos_sp(0)); - EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[1], pos_sp(1)); - EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[2], pos_sp(2)); - EXPECT_TRUE(vel_sp.isAllNan()); - EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw, yaw_sp); - EXPECT_FALSE(PX4_ISFINITE(yaw_speed_sp)); -} - -TEST_F(ObstacleAvoidanceTest, oa_enabled_healthy_bezier) -{ - // GIVEN: the flight controller setpoints from FlightTaskAuto and a vehicle_trajectory_waypoint message coming - // from offboard - TestObstacleAvoidance oa; - - vehicle_trajectory_bezier_s message {}; - message.timestamp = hrt_absolute_time(); - message.bezier_order = 2; - message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[0] = 2.6f; - message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[1] = 2.4f; - message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[2] = 2.7f; - message.control_points[vehicle_trajectory_bezier_s::POINT_0].yaw = 0.23f; - message.control_points[vehicle_trajectory_bezier_s::POINT_0].delta = NAN; - message.control_points[vehicle_trajectory_bezier_s::POINT_1].position[0] = 2.6f; - message.control_points[vehicle_trajectory_bezier_s::POINT_1].position[1] = 2.4f; - message.control_points[vehicle_trajectory_bezier_s::POINT_1].position[2] = 3.7f; - message.control_points[vehicle_trajectory_bezier_s::POINT_1].yaw = 0.23f; - message.control_points[vehicle_trajectory_bezier_s::POINT_1].delta = 0.5f; - - // GIVEN: and we publish the vehicle_trajectory_waypoint message and vehicle status message - uORB::Publication vehicle_trajectory_bezier_pub{ORB_ID(vehicle_trajectory_bezier)}; - vehicle_trajectory_bezier_pub.publish(message); - - vehicle_status_s vehicle_status{}; - vehicle_status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; - uORB::Publication vehicle_status_pub{ORB_ID(vehicle_status)}; - vehicle_status_pub.publish(vehicle_status); - - // WHEN: we inject the vehicle_trajectory_waypoint in the interface - oa.injectAvoidanceSetpoints(pos_sp, vel_sp, yaw_sp, yaw_speed_sp); - - // THEN: the setpoints should be injected - EXPECT_FLOAT_EQ(2.6f, pos_sp(0)); - EXPECT_FLOAT_EQ(2.4f, pos_sp(1)); - EXPECT_LT(2.7f, pos_sp(2)); - EXPECT_GT(2.8f, pos_sp(2)); // probably only a tiny bit above 2.7, but let's not have flakey tests - EXPECT_FLOAT_EQ(vel_sp.xy().norm(), 0); - EXPECT_FLOAT_EQ(vel_sp(2), (3.7f - 2.7f) / 0.5f); - EXPECT_FLOAT_EQ(0.23, yaw_sp); - EXPECT_FLOAT_EQ(yaw_speed_sp, 0); -} - -TEST_F(ObstacleAvoidanceTest, oa_enabled_not_healthy) -{ - // GIVEN: the flight controller setpoints from FlightTaskAuto and a vehicle_trajectory_waypoint message - TestObstacleAvoidance oa; - - vehicle_trajectory_waypoint_s message = empty_trajectory_waypoint; - Vector3f pos(3.1f, 4.7f, 5.2f); - oa.test_setPosition(pos); - - // GIVEN: and we publish the vehicle_trajectory_waypoint message and vehicle_status - uORB::Publication vehicle_trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)}; - vehicle_trajectory_waypoint_pub.publish(message); - - vehicle_status_s vehicle_status{}; - vehicle_status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; - uORB::Publication vehicle_status_pub{ORB_ID(vehicle_status)}; - vehicle_status_pub.publish(vehicle_status); - - // WHEN: we inject the vehicle_trajectory_waypoint in the interface - oa.injectAvoidanceSetpoints(pos_sp, vel_sp, yaw_sp, yaw_speed_sp); - - // THEN: the setpoints shouldn't be injected - EXPECT_FLOAT_EQ(pos(0), pos_sp(0)); - EXPECT_FLOAT_EQ(pos(1), pos_sp(1)); - EXPECT_FLOAT_EQ(pos(2), pos_sp(2)); - EXPECT_TRUE(vel_sp.isAllNan()); - EXPECT_FALSE(PX4_ISFINITE(yaw_sp)); - EXPECT_FALSE(PX4_ISFINITE(yaw_speed_sp)); -} - -TEST_F(ObstacleAvoidanceTest, oa_desired) -{ - // GIVEN: the flight controller setpoints from FlightTaskAuto and the waypoints from FLightTaskAuto - TestObstacleAvoidance oa; - - pos_sp = Vector3f(1.f, 1.2f, NAN); - vel_sp = Vector3f(NAN, NAN, 0.7f); - int type = 4; - Vector3f curr_wp(1.f, 1.2f, 5.0f); - float curr_yaw = 1.02f; - float curr_yawspeed = NAN; - Vector3f next_wp(11.f, 1.2f, 5.0f); - float next_yaw = 0.82f; - float next_yawspeed = NAN; - bool ext_yaw_active = false; - - // WHEN: we inject the setpoints and waypoints in the interface - oa.updateAvoidanceDesiredWaypoints(curr_wp, curr_yaw, curr_yawspeed, next_wp, next_yaw, next_yawspeed, ext_yaw_active, - type); - oa.updateAvoidanceDesiredSetpoints(pos_sp, vel_sp, type); - - // WHEN: we subscribe to the uORB message out of the interface - uORB::SubscriptionData _sub_traj_wp_avoidance_desired{ORB_ID(vehicle_trajectory_waypoint_desired)}; - - // THEN: we expect the setpoints in POINT_0 and waypoints in POINT_1 and POINT_2 - EXPECT_FLOAT_EQ(pos_sp(0), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[0]); - EXPECT_FLOAT_EQ(pos_sp(1), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[1]); - EXPECT_FALSE(PX4_ISFINITE( - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[2])); - EXPECT_FALSE(PX4_ISFINITE( - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[0])); - EXPECT_FALSE(PX4_ISFINITE( - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[1])); - EXPECT_FLOAT_EQ(vel_sp(2), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[2]); - EXPECT_EQ(type, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].type); - EXPECT_TRUE(_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid); - - EXPECT_FLOAT_EQ(curr_wp(0), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].position[0]); - EXPECT_FLOAT_EQ(curr_wp(1), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].position[1]); - EXPECT_FLOAT_EQ(curr_wp(2), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].position[2]); - EXPECT_FLOAT_EQ(curr_yaw, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw); - EXPECT_FALSE(PX4_ISFINITE( - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw_speed)); - EXPECT_EQ(type, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].type); - EXPECT_TRUE(_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].point_valid); - - EXPECT_FLOAT_EQ(next_wp(0), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].position[0]); - EXPECT_FLOAT_EQ(next_wp(1), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].position[1]); - EXPECT_FLOAT_EQ(next_wp(2), - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].position[2]); - EXPECT_FLOAT_EQ(next_yaw, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw); - EXPECT_FALSE(PX4_ISFINITE( - _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw_speed)); - EXPECT_EQ(UINT8_MAX, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].type); - EXPECT_TRUE(_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].point_valid); - -} diff --git a/src/lib/avoidance/ObstacleAvoidance_dummy.hpp b/src/lib/avoidance/ObstacleAvoidance_dummy.hpp deleted file mode 100644 index 7831c919fd..0000000000 --- a/src/lib/avoidance/ObstacleAvoidance_dummy.hpp +++ /dev/null @@ -1,93 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file ObstacleAvoidance_dummy.hpp - * This is a dummy class to reduce flash space for when obstacle avoidance is not required - * - * @author Julian Kent - */ - -#pragma once - -#include -#include - - -#include - - -class ObstacleAvoidance -{ -public: - ObstacleAvoidance(void *) {} // takes void* argument to be compatible with ModuleParams constructor - - - void injectAvoidanceSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, float &yaw_sp, - float &yaw_speed_sp) - { - notify_dummy(); - }; - - void updateAvoidanceDesiredWaypoints(const matrix::Vector3f &curr_wp, const float curr_yaw, - const float curr_yawspeed, - const matrix::Vector3f &next_wp, const float next_yaw, const float next_yawspeed, const bool ext_yaw_active, - const int wp_type) - { - notify_dummy(); - }; - - void updateAvoidanceDesiredSetpoints(const matrix::Vector3f &pos_sp, const matrix::Vector3f &vel_sp, - const int type) - { - notify_dummy(); - } - - - void checkAvoidanceProgress(const matrix::Vector3f &pos, const matrix::Vector3f &prev_wp, - float target_acceptance_radius, const matrix::Vector2f &closest_pt) - { - notify_dummy(); - }; - -protected: - - bool _logged_error = false; - void notify_dummy() - { - if (!_logged_error) { - PX4_ERR("Dummy avoidance library called!"); - _logged_error = true; - } - } -}; diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index a5e1583175..f0537c7c6e 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1717,8 +1717,6 @@ void Commander::updateParameters() _vehicle_status.system_type = value_int32; } - _vehicle_status.avoidance_system_required = _param_com_obs_avoid.get(); - _auto_disarm_killed.set_hysteresis_time_from(false, _param_com_kill_disarm.get() * 1_s); const bool is_rotary = is_rotary_wing(_vehicle_status) || (is_vtol(_vehicle_status) @@ -1799,9 +1797,6 @@ void Commander::run() _status_changed = true; } - /* Update OA parameter */ - _vehicle_status.avoidance_system_required = _param_com_obs_avoid.get(); - handlePowerButtonState(); systemPowerUpdate(); @@ -2814,16 +2809,6 @@ void Commander::dataLinkCheck() _vehicle_status.open_drone_id_system_present = true; _vehicle_status.open_drone_id_system_healthy = healthy; } - - if (telemetry.heartbeat_component_obstacle_avoidance) { - if (_avoidance_system_lost) { - _avoidance_system_lost = false; - _status_changed = true; - } - - _datalink_last_heartbeat_avoidance_system = telemetry.timestamp; - _vehicle_status.avoidance_system_valid = telemetry.avoidance_system_healthy; - } } } @@ -2875,17 +2860,6 @@ void Commander::dataLinkCheck() _open_drone_id_system_lost = true; _status_changed = true; } - - // AVOIDANCE SYSTEM state check (only if it is enabled) - if (_vehicle_status.avoidance_system_required && !_onboard_controller_lost) { - // if heartbeats stop - if (!_avoidance_system_lost && (_datalink_last_heartbeat_avoidance_system > 0) - && (hrt_elapsed_time(&_datalink_last_heartbeat_avoidance_system) > 5_s)) { - - _avoidance_system_lost = true; - _vehicle_status.avoidance_system_valid = false; - } - } } void Commander::battery_status_check() diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 04c1a0d05a..395e7954cb 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -241,7 +241,6 @@ private: Hysteresis _auto_disarm_killed{false}; hrt_abstime _datalink_last_heartbeat_open_drone_id_system{0}; - hrt_abstime _datalink_last_heartbeat_avoidance_system{0}; hrt_abstime _datalink_last_heartbeat_gcs{0}; hrt_abstime _datalink_last_heartbeat_onboard_controller{0}; hrt_abstime _datalink_last_heartbeat_parachute_system{0}; @@ -268,7 +267,6 @@ private: bool _failsafe_user_override_request{false}; ///< override request due to stick movements bool _open_drone_id_system_lost{true}; - bool _avoidance_system_lost{false}; bool _onboard_controller_lost{false}; bool _parachute_system_lost{true}; @@ -342,7 +340,6 @@ private: (ParamBool) _param_com_force_safety, (ParamFloat) _param_com_kill_disarm, (ParamBool) _param_com_mot_test_en, - (ParamBool) _param_com_obs_avoid, (ParamFloat) _param_com_obc_loss_t, (ParamInt) _param_com_prearm_mode, (ParamInt) _param_com_rc_override, diff --git a/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp index 32beaba9af..1f2592c9ed 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp @@ -121,27 +121,6 @@ void SystemChecks::checkAndReport(const Context &context, Report &reporter) } } - // avoidance system - if (context.status().avoidance_system_required) { - if (context.status().avoidance_system_valid) { - reporter.setIsPresent(health_component_t::avoidance); - - } else { - /* EVENT - * @description - * - * This check can be configured via COM_OBS_AVOID parameter. - * - */ - reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_avoidance_not_ready"), - events::Log::Error, "Avoidance system not ready"); - - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Avoidance system not ready"); - } - } - } - // VTOL in transition if (context.status().is_vtol && !context.isArmed()) { if (context.status().in_transition_mode) { diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 37dcbdc9a9..44c2f07987 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -640,14 +640,6 @@ PARAM_DEFINE_INT32(COM_RCL_EXCEPT, 0); */ PARAM_DEFINE_INT32(COM_ACT_FAIL_ACT, 0); -/** - * Flag to enable obstacle avoidance. - * - * @boolean - * @group Commander - */ -PARAM_DEFINE_INT32(COM_OBS_AVOID, 0); - /** * Expect and require a healthy MAVLink parachute system * diff --git a/src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt index 2fadf2754d..905e08e80b 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt +++ b/src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt @@ -35,5 +35,5 @@ px4_add_library(FlightTaskAuto FlightTaskAuto.cpp ) -target_link_libraries(FlightTaskAuto PUBLIC avoidance FlightTask FlightTaskUtility WeatherVane) +target_link_libraries(FlightTaskAuto PUBLIC FlightTask FlightTaskUtility WeatherVane) target_include_directories(FlightTaskAuto PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 8e19ffd5a3..614bcc6118 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -156,12 +156,6 @@ bool FlightTaskAuto::update() break; } - if (_param_com_obs_avoid.get()) { - _obstacle_avoidance.updateAvoidanceDesiredSetpoints(_position_setpoint, _velocity_setpoint, (int)_type); - _obstacle_avoidance.injectAvoidanceSetpoints(_position_setpoint, _velocity_setpoint, _yaw_setpoint, - _yawspeed_setpoint); - } - _checkEmergencyBraking(); Vector3f waypoints[] = {_prev_wp, _position_setpoint, _next_wp}; @@ -477,17 +471,6 @@ bool FlightTaskAuto::_evaluateTriplets() _updateInternalWaypoints(); } - if (_param_com_obs_avoid.get() - && _sub_vehicle_status.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - _obstacle_avoidance.updateAvoidanceDesiredWaypoints(_triplet_target, _yaw_setpoint, _yawspeed_setpoint, - _triplet_next_wp, - _sub_triplet_setpoint.get().next.yaw, - (float)NAN, - _weathervane.isActive(), _sub_triplet_setpoint.get().current.type); - _obstacle_avoidance.checkAvoidanceProgress( - _position, _triplet_prev_wp, _target_acceptance_radius, Vector2f(_closest_pt)); - } - // set heading _weathervane.update(); diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp index 90a98cac23..6c49dd8726 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp @@ -53,13 +53,6 @@ #include "StickAccelerationXY.hpp" #include "StickYaw.hpp" -// TODO: make this switchable in the board config, like a module -#if CONSTRAINED_FLASH -#include -#else -#include -#endif - /** * This enum has to agree with position_setpoint_s type definition * The only reason for not using the struct position_setpoint is because @@ -147,8 +140,6 @@ protected: AlphaFilter _yawspeed_filter; bool _yaw_sp_aligned{false}; - ObstacleAvoidance _obstacle_avoidance{this}; /**< class adjusting setpoints according to external avoidance module's input */ - PositionSmoothing _position_smoothing; Vector3f _unsmoothed_velocity_setpoint; Sticks _sticks{this}; @@ -165,7 +156,6 @@ protected: (ParamFloat) _param_nav_mc_alt_rad, //vertical acceptance radius at which waypoints are updated (ParamInt) _param_mpc_yaw_mode, // defines how heading is executed, - (ParamInt) _param_com_obs_avoid, // obstacle avoidance active (ParamFloat) _param_mpc_yawrauto_max, (ParamFloat) _param_mis_yaw_err, // yaw-error threshold (ParamFloat) _param_mpc_acc_hor, // acceleration in flight diff --git a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp index fb7f5dd4e5..b1f05092d3 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp @@ -68,5 +68,5 @@ private: uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */ WeatherVane _weathervane{this}; /**< weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ - CollisionPrevention _collision_prevention{this}; /**< collision avoidance setpoint amendment */ + CollisionPrevention _collision_prevention{this}; /**< collision prevention setpoint amendment */ }; diff --git a/src/modules/logger/params.c b/src/modules/logger/params.c index 3d7e873854..e1bbd5145f 100644 --- a/src/modules/logger/params.c +++ b/src/modules/logger/params.c @@ -123,7 +123,7 @@ PARAM_DEFINE_INT32(SDLOG_MISSION, 0); * 4 : Full rates for analysis of fast maneuvers (RC, attitude, rates and actuators) * 5 : Debugging topics (debug_*.msg topics, for custom code) * 6 : Topics for sensor comparison (low rate raw IMU, Baro and magnetometer data) - * 7 : Topics for computer vision and collision avoidance + * 7 : Topics for computer vision and collision prevention * 8 : Raw FIFO high-rate IMU (Gyro) * 9 : Raw FIFO high-rate IMU (Accel) * 10: Logging of mavlink tunnel message (useful for payload communication debugging) From b34a5eb6f7c590e5acccba356fc0c477daa8e59e Mon Sep 17 00:00:00 2001 From: Silvan Date: Thu, 26 Dec 2024 21:44:30 +0100 Subject: [PATCH 23/91] PositionControllerStatus: remove unused fields Remove yaw_acceptance and altitude_acceptance_radius fields as they were only filled by now removed avoidance controller. Signed-off-by: Silvan --- msg/PositionControllerStatus.msg | 2 -- .../fw_pos_control/FixedwingPositionControl.cpp | 2 -- src/modules/navigator/navigator_main.cpp | 14 -------------- .../rover_pos_control/RoverPositionControl.cpp | 1 - 4 files changed, 19 deletions(-) diff --git a/msg/PositionControllerStatus.msg b/msg/PositionControllerStatus.msg index 7237351fd0..44ec5412ba 100644 --- a/msg/PositionControllerStatus.msg +++ b/msg/PositionControllerStatus.msg @@ -7,6 +7,4 @@ float32 target_bearing # Bearing angle from aircraft to current target [rad] float32 xtrack_error # Signed track error [m] float32 wp_dist # Distance to active (next) waypoint [m] float32 acceptance_radius # Current horizontal acceptance radius [m] -float32 yaw_acceptance # Yaw acceptance error[rad] -float32 altitude_acceptance # Current vertical acceptance error [m] uint8 type # Current (applied) position setpoint type (see PositionSetpoint.msg) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 8baddd55e6..5a55be1cc8 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -487,8 +487,6 @@ FixedwingPositionControl::status_publish() pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(_current_latitude, _current_longitude, _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon); - pos_ctrl_status.yaw_acceptance = NAN; - pos_ctrl_status.timestamp = hrt_absolute_time(); pos_ctrl_status.type = _position_sp_type; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 201eeb6123..34b4d2c00e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1142,17 +1142,11 @@ float Navigator::get_altitude_acceptance_radius() } else { float alt_acceptance_radius = _param_nav_mc_alt_rad.get(); - - const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get(); - const position_setpoint_s &curr_sp = get_position_setpoint_triplet()->current; if (PX4_ISFINITE(curr_sp.alt_acceptance_radius) && curr_sp.alt_acceptance_radius > FLT_EPSILON) { alt_acceptance_radius = curr_sp.alt_acceptance_radius; - } else if ((pos_ctrl_status.timestamp > _pos_sp_triplet.timestamp) - && pos_ctrl_status.altitude_acceptance > alt_acceptance_radius) { - alt_acceptance_radius = pos_ctrl_status.altitude_acceptance; } return alt_acceptance_radius; @@ -1214,14 +1208,6 @@ bool Navigator::get_yaw_to_be_accepted(float mission_item_yaw) { float yaw = mission_item_yaw; - const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get(); - - // if yaw_acceptance from position controller is NaN overwrite the mission item yaw such that - // the waypoint can be reached from any direction - if ((pos_ctrl_status.timestamp > _pos_sp_triplet.timestamp) && !PX4_ISFINITE(pos_ctrl_status.yaw_acceptance)) { - yaw = pos_ctrl_status.yaw_acceptance; - } - return PX4_ISFINITE(yaw); } diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index 7466077a93..119c8c18e8 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -441,7 +441,6 @@ RoverPositionControl::Run() _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon); pos_ctrl_status.acceptance_radius = turn_distance; - pos_ctrl_status.yaw_acceptance = NAN; pos_ctrl_status.timestamp = hrt_absolute_time(); From b7b6d45e183175815b08d278aab6f3c0249d45dc Mon Sep 17 00:00:00 2001 From: Silvan Date: Fri, 10 Jan 2025 11:35:35 +0100 Subject: [PATCH 24/91] lib: remove bezier Signed-off-by: Silvan --- platforms/posix/cmake/sitl_tests.cmake | 1 - src/lib/CMakeLists.txt | 1 - src/lib/bezier/BezierN.cpp | 163 --------- src/lib/bezier/BezierN.hpp | 76 ---- src/lib/bezier/BezierNTest.cpp | 325 ------------------ src/lib/bezier/BezierQuad.cpp | 223 ------------ src/lib/bezier/BezierQuad.hpp | 210 ----------- src/lib/bezier/CMakeLists.txt | 39 --- .../tasks/Utility/CMakeLists.txt | 2 +- 9 files changed, 1 insertion(+), 1039 deletions(-) delete mode 100644 src/lib/bezier/BezierN.cpp delete mode 100644 src/lib/bezier/BezierN.hpp delete mode 100644 src/lib/bezier/BezierNTest.cpp delete mode 100644 src/lib/bezier/BezierQuad.cpp delete mode 100644 src/lib/bezier/BezierQuad.hpp delete mode 100644 src/lib/bezier/CMakeLists.txt diff --git a/platforms/posix/cmake/sitl_tests.cmake b/platforms/posix/cmake/sitl_tests.cmake index bf69b6a7ce..fdd8a5300e 100644 --- a/platforms/posix/cmake/sitl_tests.cmake +++ b/platforms/posix/cmake/sitl_tests.cmake @@ -5,7 +5,6 @@ # tests command arguments set(tests atomic_bitset - bezier bitset bson dataman diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 5fd36f546f..a9396caa1f 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -35,7 +35,6 @@ add_subdirectory(adsb EXCLUDE_FROM_ALL) add_subdirectory(airspeed EXCLUDE_FROM_ALL) add_subdirectory(atmosphere EXCLUDE_FROM_ALL) add_subdirectory(battery EXCLUDE_FROM_ALL) -add_subdirectory(bezier EXCLUDE_FROM_ALL) add_subdirectory(button EXCLUDE_FROM_ALL) add_subdirectory(cdev EXCLUDE_FROM_ALL) add_subdirectory(cdrstream EXCLUDE_FROM_ALL) diff --git a/src/lib/bezier/BezierN.cpp b/src/lib/bezier/BezierN.cpp deleted file mode 100644 index 0d30aebd88..0000000000 --- a/src/lib/bezier/BezierN.cpp +++ /dev/null @@ -1,163 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -/** - * @file BezierN.cpp - * Bezier function - * - * @author Julian Kent - */ - -#include -#include - -namespace -{ - -/* - * Generic in-place bezier implementation. Leaves result in first element. - * - */ -template -void calculateBezier(matrix::Vector *positions, int N, Scalar t, Scalar one_minus_t) -{ - for (int bezier_order = 1; bezier_order < N; bezier_order++) { - for (int i = 0; i < N - bezier_order; i++) { - positions[i] = positions[i] * one_minus_t + positions[i + 1] * t; - } - } -} -} - -namespace bezier -{ - -bool calculateBezierPosVel(const matrix::Vector3f *positions, int N, float t, - matrix::Vector3f &position, matrix::Vector3f &velocity) -{ - if (positions == nullptr || N == 0 || t < 0 || t > 1) { - return false; - } - - using Df = matrix::Dual; - using Vector3Df = matrix::Vector3; - - Vector3Df intermediates[N]; - - for (int i = 0; i < N; i++) { - for (int j = 0; j < 3; j++) { - intermediates[i](j) = positions[i](j); - } - } - - Df dual_t(t, 0); // derivative with respect to time - calculateBezier(intermediates, N, dual_t, Df(1) - dual_t); - - position = matrix::collectReals(intermediates[0]); - velocity = matrix::collectDerivatives(intermediates[0]); - - return true; -} - -bool calculateBezierPosVelAcc(const matrix::Vector3f *positions, int N, float t, - matrix::Vector3f &position, matrix::Vector3f &velocity, matrix::Vector3f &acceleration) -{ - if (positions == nullptr || N == 0 || t < 0 || t > 1) { - return false; - } - - using Df = matrix::Dual; - using DDf = matrix::Dual; - using Vector3DDf = matrix::Vector3; - - Vector3DDf intermediates[N]; - - for (int i = 0; i < N; i++) { - for (int j = 0; j < 3; j++) { - intermediates[i](j) = Df(positions[i](j)); - } - } - - DDf dual_t(Df(t, 0), 0); // 1st and 2nd derivative with respect to time - calculateBezier(intermediates, N, dual_t, Df(1) - dual_t); - - position = matrix::collectReals(matrix::collectReals(intermediates[0])); - velocity = matrix::collectReals(matrix::collectDerivatives(intermediates[0])); - acceleration = matrix::collectDerivatives(matrix::collectDerivatives(intermediates[0])); - - return true; -} - -bool calculateBezierYaw(const float *setpoints, int N, float t, float &yaw_setpoint, float &yaw_vel_setpoint) -{ - if (setpoints == nullptr || N == 0 || t < 0 || t > 1) { - return false; - } - - - using Df = matrix::Dual; - using Vector1Df = matrix::Vector; - - Vector1Df intermediates[N]; - - // all yaw setpoints are wrapped relative to the starting yaw - const float offset = setpoints[0]; - - for (int i = 0; i < N; i++) { - intermediates[i](0) = matrix::wrap_pi(setpoints[i] - offset); - } - - Df dual_t (t, 0); // derivative with respect to time - calculateBezier(intermediates, N, dual_t, Df(1) - dual_t); - - Df result = intermediates[0](0); - yaw_setpoint = matrix::wrap_pi(result.value + offset); - yaw_vel_setpoint = result.derivative(0); - - return true; -} - -bool calculateT(int64_t start_time, int64_t end_time, int64_t now, float &T) -{ - if (now < start_time || end_time < now) { - return false; - } - - int64_t total_duration = end_time - start_time; - int64_t elapsed_duration = now - start_time; - - T = (float) elapsed_duration / (float) total_duration; - - return true; -} - -} diff --git a/src/lib/bezier/BezierN.hpp b/src/lib/bezier/BezierN.hpp deleted file mode 100644 index 7f635e94b5..0000000000 --- a/src/lib/bezier/BezierN.hpp +++ /dev/null @@ -1,76 +0,0 @@ - - -/**************************************************************************** - * - * Copyright (C) 2020 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file BezierN.hpp - * - * @author Julian Kent - * - * N-order Bezier library designed for time-aware trajectory tracking - */ - -#pragma once -#include - -namespace bezier -{ - -/* - * Calculates the location and velocity with respect to T on a given bezier curve of any order. - * - */ -bool calculateBezierPosVel(const matrix::Vector3f *positions, int N, float t, - matrix::Vector3f &position, matrix::Vector3f &velocity); - -/* - * Calculates the position, velocity and acceleration with respect to T on a given bezier curve of any order. - * - */ -bool calculateBezierPosVelAcc(const matrix::Vector3f *positions, int N, float t, - matrix::Vector3f &position, matrix::Vector3f &velocity, matrix::Vector3f &acceleration); - -/* - * Calculates the position and velocity of yaw with respect to t on a bezier curve. - * All yaw setpoints are wrapped relative to the starting yaw. - * - */ -bool calculateBezierYaw(const float *setpoints, int N, float t, float &yaw_setpoint, float &yaw_vel_setpoint); - -/* - * Calculates the fraction between the begin and end time which can be used for fast bezier curve lookups - */ -bool calculateT(int64_t start_time, int64_t end_time, int64_t now, float &T); - -} diff --git a/src/lib/bezier/BezierNTest.cpp b/src/lib/bezier/BezierNTest.cpp deleted file mode 100644 index b08a40fd62..0000000000 --- a/src/lib/bezier/BezierNTest.cpp +++ /dev/null @@ -1,325 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * Test code for the Velocity Smoothing library - * Run this test only using make tests TESTFILTER=BezierN - * - * @author Julian Kent - */ - -#include -#include - -#include "BezierN.hpp" - -TEST(BezierN_calculateBezier, checks_validity) -{ - matrix::Vector3f points[10]; - matrix::Vector3f a, b; - EXPECT_FALSE(bezier::calculateBezierPosVel(nullptr, 10, 0.5f, a, b)); - EXPECT_FALSE(bezier::calculateBezierPosVel(points, 0, 0.5f, a, b)); - EXPECT_FALSE(bezier::calculateBezierPosVel(points, 10, -0.5f, a, b)); - EXPECT_FALSE(bezier::calculateBezierPosVel(points, 10, 1.5f, a, b)); -} - -TEST(BezierN_calculateBezier, checks_validity_accel) -{ - matrix::Vector3f points[10]; - matrix::Vector3f a, b, c; - EXPECT_FALSE(bezier::calculateBezierPosVelAcc(nullptr, 10, 0.5f, a, b, c)); - EXPECT_FALSE(bezier::calculateBezierPosVelAcc(points, 0, 0.5f, a, b, c)); - EXPECT_FALSE(bezier::calculateBezierPosVelAcc(points, 10, -0.5f, a, b, c)); - EXPECT_FALSE(bezier::calculateBezierPosVelAcc(points, 10, 1.5f, a, b, c)); -} - -TEST(BezierN_calculateBezier, work_1_point) -{ - // GIVEN: a single point bezier curve - matrix::Vector3f points[2] = {matrix::Vector3f(1, 2, 3), matrix::Vector3f(NAN, NAN, NAN)}; - matrix::Vector3f pos, vel; - pos *= NAN; - vel *= NAN; - - // WHEN: we get the half-way point - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 1, 0.5f, pos, vel)); - - // THEN: it should be the same as the point, and the velocity should be 0 - EXPECT_EQ((pos - points[0]).norm(), 0.f); - EXPECT_EQ(vel.norm(), 0.f); -} - -TEST(BezierN_calculateBezier, works_2_points) -{ - // GIVEN: a 2-point bezier curve - matrix::Vector3f points[3] = {matrix::Vector3f(1, 2, 3), matrix::Vector3f(5, 0, 1), matrix::Vector3f(NAN, NAN, NAN)}; - matrix::Vector3f pos, vel; - pos *= NAN; - vel *= NAN; - - // WHEN: we get the half-way point - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 2, 0.5f, pos, vel)); - - // THEN: the position should be the mid-point between the start and end, and velocity should be the length - EXPECT_EQ((pos - matrix::Vector3f(3, 1, 2)).norm(), 0.f); - EXPECT_FLOAT_EQ((vel - (points[1] - points[0])).norm(), 0.f); - - // WHEN: we get the beginning point - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 2, 0.f, pos, vel)); - - // THEN: the position should be the first point, and the velocity should still be the length - EXPECT_EQ((pos - points[0]).norm(), 0.f); - EXPECT_FLOAT_EQ((vel - (points[1] - points[0])).norm(), 0.f); - - // WHEN: we get the end point - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 2, 1.f, pos, vel)); - - // THEN: the position should be the first point, and the velocity should still be the length - EXPECT_EQ((pos - points[1]).norm(), 0.f); - EXPECT_FLOAT_EQ((vel - (points[1] - points[0])).norm(), 0.f); -} - -TEST(BezierN_calculateBezier, works_3_points_zero_accel) -{ - // GIVEN: 3 points bezier, evenly spaced in a straight line - matrix::Vector3f points[4] = {matrix::Vector3f(1, 2, 3), matrix::Vector3f(5, 0, 1), matrix::Vector3f(9, -2, -1), matrix::Vector3f(NAN, NAN, NAN)}; - matrix::Vector3f pos, vel; - pos *= NAN; - vel *= NAN; - - // WHEN: we get the half-way point - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 3, 0.5f, pos, vel)); - - // THEN: it should be the middle point, with velocity of 1st to last - EXPECT_FLOAT_EQ((pos - points[1]).norm(), 0.f); - EXPECT_FLOAT_EQ((vel - (points[2] - points[0])).norm(), 0.f); - - matrix::Vector3f pos2, vel2, accel2; - - // WHEN: we use the accel interface - EXPECT_TRUE(bezier::calculateBezierPosVelAcc(points, 3, 0.5f, pos2, vel2, accel2)); - - // THEN: it should give same position, velocity as the non-accel interface, and zero accel (since this curve is 0 accel) - EXPECT_FLOAT_EQ((pos2 - pos).norm(), 0.f); - EXPECT_FLOAT_EQ((vel2 - vel).norm(), 0.f); - EXPECT_FLOAT_EQ(accel2.norm(), 0.f); - - // WHEN: we check at the beginning - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 3, 0.f, pos, vel)); - EXPECT_TRUE(bezier::calculateBezierPosVelAcc(points, 3, 0.f, pos2, vel2, accel2)); - - // THEN: it should be the starting point and same velocity - EXPECT_FLOAT_EQ((pos - points[0]).norm(), 0.f); - EXPECT_FLOAT_EQ((vel - (points[2] - points[0])).norm(), 0.f); - EXPECT_FLOAT_EQ((pos2 - pos).norm(), 0.f); - EXPECT_FLOAT_EQ((vel2 - vel).norm(), 0.f); - EXPECT_FLOAT_EQ(accel2.norm(), 0.f); - - // WHEN: we check at the end - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 3, 1.f, pos, vel)); - EXPECT_TRUE(bezier::calculateBezierPosVelAcc(points, 3, 1.f, pos2, vel2, accel2)); - - // THEN: it should be the ending point and same velocity - EXPECT_FLOAT_EQ((pos - points[2]).norm(), 0.f); - EXPECT_FLOAT_EQ((vel - (points[2] - points[0])).norm(), 0.f); - EXPECT_FLOAT_EQ((pos2 - pos).norm(), 0.f); - EXPECT_FLOAT_EQ((vel2 - vel).norm(), 0.f); - EXPECT_FLOAT_EQ(accel2.norm(), 0.f); -} - -TEST(BezierN_calculateBezier, works_3_points_accel) -{ - // GIVEN: 3 points bezier, in a curve - matrix::Vector3f points[4] = {matrix::Vector3f(1, 2, 3), matrix::Vector3f(5, 0, 1), matrix::Vector3f(19, -8, 1), matrix::Vector3f(NAN, NAN, NAN)}; - matrix::Vector3f pos, vel; - pos *= NAN; - vel *= NAN; - - matrix::Vector3f pos2; - pos2 *= NAN; - - matrix::Vector3f accel_start, accel_mid, accel_end; - matrix::Vector3f vel_start, vel_mid, vel_end; - - - // WHEN: we check at the beginning - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 3, 0.f, pos, vel)); - EXPECT_TRUE(bezier::calculateBezierPosVelAcc(points, 3, 0.f, pos2, vel_start, accel_start)); - - // THEN: it should give same position, velocity as the non-accel interface, and non-zero accel - EXPECT_FLOAT_EQ((pos2 - pos).norm(), 0.f); - EXPECT_FLOAT_EQ((vel_start - vel).norm(), 0.f); - EXPECT_GT(accel_start.norm(), 0.f); - - // WHEN: we use the accel interface to get the half-way point - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 3, 0.5f, pos, vel)); - EXPECT_TRUE(bezier::calculateBezierPosVelAcc(points, 3, 0.5f, pos2, vel_mid, accel_mid)); - - // THEN: the values should matche between accel and non-accel version - EXPECT_FLOAT_EQ((pos2 - pos).norm(), 0.f); - EXPECT_FLOAT_EQ((vel_mid - vel).norm(), 0.f); - - // AND: the accel should be the same as the start - EXPECT_FLOAT_EQ((accel_mid - accel_start).norm(), 0.f); - - - // WHEN: we check at the end - EXPECT_TRUE(bezier::calculateBezierPosVel(points, 3, 1.f, pos, vel)); - EXPECT_TRUE(bezier::calculateBezierPosVelAcc(points, 3, 1.f, pos2, vel_end, accel_end)); - - // THEN: it should be the ending point, and accel should match - EXPECT_FLOAT_EQ((pos - points[2]).norm(), 0.f); - EXPECT_FLOAT_EQ((pos2 - pos).norm(), 0.f); - EXPECT_FLOAT_EQ((vel_end - vel).norm(), 0.f); - EXPECT_FLOAT_EQ((accel_end - accel_start).norm(), 0.f); - - // FINALLY: mid point velocity should be average of start and end velocity - EXPECT_FLOAT_EQ((vel_mid - 0.5f * (vel_start + vel_end)).norm(), 0.f); -} - -TEST(BezierN_calculateBezierYaw, checks_validity) -{ - float points[10]; - float a, b; - EXPECT_FALSE(bezier::calculateBezierYaw(nullptr, 10, 0.5f, a, b)); - EXPECT_FALSE(bezier::calculateBezierYaw(points, 0, 0.5f, a, b)); - EXPECT_FALSE(bezier::calculateBezierYaw(points, 10, -0.5f, a, b)); - EXPECT_FALSE(bezier::calculateBezierYaw(points, 10, 1.5f, a, b)); -} - -TEST(BezierN_calculateBezierYaw, work_1_point) -{ - // GIVEN: a single yaw point - float points[2] = {M_PI / 2, NAN}; - float yaw, yaw_speed; - - // WHEN: we use it as a 1-point bezier curve - EXPECT_TRUE(bezier::calculateBezierYaw(points, 1, 0.5f, yaw, yaw_speed)); - - // THEN: it should have that same value, and the velocity should be 0 - EXPECT_FLOAT_EQ(yaw, M_PI / 2); - EXPECT_FLOAT_EQ(yaw_speed, 0); -} - -TEST(BezierN_calculateBezierYaw, work_2_points) -{ - // GIVEN: a single yaw point - float points[3] = {0, M_PI / 2, NAN}; - float yaw, yaw_speed; - - // WHEN: we get the beginning - EXPECT_TRUE(bezier::calculateBezierYaw(points, 2, 0.f, yaw, yaw_speed)); - - // THEN: it should have the beginning value, and the velocity should be the difference between first and last - EXPECT_FLOAT_EQ(yaw, 0); - EXPECT_FLOAT_EQ(yaw_speed, M_PI / 2); - - // WHEN: we get the middle - EXPECT_TRUE(bezier::calculateBezierYaw(points, 2, 0.5f, yaw, yaw_speed)); - - // THEN: it should have the beginning value, and the velocity should be the difference between first and last - EXPECT_FLOAT_EQ(yaw, M_PI / 4); - EXPECT_FLOAT_EQ(yaw_speed, M_PI / 2); - - // WHEN: we get the end - EXPECT_TRUE(bezier::calculateBezierYaw(points, 2, 1.f, yaw, yaw_speed)); - - // THEN: it should have the beginning value, and the velocity should be the difference between first and last - EXPECT_FLOAT_EQ(yaw, M_PI / 2); - EXPECT_FLOAT_EQ(yaw_speed, M_PI / 2); -} - -TEST(BezierN_calculateBezierYaw, work_2_points_wrap) -{ - // GIVEN: 2 yaw points on either side of the +- PI wrap line - float points[3] = {-M_PI + 0.1, M_PI - 0.1, NAN}; - float yaw, yaw_speed; - - // WHEN: we get the beginning - EXPECT_TRUE(bezier::calculateBezierYaw(points, 2, 0.f, yaw, yaw_speed)); - - // THEN: it should have the beginning value, and the velocity should be the wrapped distance between first and last - EXPECT_FLOAT_EQ(yaw, -M_PI + 0.1); - EXPECT_NEAR(yaw_speed, -0.2, 1e-6f); - - // WHEN: we get the middle - EXPECT_TRUE(bezier::calculateBezierYaw(points, 2, 0.5f, yaw, yaw_speed)); - - // THEN: it should have the wrapped middle value, and the velocity should be the wrapped distance between first and last - EXPECT_FLOAT_EQ(matrix::wrap_pi(yaw - float(M_PI)), 0); - EXPECT_NEAR(yaw_speed, -0.2, 1e-6f); - - // WHEN: we get the end - EXPECT_TRUE(bezier::calculateBezierYaw(points, 2, 1.f, yaw, yaw_speed)); - - // THEN: it should have the end value, and the velocity should be the wrapped distance between first and last - EXPECT_FLOAT_EQ(yaw, M_PI - 0.1); - EXPECT_NEAR(yaw_speed, -0.2, 1e-6f); -} - - -TEST(BezierN_calculateT, rejects_bad_timestamps) -{ - float f = NAN; - EXPECT_FALSE(bezier::calculateT(100, 1000, 99, f)); - EXPECT_FALSE(bezier::calculateT(100, 1000, 1001, f)); - EXPECT_FALSE(bezier::calculateT(1001, 1000, 1001, f)); -} - - -TEST(BezierN_calculateT, begin_middle_end) -{ - float f = NAN; - EXPECT_TRUE(bezier::calculateT(100, 1000, 100, f)); - EXPECT_FLOAT_EQ(f, 0.f); - - EXPECT_TRUE(bezier::calculateT(100, 1000, 550, f)); - EXPECT_FLOAT_EQ(f, 0.5f); - - EXPECT_TRUE(bezier::calculateT(100, 1000, 1000, f)); - EXPECT_FLOAT_EQ(f, 1.f); -} - -TEST(BezierN_calculateT, giant_offset) -{ - int64_t offset = 0xFFFFFFFFFFFF; // 48 bit max - float f = NAN; - EXPECT_TRUE(bezier::calculateT(offset + 100, offset + 1000, offset + 100, f)); - EXPECT_FLOAT_EQ(f, 0.f); - - EXPECT_TRUE(bezier::calculateT(offset + 100, offset + 1000, offset + 550, f)); - EXPECT_FLOAT_EQ(f, 0.5f); - - EXPECT_TRUE(bezier::calculateT(offset + 100, offset + 1000, offset + 1000, f)); - EXPECT_FLOAT_EQ(f, 1.f); -} diff --git a/src/lib/bezier/BezierQuad.cpp b/src/lib/bezier/BezierQuad.cpp deleted file mode 100644 index 61309cc496..0000000000 --- a/src/lib/bezier/BezierQuad.cpp +++ /dev/null @@ -1,223 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -/** - * @file BezierQuad.cpp - * Bezier function - * - * @author Dennis Mannhart - */ - -// The header includes this implementation, so in order to have this library shared with other -// .cpp files and avoid duplication of constants, we need to prevent including it twice -#ifndef BEZIER_QUAD_CPP -#define BEZIER_QUAD_CPP - - -#include "BezierQuad.hpp" - -namespace bezier -{ -static constexpr double GOLDEN_RATIO = 1.6180339887; //(sqrt(5)+1)/2 -static constexpr double RESOLUTION = 0.0001; //End criterion for golden section search - -template -void BezierQuad::setBezier(const Vector3_t &pt0, const Vector3_t &ctrl, const Vector3_t &pt1, - Tp duration) -{ - _pt0 = pt0; - _ctrl = ctrl; - _pt1 = pt1; - _duration = duration; - _cached_resolution = (Tp)(-1); -} - -template -void BezierQuad::getBezier(Vector3_t &pt0, Vector3_t &ctrl, Vector3_t &pt1) -{ - pt0 = _pt0; - ctrl = _ctrl; - pt1 = _pt1; -} - -template -matrix::Vector BezierQuad::getPoint(const Tp t) -{ - return (_pt0 * ((Tp)1 - t / _duration) * ((Tp)1 - t / _duration) + _ctrl * (Tp)2 * (( - Tp)1 - t / _duration) * t / _duration + _pt1 * - t / _duration * t / _duration); -} - -template -matrix::Vector BezierQuad::getVelocity(const Tp t) -{ - return (((_ctrl - _pt0) * _duration + (_pt0 - _ctrl * (Tp)2 + _pt1) * t) * (Tp)2 / (_duration * _duration)); -} - -template -matrix::Vector BezierQuad::getAcceleration() -{ - return ((_pt0 - _ctrl * (Tp)2 + _pt1) * (Tp)2 / (_duration * _duration)); -} - -template -void BezierQuad::getStates(Vector3_t &point, Vector3_t &vel, Vector3_t &acc, const Tp time) -{ - point = getPoint(time); - vel = getVelocity(time); - acc = getAcceleration(); -} - -template -void BezierQuad::getStatesClosest(Vector3_t &point, Vector3_t &vel, Vector3_t &acc, - const Vector3_t pose) -{ - // get t that corresponds to point closest on bezier point - Tp t = _goldenSectionSearch(pose); - - // get states corresponding to t - getStates(point, vel, acc, t); - -} - -template -void BezierQuad::setBezFromVel(const Vector3_t &ctrl, const Vector3_t &vel0, const Vector3_t &vel1, - const Tp duration) -{ - // update bezier points - _ctrl = ctrl; - _duration = duration; - _pt0 = _ctrl - vel0 * _duration / (Tp)2; - _pt1 = _ctrl + vel1 * _duration / (Tp)2; - _cached_resolution = (Tp)(-1); -} - -template -Tp BezierQuad::getArcLength(const Tp resolution) -{ - // we don't need to recompute arc length if: - // 1. _cached_resolution is up to date; 2. _cached_resolution is smaller than desired resolution (= more accurate) - if ((_cached_resolution > (Tp)0) && (_cached_resolution <= resolution)) { - return _cached_arc_length; - } - - // get number of elements - int n = (int)(roundf(_duration / resolution)); - - // check if n is even - if (n % 2 == 1) { - n += 1; - } - - // step size - Tp h = (_duration) / n; - - // get integration - _cached_arc_length = (Tp)0; - Vector3_t y; - - for (int i = 1; i < n; i++) { - - y = getVelocity(h * i); - - if (i % 2 == 1) { - _cached_arc_length += (Tp)4 * y.length(); - - } else { - _cached_arc_length += (Tp)2 * y.length(); - } - } - - // velocity length at start and end points - Tp y0 = getVelocity((Tp)0).length(); - Tp yn = getVelocity(_duration).length(); - - // 1/3 simpsons rule - _cached_arc_length = h / (Tp)3 * (y0 + yn + _cached_arc_length); - - // update cache - _cached_resolution = resolution; - - return _cached_arc_length; -} - -template -Tp BezierQuad::getDistToClosestPoint(const Vector3_t &pose) -{ - // get t that corresponds to point closest on bezier point - Tp t = _goldenSectionSearch(pose); - - // get closest point - Vector3_t point = getPoint(t); - return (pose - point).length(); -} - -template -Tp BezierQuad::_goldenSectionSearch(const Vector3_t &pose) -{ - Tp a, b, c, d; - a = (Tp)0; // represents most left point - b = _duration * (Tp)1; // represents most right point - - c = b - (b - a) / GOLDEN_RATIO; - d = a + (b - a) / GOLDEN_RATIO; - - while (fabsf(c - d) > RESOLUTION) { - if (_getDistanceSquared(c, pose) < _getDistanceSquared(d, pose)) { - b = d; - - } else { - a = c; - } - - c = b - (b - a) / GOLDEN_RATIO; - d = a + (b - a) / GOLDEN_RATIO; - } - - return (b + a) / (Tp)2; -} - -template -Tp BezierQuad::_getDistanceSquared(const Tp t, const Vector3_t &pose) -{ - // get point on bezier - Vector3_t vec = getPoint(t); - - // get vector from point to pose - vec = vec - pose; - - // norm squared - return (vec * vec); -} -} - -#endif diff --git a/src/lib/bezier/BezierQuad.hpp b/src/lib/bezier/BezierQuad.hpp deleted file mode 100644 index 5aebe94b7b..0000000000 --- a/src/lib/bezier/BezierQuad.hpp +++ /dev/null @@ -1,210 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2018 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file BezierQuad.hpp - * - * Quadratic bezier lib - * - * A quadratic bezier function/spline is completely defined by three 3D points in space and a time scaling factor. - * pt0 and pt1 define the start and end points of the spline. ctrl point is a point in space that effects the curvature - * of the spline. The time scaling factor (= duration) defines the time it takes to travel along the spline from pt0 to - * pt1. - * A bezier spline is a continuous function from which position, velocity and acceleration can be extracted. For a given spline, - * acceleration stays constant. - */ - - -#pragma once - -#include - -namespace bezier -{ -template -class BezierQuad -{ -public: - - using Vector3_t = matrix::Vector; - - /** - * Empty constructor - */ - BezierQuad() : - _pt0(Vector3_t()), _ctrl(Vector3_t()), _pt1(Vector3_t()), _duration(1.0f) {} - - /** - * Constructor from array - */ - BezierQuad(const Tp pt0[3], const Tp ctrl[3], const Tp pt1[3], Tp duration = 1.0f) : - _pt0(Vector3_t(pt0)), _ctrl(Vector3_t(ctrl)), _pt1(Vector3_t(pt1)), _duration(duration) {} - - /** - * Constructor from vector - */ - BezierQuad(const Vector3_t &pt0, const Vector3_t &ctrl, const Vector3_t &pt1, - Tp duration = 1.0f): - _pt0(pt0), _ctrl(ctrl), _pt1(pt1), _duration(duration) {} - - - /* - * Get bezier points - */ - void getBezier(Vector3_t &pt0, Vector3_t &ctrl, Vector3_t &pt1); - - /* - * Return pt0 - */ - Vector3_t getPt0() {return _pt0;} - - /* - * Return ctrl - */ - Vector3_t getCtrl() {return _ctrl;} - - /* - * Return pt1 - */ - Vector3_t getPt1() {return _pt1;} - - /** - * Set new bezier points and duration - */ - void setBezier(const Vector3_t &pt0, const Vector3_t &ctrl, const Vector3_t &pt1, - Tp duration = (Tp)1); - - /* - * Set duration - * - * @param time is the total time it takes to travel along the bezier spline. - */ - void setDuration(const Tp time) {_duration = time;} - - /** - * Return point on bezier point corresponding to time t - * - * @param t is a time in seconds in between [0, duration] - * @return a point on bezier - */ - Vector3_t getPoint(const Tp t); - - /* - * Distance to closest point given a position - * - * @param pose is a position in 3D space from which distance to bezier is computed. - * @return distance to closest point on bezier - */ - Tp getDistToClosestPoint(const Vector3_t &pose); - - /* - * Return velocity on bezier corresponding to time t - * - * @param t is a time in seconds in between [0, duration] - * @return velocity vector at time t - */ - Vector3_t getVelocity(const Tp t); - - /* - * Return acceleration on bezier corresponding to time t - * - * @return constant acceleration of bezier - */ - Vector3_t getAcceleration(); - - /* - * Get all states on bezier corresponding to time t - */ - void getStates(Vector3_t &point, Vector3_t &vel, Vector3_t &acc, const Tp t); - - /* - * Get states on bezier which are closest to pose in space - * - * @param point is a posiiton on the spline that is closest to a given pose - * @param vel is the velocity at that given point - * @param acc is the acceleration for that spline - * @param pose represent a position in space from which closest point is computed - */ - void getStatesClosest(Vector3_t &point, Vector3_t &vel, Vector3_t &acc, - const Vector3_t pose); - - /* - * Compute bezier from velocity at bezier end points and ctrl point - * - * The bezier end points are fully defined by a given control point ctrl, the duration and - * the desired velocity vectors at the end points. - */ - void setBezFromVel(const Vector3_t &ctrl, const Vector3_t &vel0, const Vector3_t &vel1, - const Tp duration = (Tp)1); - - /* - * Return the arc length of a bezier spline - * - * The arc length is computed with simpsons integration. - * @param resolution in meters. - */ - Tp getArcLength(const Tp resolution); - -private: - - Vector3_t _pt0; /**< Bezier starting point */ - Vector3_t _ctrl; /**< Bezier control point */ - Vector3_t _pt1; /**< bezier end point */ - Tp _duration = (Tp)1; /**< Total time to travle along spline */ - - Tp _cached_arc_length = (Tp)0; /**< The saved arc length of the spline */ - Tp _cached_resolution = (Tp)(-1); /**< The resolution used to compute the arc length. - Negative number means that cache is not up to date. */ - - /* - * Golden section search - */ - Tp _goldenSectionSearch(const Vector3_t &pose); - - /* - * Get squared distance from 3D pose in space and a point on bezier. - * - * @param t is the time in between [0, duration] that defines a point on the bezier. - * @param pose is a 3D pose in space. - */ - Tp _getDistanceSquared(const Tp t, const Vector3_t &pose); - - -}; - -using BezierQuad_f = BezierQuad; -using BezierQuad_d = BezierQuad; -} - -// include implementation -#include "BezierQuad.cpp" diff --git a/src/lib/bezier/CMakeLists.txt b/src/lib/bezier/CMakeLists.txt deleted file mode 100644 index f069fff591..0000000000 --- a/src/lib/bezier/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -############################################################################ -# -# Copyright (c) 2018 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -px4_add_library(bezier - BezierQuad.cpp - BezierN.cpp -) - -px4_add_unit_gtest(SRC BezierNTest.cpp LINKLIBS bezier) diff --git a/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt index 911da11d26..6c32116237 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt +++ b/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt @@ -38,7 +38,7 @@ px4_add_library(FlightTaskUtility StickYaw.cpp ) -target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis bezier SlewRate motion_planning mathlib) +target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis SlewRate motion_planning mathlib) target_include_directories(FlightTaskUtility PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) px4_add_functional_gtest(SRC StickTiltXYTest.cpp LINKLIBS FlightTaskUtility) From b916a96e005227458c4f3ae1e02fcd62a873ecfe Mon Sep 17 00:00:00 2001 From: Silvan Date: Fri, 10 Jan 2025 13:11:23 +0100 Subject: [PATCH 25/91] Remove uorb topics exclusively used for avoidance - TrajectoryBezier.msg - TrajectoryWaypoint.msg - VehicleTrajectoryBezier.msg - VehicleTrajectoryWaypoint.msg Additionally remove TRAJECTORY_REPRESENTATION_WAYPOINTS mavlink stream. Signed-off-by: Silvan --- msg/CMakeLists.txt | 4 - msg/TrajectoryBezier.msg | 8 -- msg/TrajectoryWaypoint.msg | 13 -- msg/VehicleTrajectoryBezier.msg | 18 --- msg/VehicleTrajectoryWaypoint.msg | 21 --- src/modules/logger/logged_topics.cpp | 2 - src/modules/mavlink/mavlink_main.cpp | 3 - src/modules/mavlink/mavlink_messages.cpp | 4 - src/modules/mavlink/mavlink_receiver.cpp | 66 ---------- src/modules/mavlink/mavlink_receiver.h | 6 - .../TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp | 124 ------------------ src/modules/uxrce_dds_client/dds_topics.yaml | 9 -- src/modules/zenoh/Kconfig | 3 - src/modules/zenoh/Kconfig.topics | 15 --- 14 files changed, 296 deletions(-) delete mode 100644 msg/TrajectoryBezier.msg delete mode 100644 msg/TrajectoryWaypoint.msg delete mode 100644 msg/VehicleTrajectoryBezier.msg delete mode 100644 msg/VehicleTrajectoryWaypoint.msg delete mode 100644 src/modules/mavlink/streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 51c9325413..af7a98cf54 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -214,8 +214,6 @@ set(msg_files TelemetryStatus.msg TiltrotorExtraControls.msg TimesyncStatus.msg - TrajectoryBezier.msg - TrajectoryWaypoint.msg TransponderReport.msg TuneControl.msg UavcanParameterRequest.msg @@ -235,8 +233,6 @@ set(msg_files VehicleRoi.msg VehicleThrustSetpoint.msg VehicleTorqueSetpoint.msg - VehicleTrajectoryBezier.msg - VehicleTrajectoryWaypoint.msg VelocityLimits.msg WheelEncoders.msg Wind.msg diff --git a/msg/TrajectoryBezier.msg b/msg/TrajectoryBezier.msg deleted file mode 100644 index e3d9d4e0ff..0000000000 --- a/msg/TrajectoryBezier.msg +++ /dev/null @@ -1,8 +0,0 @@ -# Bezier Trajectory description. See also Mavlink TRAJECTORY msg -# The topic trajectory_bezier describe each waypoint defined in vehicle_trajectory_bezier - -uint64 timestamp # time since system start (microseconds) - -float32[3] position # local position x,y,z (metres) -float32 yaw # yaw angle (rad) -float32 delta # time it should take to get to this waypoint, if this is the final waypoint (seconds) diff --git a/msg/TrajectoryWaypoint.msg b/msg/TrajectoryWaypoint.msg deleted file mode 100644 index 6ea9bae4d3..0000000000 --- a/msg/TrajectoryWaypoint.msg +++ /dev/null @@ -1,13 +0,0 @@ -# Waypoint Trajectory description. See also Mavlink TRAJECTORY msg -# The topic trajectory_waypoint describe each waypoint defined in vehicle_trajectory_waypoint - -uint64 timestamp # time since system start (microseconds) - -float32[3] position -float32[3] velocity -float32[3] acceleration -float32 yaw -float32 yaw_speed - -bool point_valid -uint8 type diff --git a/msg/VehicleTrajectoryBezier.msg b/msg/VehicleTrajectoryBezier.msg deleted file mode 100644 index d4bf99b469..0000000000 --- a/msg/VehicleTrajectoryBezier.msg +++ /dev/null @@ -1,18 +0,0 @@ -# Vehicle Waypoints Trajectory description. See also MAVLink MAV_TRAJECTORY_REPRESENTATION msg -# The topic vehicle_trajectory_bezier is used to send a smooth flight path from the -# companion computer / avoidance module to the position controller. - -uint64 timestamp # time since system start (microseconds) - -uint8 POINT_0 = 0 -uint8 POINT_1 = 1 -uint8 POINT_2 = 2 -uint8 POINT_3 = 3 -uint8 POINT_4 = 4 - -uint8 NUMBER_POINTS = 5 - -TrajectoryBezier[5] control_points -uint8 bezier_order - -# TOPICS vehicle_trajectory_bezier diff --git a/msg/VehicleTrajectoryWaypoint.msg b/msg/VehicleTrajectoryWaypoint.msg deleted file mode 100644 index 6bff1cec84..0000000000 --- a/msg/VehicleTrajectoryWaypoint.msg +++ /dev/null @@ -1,21 +0,0 @@ -# Vehicle Waypoints Trajectory description. See also MAVLink MAV_TRAJECTORY_REPRESENTATION msg -# The topic vehicle_trajectory_waypoint_desired is used to send the user desired waypoints from the position controller to the companion computer / avoidance module. -# The topic vehicle_trajectory_waypoint is used to send the adjusted waypoints from the companion computer / avoidance module to the position controller. - -uint64 timestamp # time since system start (microseconds) - -uint8 MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS = 0 - -uint8 type # Type from MAV_TRAJECTORY_REPRESENTATION enum. - -uint8 POINT_0 = 0 -uint8 POINT_1 = 1 -uint8 POINT_2 = 2 -uint8 POINT_3 = 3 -uint8 POINT_4 = 4 - -uint8 NUMBER_POINTS = 5 - -TrajectoryWaypoint[5] waypoints - -# TOPICS vehicle_trajectory_waypoint vehicle_trajectory_waypoint_desired diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 57b2a429de..3114d83bb1 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -335,8 +335,6 @@ void LoggedTopics::add_vision_and_avoidance_topics() add_topic("obstacle_distance_fused"); add_topic("obstacle_distance"); add_topic("vehicle_mocap_odometry", 30); - add_topic("vehicle_trajectory_waypoint", 200); - add_topic("vehicle_trajectory_waypoint_desired", 200); add_topic("vehicle_visual_odometry", 30); } diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 6e81849600..724391cf86 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1525,7 +1525,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("SYS_STATUS", 5.0f); configure_stream_local("SYSTEM_TIME", 1.0f); configure_stream_local("TIME_ESTIMATE_TO_TARGET", 1.0f); - configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f); configure_stream_local("VFR_HUD", 10.0f); configure_stream_local("VIBRATION", 0.5f); configure_stream_local("WIND_COV", 10.0f); @@ -1592,7 +1591,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("RC_CHANNELS", 5.0f); configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f); configure_stream_local("SYS_STATUS", 5.0f); - configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f); configure_stream_local("VFR_HUD", 4.0f); configure_stream_local("VIBRATION", 0.5f); configure_stream_local("WIND_COV", 1.0f); @@ -1784,7 +1782,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("SYS_STATUS", 5.0f); configure_stream_local("SYSTEM_TIME", 2.0f); configure_stream_local("TIME_ESTIMATE_TO_TARGET", 1.0f); - configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f); configure_stream_local("VFR_HUD", 4.0f); configure_stream_local("VIBRATION", 0.5f); configure_stream_local("WIND_COV", 1.0f); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 72b5b49123..7d5e23bb6e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -116,7 +116,6 @@ #include "streams/SYSTEM_TIME.hpp" #include "streams/TIME_ESTIMATE_TO_TARGET.hpp" #include "streams/TIMESYNC.hpp" -#include "streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp" #include "streams/VFR_HUD.hpp" #include "streams/VIBRATION.hpp" #include "streams/WIND_COV.hpp" @@ -382,9 +381,6 @@ static const StreamListItem streams_list[] = { #if defined(MANUAL_CONTROL_HPP) create_stream_list_item(), #endif // MANUAL_CONTROL_HPP -#if defined(TRAJECTORY_REPRESENTATION_WAYPOINTS_HPP) - create_stream_list_item(), -#endif // TRAJECTORY_REPRESENTATION_WAYPOINTS_HPP #if defined(OPTICAL_FLOW_RAD_HPP) create_stream_list_item(), #endif // OPTICAL_FLOW_RAD_HPP diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 00c0b9ec93..6afab95313 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -256,14 +256,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_tunnel(msg); break; - case MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER: - handle_message_trajectory_representation_bezier(msg); - break; - - case MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS: - handle_message_trajectory_representation_waypoints(msg); - break; - case MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS: handle_message_onboard_computer_status(msg); break; @@ -1988,64 +1980,6 @@ MavlinkReceiver::handle_message_tunnel(mavlink_message_t *msg) } -void -MavlinkReceiver::handle_message_trajectory_representation_bezier(mavlink_message_t *msg) -{ - mavlink_trajectory_representation_bezier_t trajectory; - mavlink_msg_trajectory_representation_bezier_decode(msg, &trajectory); - - vehicle_trajectory_bezier_s trajectory_bezier{}; - - trajectory_bezier.timestamp = _mavlink_timesync.sync_stamp(trajectory.time_usec); - - for (int i = 0; i < vehicle_trajectory_bezier_s::NUMBER_POINTS; ++i) { - trajectory_bezier.control_points[i].position[0] = trajectory.pos_x[i]; - trajectory_bezier.control_points[i].position[1] = trajectory.pos_y[i]; - trajectory_bezier.control_points[i].position[2] = trajectory.pos_z[i]; - - trajectory_bezier.control_points[i].delta = trajectory.delta[i]; - trajectory_bezier.control_points[i].yaw = trajectory.pos_yaw[i]; - } - - trajectory_bezier.bezier_order = math::min(trajectory.valid_points, vehicle_trajectory_bezier_s::NUMBER_POINTS); - _trajectory_bezier_pub.publish(trajectory_bezier); -} - -void -MavlinkReceiver::handle_message_trajectory_representation_waypoints(mavlink_message_t *msg) -{ - mavlink_trajectory_representation_waypoints_t trajectory; - mavlink_msg_trajectory_representation_waypoints_decode(msg, &trajectory); - - vehicle_trajectory_waypoint_s trajectory_waypoint{}; - - const int number_valid_points = math::min(trajectory.valid_points, vehicle_trajectory_waypoint_s::NUMBER_POINTS); - - for (int i = 0; i < number_valid_points; ++i) { - trajectory_waypoint.waypoints[i].position[0] = trajectory.pos_x[i]; - trajectory_waypoint.waypoints[i].position[1] = trajectory.pos_y[i]; - trajectory_waypoint.waypoints[i].position[2] = trajectory.pos_z[i]; - - trajectory_waypoint.waypoints[i].velocity[0] = trajectory.vel_x[i]; - trajectory_waypoint.waypoints[i].velocity[1] = trajectory.vel_y[i]; - trajectory_waypoint.waypoints[i].velocity[2] = trajectory.vel_z[i]; - - trajectory_waypoint.waypoints[i].acceleration[0] = trajectory.acc_x[i]; - trajectory_waypoint.waypoints[i].acceleration[1] = trajectory.acc_y[i]; - trajectory_waypoint.waypoints[i].acceleration[2] = trajectory.acc_z[i]; - - trajectory_waypoint.waypoints[i].yaw = trajectory.pos_yaw[i]; - trajectory_waypoint.waypoints[i].yaw_speed = trajectory.vel_yaw[i]; - - trajectory_waypoint.waypoints[i].point_valid = true; - - trajectory_waypoint.waypoints[i].type = UINT8_MAX; - } - - trajectory_waypoint.timestamp = hrt_absolute_time(); - _trajectory_waypoint_pub.publish(trajectory_waypoint); -} - void MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 326d0ee19e..84ad2e7c62 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -110,8 +110,6 @@ #include #include #include -#include -#include #include #if !defined(CONSTRAINED_FLASH) @@ -201,8 +199,6 @@ private: void handle_message_set_position_target_local_ned(mavlink_message_t *msg); void handle_message_statustext(mavlink_message_t *msg); void handle_message_tunnel(mavlink_message_t *msg); - void handle_message_trajectory_representation_bezier(mavlink_message_t *msg); - void handle_message_trajectory_representation_waypoints(mavlink_message_t *msg); void handle_message_utm_global_position(mavlink_message_t *msg); #if defined(MAVLINK_MSG_ID_SET_VELOCITY_LIMITS) // For now only defined if development.xml is used void handle_message_set_velocity_limits(mavlink_message_t *msg); @@ -329,8 +325,6 @@ private: uORB::Publication _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)}; uORB::Publication _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; uORB::Publication _rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; - uORB::Publication _trajectory_bezier_pub{ORB_ID(vehicle_trajectory_bezier)}; - uORB::Publication _trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)}; #if !defined(CONSTRAINED_FLASH) uORB::Publication _debug_array_pub {ORB_ID(debug_array)}; diff --git a/src/modules/mavlink/streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp b/src/modules/mavlink/streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp deleted file mode 100644 index 174c633fd2..0000000000 --- a/src/modules/mavlink/streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp +++ /dev/null @@ -1,124 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#ifndef TRAJECTORY_REPRESENTATION_WAYPOINTS_HPP -#define TRAJECTORY_REPRESENTATION_WAYPOINTS_HPP - -#include - -class MavlinkStreamTrajectoryRepresentationWaypoints: public MavlinkStream -{ -public: - static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamTrajectoryRepresentationWaypoints(mavlink); } - - const char *get_name() const override { return get_name_static(); } - uint16_t get_id() override { return get_id_static(); } - - static constexpr const char *get_name_static() { return "TRAJECTORY_REPRESENTATION_WAYPOINTS"; } - static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS; } - - unsigned get_size() override - { - if (_traj_wp_avoidance_sub.advertised()) { - return MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; - } - - return 0; - } - -private: - explicit MavlinkStreamTrajectoryRepresentationWaypoints(Mavlink *mavlink) : MavlinkStream(mavlink) {} - - uORB::Subscription _traj_wp_avoidance_sub{ORB_ID(vehicle_trajectory_waypoint_desired)}; - - bool send() override - { - vehicle_trajectory_waypoint_s traj_wp_avoidance_desired; - - if (_traj_wp_avoidance_sub.update(&traj_wp_avoidance_desired)) { - mavlink_trajectory_representation_waypoints_t msg{}; - - msg.time_usec = traj_wp_avoidance_desired.timestamp; - int number_valid_points = 0; - - for (int i = 0; i < vehicle_trajectory_waypoint_s::NUMBER_POINTS; ++i) { - msg.pos_x[i] = traj_wp_avoidance_desired.waypoints[i].position[0]; - msg.pos_y[i] = traj_wp_avoidance_desired.waypoints[i].position[1]; - msg.pos_z[i] = traj_wp_avoidance_desired.waypoints[i].position[2]; - - msg.vel_x[i] = traj_wp_avoidance_desired.waypoints[i].velocity[0]; - msg.vel_y[i] = traj_wp_avoidance_desired.waypoints[i].velocity[1]; - msg.vel_z[i] = traj_wp_avoidance_desired.waypoints[i].velocity[2]; - - msg.acc_x[i] = traj_wp_avoidance_desired.waypoints[i].acceleration[0]; - msg.acc_y[i] = traj_wp_avoidance_desired.waypoints[i].acceleration[1]; - msg.acc_z[i] = traj_wp_avoidance_desired.waypoints[i].acceleration[2]; - - msg.pos_yaw[i] = traj_wp_avoidance_desired.waypoints[i].yaw; - msg.vel_yaw[i] = traj_wp_avoidance_desired.waypoints[i].yaw_speed; - - switch (traj_wp_avoidance_desired.waypoints[i].type) { - case position_setpoint_s::SETPOINT_TYPE_TAKEOFF: - msg.command[i] = vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF; - break; - - case position_setpoint_s::SETPOINT_TYPE_LOITER: - msg.command[i] = vehicle_command_s::VEHICLE_CMD_NAV_LOITER_UNLIM; - break; - - case position_setpoint_s::SETPOINT_TYPE_LAND: - msg.command[i] = vehicle_command_s::VEHICLE_CMD_NAV_LAND; - break; - - default: - msg.command[i] = UINT16_MAX; - } - - if (traj_wp_avoidance_desired.waypoints[i].point_valid) { - number_valid_points++; - } - - } - - msg.valid_points = number_valid_points; - - mavlink_msg_trajectory_representation_waypoints_send_struct(_mavlink->get_channel(), &msg); - - return true; - } - - return false; - } -}; - -#endif // TRAJECTORY_REPRESENTATION_WAYPOINTS_HPP diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml index 211e75fd05..bbd0d8c631 100644 --- a/src/modules/uxrce_dds_client/dds_topics.yaml +++ b/src/modules/uxrce_dds_client/dds_topics.yaml @@ -71,9 +71,6 @@ publications: - topic: /fmu/out/vehicle_status type: px4_msgs::msg::VehicleStatus - - topic: /fmu/out/vehicle_trajectory_waypoint_desired - type: px4_msgs::msg::VehicleTrajectoryWaypoint - - topic: /fmu/out/airspeed_validated type: px4_msgs::msg::AirspeedValidated @@ -145,12 +142,6 @@ subscriptions: - topic: /fmu/in/vehicle_command_mode_executor type: px4_msgs::msg::VehicleCommand - - topic: /fmu/in/vehicle_trajectory_bezier - type: px4_msgs::msg::VehicleTrajectoryBezier - - - topic: /fmu/in/vehicle_trajectory_waypoint - type: px4_msgs::msg::VehicleTrajectoryWaypoint - - topic: /fmu/in/vehicle_thrust_setpoint type: px4_msgs::msg::VehicleThrustSetpoint diff --git a/src/modules/zenoh/Kconfig b/src/modules/zenoh/Kconfig index c3c042df13..3cce22f026 100644 --- a/src/modules/zenoh/Kconfig +++ b/src/modules/zenoh/Kconfig @@ -46,9 +46,6 @@ if MODULES_ZENOH select ZENOH_PUBSUB_VEHICLE_ODOMETRY select ZENOH_PUBSUB_VEHICLE_RATES_SETPOINT select ZENOH_PUBSUB_VEHICLE_COMMAND - select ZENOH_PUBSUB_VEHICLE_TRAJECTORY_BEZIER - select ZENOH_PUBSUB_VEHICLE_TRAJECTORY_WAYPOINT - config ZENOH_PUBSUB_ALL bool "All" diff --git a/src/modules/zenoh/Kconfig.topics b/src/modules/zenoh/Kconfig.topics index 585831cd2f..ac96b8586f 100644 --- a/src/modules/zenoh/Kconfig.topics +++ b/src/modules/zenoh/Kconfig.topics @@ -741,10 +741,6 @@ menu "Zenoh publishers/subscribers" bool "timesync_status" default n - config ZENOH_PUBSUB_TRAJECTORY_BEZIER - bool "trajectory_bezier" - default n - config ZENOH_PUBSUB_TRAJECTORY_SETPOINT bool "trajectory_setpoint" default n @@ -881,14 +877,6 @@ menu "Zenoh publishers/subscribers" bool "vehicle_torque_setpoint" default n - config ZENOH_PUBSUB_VEHICLE_TRAJECTORY_BEZIER - bool "vehicle_trajectory_bezier" - default n - - config ZENOH_PUBSUB_VEHICLE_TRAJECTORY_WAYPOINT - bool "vehicle_trajectory_waypoint" - default n - config ZENOH_PUBSUB_VELOCITY_LIMITS bool "velocity_limits" default n @@ -1099,7 +1087,6 @@ config ZENOH_PUBSUB_ALL_SELECTION select ZENOH_PUBSUB_TELEMETRY_STATUS select ZENOH_PUBSUB_TILTROTOR_EXTRA_CONTROLS select ZENOH_PUBSUB_TIMESYNC_STATUS - select ZENOH_PUBSUB_TRAJECTORY_BEZIER select ZENOH_PUBSUB_TRAJECTORY_SETPOINT select ZENOH_PUBSUB_TRAJECTORY_WAYPOINT select ZENOH_PUBSUB_TRANSPONDER_REPORT @@ -1134,8 +1121,6 @@ config ZENOH_PUBSUB_ALL_SELECTION select ZENOH_PUBSUB_VEHICLE_STATUS select ZENOH_PUBSUB_VEHICLE_THRUST_SETPOINT select ZENOH_PUBSUB_VEHICLE_TORQUE_SETPOINT - select ZENOH_PUBSUB_VEHICLE_TRAJECTORY_BEZIER - select ZENOH_PUBSUB_VEHICLE_TRAJECTORY_WAYPOINT select ZENOH_PUBSUB_VELOCITY_LIMITS select ZENOH_PUBSUB_VTOL_VEHICLE_STATUS select ZENOH_PUBSUB_WHEEL_ENCODERS From a1ff1d83725e483e0cb98dc0765e15bcef2b34e7 Mon Sep 17 00:00:00 2001 From: Silvan Date: Fri, 10 Jan 2025 13:21:26 +0100 Subject: [PATCH 26/91] tests: remove test_bezier Signed-off-by: Silvan --- Tools/HIL/run_tests.py | 3 - src/systemcmds/tests/CMakeLists.txt | 1 - src/systemcmds/tests/test_bezierQuad.cpp | 278 ----------------------- src/systemcmds/tests/tests_main.c | 1 - src/systemcmds/tests/tests_main.h | 1 - 5 files changed, 284 deletions(-) delete mode 100644 src/systemcmds/tests/test_bezierQuad.cpp diff --git a/Tools/HIL/run_tests.py b/Tools/HIL/run_tests.py index 3e79c70227..0d9c5f5d1b 100755 --- a/Tools/HIL/run_tests.py +++ b/Tools/HIL/run_tests.py @@ -136,9 +136,6 @@ class TestHardwareMethods(unittest.TestCase): def test_atomic_bitset(self): self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "atomic_bitset")) - def test_bezier(self): - self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "bezier")) - def test_bitset(self): self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "bitset")) diff --git a/src/systemcmds/tests/CMakeLists.txt b/src/systemcmds/tests/CMakeLists.txt index 72d99c6708..2beab87943 100644 --- a/src/systemcmds/tests/CMakeLists.txt +++ b/src/systemcmds/tests/CMakeLists.txt @@ -33,7 +33,6 @@ set(srcs test_atomic_bitset.cpp - test_bezierQuad.cpp test_bitset.cpp test_bson.cpp test_dataman.cpp diff --git a/src/systemcmds/tests/test_bezierQuad.cpp b/src/systemcmds/tests/test_bezierQuad.cpp deleted file mode 100644 index 9fc5f7f987..0000000000 --- a/src/systemcmds/tests/test_bezierQuad.cpp +++ /dev/null @@ -1,278 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2018-2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file test_bezierQuad.cpp - * Test for Bezier curve computation. - */ - -#include -#include -#include -#include - -#include "../../lib/bezier/BezierQuad.hpp" - -class BezierQuadTest : public UnitTest -{ -public: - virtual bool run_tests(); - -private: - - bool _get_states_from_time(); - bool _get_arc_length(); - bool _set_bez_from_vel(); - - float random(float min, float max); - -}; - - -bool BezierQuadTest::run_tests() -{ - ut_run_test(_get_states_from_time); - ut_run_test(_get_arc_length); - ut_run_test(_set_bez_from_vel); - - return (_tests_failed == 0); -} - -bool BezierQuadTest::_get_states_from_time() -{ - // symmetric around 0 - matrix::Vector3f pt0(-0.5f, 0.0f, 0.0f); - matrix::Vector3f ctrl(0.0f, 0.5f, 0.0f); - matrix::Vector3f pt1(0.5f, 0.0f, 0.0f); - - // create bezier with default t = [0,1] - bezier::BezierQuad_f bz(pt0, ctrl, pt1); - - matrix::Vector3f pos, vel, acc; - float precision = 0.00001; - - // states at time = 0 - bz.getStates(pos, vel, acc, 0.0f); - - ut_compare_float("pos[0] not equal pt0[0]", pos(0), pt0(0), precision); - ut_compare_float("pos[1] not equal pt0[1]", pos(1), pt0(1), precision); - ut_compare_float("pos[2] not equal pt0[2]", pos(2), pt0(2), precision); - - ut_compare_float("slope not equal 1", vel(0), 1.0f, precision); - ut_compare_float("slope not equal 1", vel(1), 1.0f, precision); - ut_compare_float("slope not equal 0", vel(2), 0.0f, precision); - - ut_compare_float("acc not equal 0", acc(0), 0.0f, precision); - ut_compare_float("acc not equal 1", acc(1), -2.0f, precision); - ut_compare_float("acc not equal 0", acc(2), 0.0f, precision); - - // states at time = 1 - bz.getStates(pos, vel, acc, 1.0f); - - ut_compare_float("pos[0] not equal pt1[0]", pos(0), pt1(0), precision); - ut_compare_float("pos[1] not equal pt1[1]", pos(1), pt1(1), precision); - ut_compare_float("pos[2] not equal pt1[2]", pos(2), pt1(2), precision); - - ut_compare_float("slope not equal 1", vel(0), 1.0f, precision); - ut_compare_float("slope not equal -1", vel(1), -1.0f, precision); - ut_compare_float("slope not equal 0", vel(2), 0.0f, precision); - - ut_compare_float("acc not equal 0", acc(0), 0.0f, precision); - ut_compare_float("acc not equal 1", acc(1), -2.0f, precision); - ut_compare_float("acc not equal 0", acc(2), 0.0f, precision); - - // states at time = 0.5 - bz.getStates(pos, vel, acc, 0.50f); - - // pos must be equal to ctrl(0) and lower than ctrl(1) - ut_compare_float("pos[0] not equal ctrl[0]", pos(0), ctrl(0), precision); - ut_assert_true(pos(1) < ctrl(1)); - - ut_compare_float("slope not equal 1", vel(0), 1.0f, precision); - ut_compare_float("slope not equal -1", vel(1), 0.0f, precision); - ut_compare_float("slope not equal 0", vel(2), 0.0f, precision); - - ut_compare_float("acc not equal 0", acc(0), 0.0f, precision); - ut_compare_float("acc not equal -2", acc(1), -2.0f, precision); - ut_compare_float("acc not equal 0", acc(2), 0.0f, precision); - - // acceleration - pt0 = matrix::Vector3f(0.0f, 0.0f, 0.0f); - ctrl = matrix::Vector3f(0.0f, 0.0f, 0.0f); - pt1 = matrix::Vector3f(1.0f, 0.0f, 0.0f); - - // create bezier with default t = [0,1] - bz.setBezier(pt0, ctrl, pt1, 1.0f); - - // states at time = 0.0 - bz.getStates(pos, vel, acc, 0.0f); - - ut_compare_float("pos[0] not equal pt0[0]", pos(0), pt0(0), precision); - ut_compare_float("pos[1] not equal pt0[1]", pos(1), pt0(1), precision); - ut_compare_float("pos[2] not equal pt0[2]", pos(2), pt0(2), precision); - - ut_compare_float("slope not equal 0", vel(0), 0.0f, precision); - ut_compare_float("slope not equal 0", vel(1), 0.0f, precision); - ut_compare_float("slope not equal 0", vel(2), 0.0f, precision); - - ut_compare_float("acc not equal 2", acc(0), 2.0f, precision); - ut_compare_float("acc not equal 0", acc(1), 0.0f, precision); - ut_compare_float("acc not equal 0", acc(2), 0.0f, precision); - - // states at time = 1.0 - bz.getStates(pos, vel, acc, 1.0f); - - ut_compare_float("pos[0] not equal pt1[0]", pos(0), pt1(0), precision); - ut_compare_float("pos[1] not equal pt1[1]", pos(1), pt1(1), precision); - ut_compare_float("pos[2] not equal pt1[2]", pos(2), pt1(2), precision); - - ut_compare_float("slope not equal 2", vel(0), 2.0f, precision); - ut_compare_float("slope not equal 0", vel(1), 0.0f, precision); - ut_compare_float("slope not equal 0", vel(2), 0.0f, precision); - - ut_compare_float("acc not equal 2", acc(0), 2.0f, precision); - ut_compare_float("acc not equal 0", acc(1), 0.0f, precision); - ut_compare_float("acc not equal 0", acc(2), 0.0f, precision); - - // states at time = 0.5 - bz.getStates(pos, vel, acc, 0.5f); - - ut_compare_float("slope not equal 1", vel(0), 1.0f, precision); - ut_compare_float("slope not equal 0", vel(1), 0.0f, precision); - ut_compare_float("slope not equal 0", vel(2), 0.0f, precision); - - ut_compare_float("acc not equal 2", acc(0), 2.0f, precision); - ut_compare_float("acc not equal 0", acc(1), 0.0f, precision); - ut_compare_float("acc not equal 0", acc(2), 0.0f, precision); - - return true; - -} - -bool BezierQuadTest::_get_arc_length() -{ - // create random numbers - srand(0); // choose a constant to make it deterministic - - float min = -50.f; - float max = 50.f; - float resolution = 0.1f; - - matrix::Vector3f pt0, pt1, ctrl; - float duration, arc_length, triangle_length, straigth_length; - float T = 100.0f; - - // loop trough different control points 100x and check if arc_length is in the expected range - for (int i = 0; i < 100 ; i++) { - // random bezier point - pt0 = matrix::Vector3f(random(min, max), random(min, max), random(min, max)); - pt1 = matrix::Vector3f(random(min, max), random(min, max), random(min, max)); - ctrl = matrix::Vector3f(random(min, max), random(min, max), random(min, max)); - - // use for each test a new duration - duration = random(0.0f, T); - - // create bezier - bezier::BezierQuad_f bz(pt0, ctrl, pt1, duration); - - // compute arc length, triangle length and straigh length - arc_length = bz.getArcLength(resolution); - triangle_length = (ctrl - pt0).length() + (pt1 - ctrl).length(); - straigth_length = (pt1 - pt0).length(); - - // we also compute length from going point to point and add segment - float time_increment = duration / T; - float t = 0.0f + time_increment; - matrix::Vector3f p0 = pt0; - float sum_segments = 0.0f; - - for (int s = 0; s < (int)T; s++) { - matrix::Vector3f nextpt = bz.getPoint(t); - sum_segments = (nextpt - p0).length() + sum_segments; - p0 = bz.getPoint(t); - t = t + time_increment; - } - - // test comparisons - ut_assert_true((triangle_length >= arc_length) && (arc_length >= straigth_length) - && (fabsf(arc_length - sum_segments) < 1.f)); - } - - - return true; -} - -bool BezierQuadTest::_set_bez_from_vel() -{ - // create random numbers - srand(100); // choose a constant to make it deterministic - - float low = -50.0f; - float max = 50.0f; - float precision = 0.001f; - - for (int i = 0; i < 20; i++) { - - // set velocity - matrix::Vector3f ctrl(random(low, max), random(low, max), random(low, max)); - matrix::Vector3f vel0(random(low, max), random(low, max), random(low, max)); - matrix::Vector3f vel1(random(low, max), random(low, max), random(low, max)); - float duration = random(0.0f, 100.0f); - - bezier::BezierQuad_f bz;; - bz.setBezFromVel(ctrl, vel0, vel1, duration); - - // get velocity back - matrix::Vector3f v0 = bz.getVelocity(0.0f); - matrix::Vector3f v1 = bz.getVelocity(duration); - ut_compare_float("", vel0(0), v0(0), precision); - ut_compare_float("", vel1(0), v1(0), precision); - - ut_compare_float("", vel0(1), v0(1), precision); - ut_compare_float("", vel1(1), v1(1), precision); - - ut_compare_float("", vel0(2), v0(2), precision); - ut_compare_float("", vel1(2), v1(2), precision); - } - - return true; -} - -float BezierQuadTest::random(float min, float max) -{ - float s = rand() / (float)RAND_MAX; - return (min + s * (max - min)); - -} - -ut_declare_test_c(test_bezierQuad, BezierQuadTest) diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index af25fd074e..3d04ad31ef 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -78,7 +78,6 @@ const struct { #endif /* __PX4_NUTTX */ {"atomic_bitset", test_atomic_bitset, 0}, - {"bezier", test_bezierQuad, 0}, {"bitset", test_bitset, 0}, {"bson", test_bson, 0}, {"dataman", test_dataman, OPT_NOJIGTEST | OPT_NOALLTEST}, diff --git a/src/systemcmds/tests/tests_main.h b/src/systemcmds/tests/tests_main.h index 0e5a14743c..c42bce07fc 100644 --- a/src/systemcmds/tests/tests_main.h +++ b/src/systemcmds/tests/tests_main.h @@ -44,7 +44,6 @@ __BEGIN_DECLS extern int test_atomic_bitset(int argc, char *argv[]); -extern int test_bezierQuad(int argc, char *argv[]); extern int test_bitset(int argc, char *argv[]); extern int test_bson(int argc, char *argv[]); extern int test_dataman(int argc, char *argv[]); From 04cd247c906bc34a4869a7cc2b1be45a85018f47 Mon Sep 17 00:00:00 2001 From: Silvan Date: Fri, 10 Jan 2025 13:34:48 +0100 Subject: [PATCH 27/91] FlightTaskAuto: remove isTargetModified() As it is no longer needed w/o avoidance. Signed-off-by: Silvan --- .../tasks/Auto/FlightTaskAuto.cpp | 14 -------------- .../tasks/Auto/FlightTaskAuto.hpp | 1 - 2 files changed, 15 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 614bcc6118..419f25827a 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -159,11 +159,6 @@ bool FlightTaskAuto::update() _checkEmergencyBraking(); Vector3f waypoints[] = {_prev_wp, _position_setpoint, _next_wp}; - if (isTargetModified()) { - // In case object avoidance has injected a new setpoint, we take this as the next waypoints - waypoints[2] = _position_setpoint; - } - const bool should_wait_for_yaw_align = _param_mpc_yaw_mode.get() == int32_t(yaw_mode::towards_waypoint_yaw_first) && !_yaw_sp_aligned; const bool force_zero_velocity_setpoint = should_wait_for_yaw_align || _is_emergency_braking_active; @@ -764,15 +759,6 @@ bool FlightTaskAuto::_generateHeadingAlongTraj() return res; } -bool FlightTaskAuto::isTargetModified() const -{ - const bool xy_modified = (_target - _position_setpoint).xy().longerThan(FLT_EPSILON); - const bool z_valid = PX4_ISFINITE(_position_setpoint(2)); - const bool z_modified = z_valid && std::fabs((_target - _position_setpoint)(2)) > FLT_EPSILON; - - return xy_modified || z_modified; -} - void FlightTaskAuto::_updateTrajConstraints() { // update params of the position smoothing diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp index 6c49dd8726..5b8d3e9b5b 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp @@ -108,7 +108,6 @@ protected: void _checkEmergencyBraking(); bool _generateHeadingAlongTraj(); /**< Generates heading along trajectory. */ - bool isTargetModified() const; void _updateTrajConstraints(); void rcHelpModifyYaw(float &yaw_sp); From 0b370ab5d3ec1bd5b81ec9d54af4cd5593a3ff58 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 13 Jan 2025 19:26:16 +0100 Subject: [PATCH 28/91] Remove obstacle avoidance MAVLink Heartbeat check --- msg/TelemetryStatus.msg | 2 -- src/modules/mavlink/mavlink_receiver.cpp | 6 ------ src/modules/mavlink/mavlink_receiver.h | 1 - 3 files changed, 9 deletions(-) diff --git a/msg/TelemetryStatus.msg b/msg/TelemetryStatus.msg index 48d85f934d..b2b3920a36 100644 --- a/msg/TelemetryStatus.msg +++ b/msg/TelemetryStatus.msg @@ -51,13 +51,11 @@ bool heartbeat_type_open_drone_id # MAV_TYPE_ODID bool heartbeat_component_telemetry_radio # MAV_COMP_ID_TELEMETRY_RADIO bool heartbeat_component_log # MAV_COMP_ID_LOG bool heartbeat_component_osd # MAV_COMP_ID_OSD -bool heartbeat_component_obstacle_avoidance # MAV_COMP_ID_OBSTACLE_AVOIDANCE bool heartbeat_component_vio # MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY bool heartbeat_component_pairing_manager # MAV_COMP_ID_PAIRING_MANAGER bool heartbeat_component_udp_bridge # MAV_COMP_ID_UDP_BRIDGE bool heartbeat_component_uart_bridge # MAV_COMP_ID_UART_BRIDGE # Misc component health -bool avoidance_system_healthy bool open_drone_id_system_healthy bool parachute_system_healthy diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 6afab95313..0b06a24b76 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2193,11 +2193,6 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) _heartbeat_component_osd = now; break; - case MAV_COMP_ID_OBSTACLE_AVOIDANCE: - _heartbeat_component_obstacle_avoidance = now; - _mavlink.telemetry_status().avoidance_system_healthy = (hb.system_status == MAV_STATE_ACTIVE); - break; - case MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY: _heartbeat_component_visual_inertial_odometry = now; break; @@ -2909,7 +2904,6 @@ void MavlinkReceiver::CheckHeartbeats(const hrt_abstime &t, bool force) tstatus.heartbeat_component_telemetry_radio = (t <= TIMEOUT + _heartbeat_component_telemetry_radio); tstatus.heartbeat_component_log = (t <= TIMEOUT + _heartbeat_component_log); tstatus.heartbeat_component_osd = (t <= TIMEOUT + _heartbeat_component_osd); - tstatus.heartbeat_component_obstacle_avoidance = (t <= TIMEOUT + _heartbeat_component_obstacle_avoidance); tstatus.heartbeat_component_vio = (t <= TIMEOUT + _heartbeat_component_visual_inertial_odometry); tstatus.heartbeat_component_pairing_manager = (t <= TIMEOUT + _heartbeat_component_pairing_manager); tstatus.heartbeat_component_udp_bridge = (t <= TIMEOUT + _heartbeat_component_udp_bridge); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 84ad2e7c62..3d617a7731 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -394,7 +394,6 @@ private: hrt_abstime _heartbeat_component_telemetry_radio{0}; hrt_abstime _heartbeat_component_log{0}; hrt_abstime _heartbeat_component_osd{0}; - hrt_abstime _heartbeat_component_obstacle_avoidance{0}; hrt_abstime _heartbeat_component_visual_inertial_odometry{0}; hrt_abstime _heartbeat_component_pairing_manager{0}; hrt_abstime _heartbeat_component_udp_bridge{0}; From ed9111ec49d1ab4ae424ec2ba13004863d27d907 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 13 Jan 2025 19:30:42 +0100 Subject: [PATCH 29/91] Remove obstacle avoidance test with Gazebo classic and ROS 1 --- Makefile | 4 - .../1014_gazebo-classic_iris_obs_avoid | 30 ------- .../1014_gazebo-classic_iris_obs_avoid.post | 2 - .../init.d-posix/airframes/CMakeLists.txt | 2 - .../px4_it/mavros/missions/avoidance.plan | 88 ------------------- .../sitl_targets_gazebo-classic.cmake | 1 - test/mavros_posix_test_avoidance.test | 52 ----------- test/rostest_avoidance_run.sh | 20 ----- 8 files changed, 199 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid delete mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid.post delete mode 100644 integrationtests/python_src/px4_it/mavros/missions/avoidance.plan delete mode 100644 test/mavros_posix_test_avoidance.test delete mode 100755 test/rostest_avoidance_run.sh diff --git a/Makefile b/Makefile index c65989ffe7..cfd87b6e23 100644 --- a/Makefile +++ b/Makefile @@ -457,10 +457,6 @@ tests_offboard: rostest @"$(SRC_DIR)"/test/rostest_px4_run.sh mavros_posix_tests_offboard_posctl.test @"$(SRC_DIR)"/test/rostest_px4_run.sh mavros_posix_tests_offboard_rpyrt_ctl.test -tests_avoidance: rostest - @"$(SRC_DIR)"/test/rostest_avoidance_run.sh mavros_posix_test_avoidance.test - @"$(SRC_DIR)"/test/rostest_avoidance_run.sh mavros_posix_test_safe_landing.test - python_coverage: @mkdir -p "$(SRC_DIR)"/build/python_coverage @cd "$(SRC_DIR)"/build/python_coverage && cmake "$(SRC_DIR)" $(CMAKE_ARGS) -G"$(PX4_CMAKE_GENERATOR)" -DCONFIG=px4_sitl_default -DPYTHON_COVERAGE=ON diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid b/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid deleted file mode 100644 index e3c25ec777..0000000000 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid +++ /dev/null @@ -1,30 +0,0 @@ -#!/bin/sh -# -# @name 3DR Iris Quadrotor SITL (Obstacle Avoidance) -# -# @type Quadrotor Wide -# - -. ${R}etc/init.d/rc.mc_defaults - - -param set-default CA_AIRFRAME 0 - -param set-default CA_ROTOR_COUNT 4 -param set-default CA_ROTOR0_PX 0.1515 -param set-default CA_ROTOR0_PY 0.245 -param set-default CA_ROTOR0_KM 0.05 -param set-default CA_ROTOR1_PX -0.1515 -param set-default CA_ROTOR1_PY -0.1875 -param set-default CA_ROTOR1_KM 0.05 -param set-default CA_ROTOR2_PX 0.1515 -param set-default CA_ROTOR2_PY -0.245 -param set-default CA_ROTOR2_KM -0.05 -param set-default CA_ROTOR3_PX -0.1515 -param set-default CA_ROTOR3_PY 0.1875 -param set-default CA_ROTOR3_KM -0.05 - -param set-default PWM_MAIN_FUNC1 101 -param set-default PWM_MAIN_FUNC2 102 -param set-default PWM_MAIN_FUNC3 103 -param set-default PWM_MAIN_FUNC4 104 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid.post b/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid.post deleted file mode 100644 index 64b8ce8b19..0000000000 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid.post +++ /dev/null @@ -1,2 +0,0 @@ -# shellcheck disable=SC2154 -mavlink start -x -u 14558 -r 4000000 -m onboard -o 14541 -p # add mavlink stream for SDK diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index 94f2f4d39a..4ae56f51e9 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -39,8 +39,6 @@ px4_add_romfs_files( 1012_gazebo-classic_iris_rplidar 1013_gazebo-classic_iris_vision 1013_gazebo-classic_iris_vision.post - 1014_gazebo-classic_iris_obs_avoid - 1014_gazebo-classic_iris_obs_avoid.post 1015_gazebo-classic_iris_depth_camera 1016_gazebo-classic_iris_downward_depth_camera 1017_gazebo-classic_iris_opt_flow_mockup diff --git a/integrationtests/python_src/px4_it/mavros/missions/avoidance.plan b/integrationtests/python_src/px4_it/mavros/missions/avoidance.plan deleted file mode 100644 index 1698c6b2bc..0000000000 --- a/integrationtests/python_src/px4_it/mavros/missions/avoidance.plan +++ /dev/null @@ -1,88 +0,0 @@ -{ - "fileType": "Plan", - "geoFence": { - "circles": [ - ], - "polygons": [ - ], - "version": 2 - }, - "groundStation": "QGroundControl", - "mission": { - "cruiseSpeed": 15, - "firmwareType": 12, - "hoverSpeed": 5, - "items": [ - { - "AMSLAltAboveTerrain": null, - "Altitude": 4, - "AltitudeMode": 0, - "autoContinue": true, - "command": 22, - "doJumpId": 1, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.3977432, - 8.5456085, - 4 - ], - "type": "SimpleItem" - }, - { - "AMSLAltAboveTerrain": null, - "Altitude": 4, - "AltitudeMode": 0, - "autoContinue": true, - "command": 16, - "doJumpId": 2, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.3977432, - 8.5458765, - 4 - ], - "type": "SimpleItem" - }, - { - "AMSLAltAboveTerrain": null, - "Altitude": -1, - "AltitudeMode": 0, - "autoContinue": true, - "command": 21, - "doJumpId": 3, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.3977432, - 8.5458812, - -1 - ], - "type": "SimpleItem" - } - ], - "plannedHomePosition": [ - 47.3977419, - 8.5458854, - 487.923 - ], - "vehicleType": 2, - "version": 2 - }, - "rallyPoints": { - "points": [ - ], - "version": 2 - }, - "version": 1 -} diff --git a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake b/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake index b67fe546cc..258e18d8b2 100644 --- a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake +++ b/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake @@ -80,7 +80,6 @@ if(gazebo_FOUND) iris_dual_gps iris_foggy_lidar iris_irlock - iris_obs_avoid iris_depth_camera iris_downward_depth_camera iris_opt_flow diff --git a/test/mavros_posix_test_avoidance.test b/test/mavros_posix_test_avoidance.test deleted file mode 100644 index 5c5adeaa4d..0000000000 --- a/test/mavros_posix_test_avoidance.test +++ /dev/null @@ -1,52 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - $(arg pointcloud_topics) - - - - diff --git a/test/rostest_avoidance_run.sh b/test/rostest_avoidance_run.sh deleted file mode 100755 index 2fc2be7eb6..0000000000 --- a/test/rostest_avoidance_run.sh +++ /dev/null @@ -1,20 +0,0 @@ -#! /bin/bash - -DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd ) -PX4_SRC_DIR=${DIR}/.. - -source /opt/ros/${ROS_DISTRO:-kinetic}/setup.bash -mkdir -p ${PX4_SRC_DIR}/catkin_ws/src -cd ${PX4_SRC_DIR}/catkin_ws/ -git clone -b 0.3.1 --single-branch --depth 1 https://github.com/PX4/avoidance.git src/avoidance - -catkin init -catkin build local_planner safe_landing_planner --cmake-args -DCMAKE_BUILD_TYPE=Release - -source ${PX4_SRC_DIR}/catkin_ws/devel/setup.bash -source /usr/share/gazebo/setup.sh - -export CATKIN_SETUP_UTIL_ARGS=--extend -export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:${PX4_SRC_DIR}/catkin_ws/src/avoidance/avoidance/sim/models - -source $DIR/rostest_px4_run.sh "$@" From 3119510f2598b774c75594c762134ba802ad103e Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 14 Jan 2025 11:13:07 +0100 Subject: [PATCH 30/91] Remove health_component::avoidance Signed-off-by: Silvan Fuhrer --- msg/versioned/ArmingCheckReply.msg | 1 - .../commander/HealthAndArmingChecks/checks/externalChecks.hpp | 3 --- src/modules/mavlink/streams/SYS_STATUS.hpp | 1 - 3 files changed, 5 deletions(-) diff --git a/msg/versioned/ArmingCheckReply.msg b/msg/versioned/ArmingCheckReply.msg index 1a1aaa5183..4c1616d849 100644 --- a/msg/versioned/ArmingCheckReply.msg +++ b/msg/versioned/ArmingCheckReply.msg @@ -6,7 +6,6 @@ uint8 request_id uint8 registration_id uint8 HEALTH_COMPONENT_INDEX_NONE = 0 -uint8 HEALTH_COMPONENT_INDEX_AVOIDANCE = 19 uint8 health_component_index # HEALTH_COMPONENT_INDEX_* bool health_component_is_present diff --git a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp index bbba418000..2efa6896c3 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp @@ -40,9 +40,6 @@ #include #include -static_assert((1ull << arming_check_reply_s::HEALTH_COMPONENT_INDEX_AVOIDANCE) == (uint64_t) - health_component_t::avoidance, "enum definition missmatch"); - class ExternalChecks : public HealthAndArmingCheckBase { public: diff --git a/src/modules/mavlink/streams/SYS_STATUS.hpp b/src/modules/mavlink/streams/SYS_STATUS.hpp index c875eaceaa..7a89501793 100644 --- a/src/modules/mavlink/streams/SYS_STATUS.hpp +++ b/src/modules/mavlink/streams/SYS_STATUS.hpp @@ -144,7 +144,6 @@ private: fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_BATTERY, health_component_t::battery, msg); fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS, health_component_t::motors_escs, msg); fillOutComponent(health_report, MAV_SYS_STATUS_RECOVERY_SYSTEM, health_component_t::parachute, msg); - fillOutComponent(health_report, MAV_SYS_STATUS_OBSTACLE_AVOIDANCE, health_component_t::avoidance, msg); fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_3D_ACCEL, health_component_t::accel, msg); fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_3D_GYRO, health_component_t::gyro, msg); fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_3D_MAG, health_component_t::magnetometer, msg); From 662c66e5463efc6131bb499d4bcd8a730198f1ae Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 15 Jan 2025 10:19:53 +0100 Subject: [PATCH 31/91] VehicleStatus: bump VERSION to 1 after removal of avoidance_system_required Signed-off-by: Silvan Fuhrer --- msg/versioned/VehicleStatus.msg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/msg/versioned/VehicleStatus.msg b/msg/versioned/VehicleStatus.msg index 062c6f0765..b9a3edd91f 100644 --- a/msg/versioned/VehicleStatus.msg +++ b/msg/versioned/VehicleStatus.msg @@ -1,6 +1,6 @@ # Encodes the system state of the vehicle published by commander -uint32 MESSAGE_VERSION = 0 +uint32 MESSAGE_VERSION = 1 uint64 timestamp # time since system start (microseconds) From 3be8b680f66be0ba286fe1e2986e46f0ca744c74 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 14 Feb 2025 10:35:27 +0100 Subject: [PATCH 32/91] msg_translation: Add vehicle_status_v1 translation Signed-off-by: Silvan Fuhrer --- msg/px4_msgs_old/msg/VehicleStatusV0.msg | 140 ++++++++++++++++++ .../translations/all_translations.h | 2 + .../translation_vehicle_status_v1.h | 110 ++++++++++++++ 3 files changed, 252 insertions(+) create mode 100644 msg/px4_msgs_old/msg/VehicleStatusV0.msg create mode 100644 msg/translation_node/translations/translation_vehicle_status_v1.h diff --git a/msg/px4_msgs_old/msg/VehicleStatusV0.msg b/msg/px4_msgs_old/msg/VehicleStatusV0.msg new file mode 100644 index 0000000000..a347dfce3d --- /dev/null +++ b/msg/px4_msgs_old/msg/VehicleStatusV0.msg @@ -0,0 +1,140 @@ +# Encodes the system state of the vehicle published by commander + +uint32 MESSAGE_VERSION = 0 + +uint64 timestamp # time since system start (microseconds) + +uint64 armed_time # Arming timestamp (microseconds) +uint64 takeoff_time # Takeoff timestamp (microseconds) + +uint8 arming_state +uint8 ARMING_STATE_DISARMED = 1 +uint8 ARMING_STATE_ARMED = 2 + +uint8 latest_arming_reason +uint8 latest_disarming_reason +uint8 ARM_DISARM_REASON_TRANSITION_TO_STANDBY = 0 +uint8 ARM_DISARM_REASON_STICK_GESTURE = 1 +uint8 ARM_DISARM_REASON_RC_SWITCH = 2 +uint8 ARM_DISARM_REASON_COMMAND_INTERNAL = 3 +uint8 ARM_DISARM_REASON_COMMAND_EXTERNAL = 4 +uint8 ARM_DISARM_REASON_MISSION_START = 5 +uint8 ARM_DISARM_REASON_SAFETY_BUTTON = 6 +uint8 ARM_DISARM_REASON_AUTO_DISARM_LAND = 7 +uint8 ARM_DISARM_REASON_AUTO_DISARM_PREFLIGHT = 8 +uint8 ARM_DISARM_REASON_KILL_SWITCH = 9 +uint8 ARM_DISARM_REASON_LOCKDOWN = 10 +uint8 ARM_DISARM_REASON_FAILURE_DETECTOR = 11 +uint8 ARM_DISARM_REASON_SHUTDOWN = 12 +uint8 ARM_DISARM_REASON_UNIT_TEST = 13 + +uint64 nav_state_timestamp # time when current nav_state activated + +uint8 nav_state_user_intention # Mode that the user selected (might be different from nav_state in a failsafe situation) + +uint8 nav_state # Currently active mode +uint8 NAVIGATION_STATE_MANUAL = 0 # Manual mode +uint8 NAVIGATION_STATE_ALTCTL = 1 # Altitude control mode +uint8 NAVIGATION_STATE_POSCTL = 2 # Position control mode +uint8 NAVIGATION_STATE_AUTO_MISSION = 3 # Auto mission mode +uint8 NAVIGATION_STATE_AUTO_LOITER = 4 # Auto loiter mode +uint8 NAVIGATION_STATE_AUTO_RTL = 5 # Auto return to launch mode +uint8 NAVIGATION_STATE_POSITION_SLOW = 6 +uint8 NAVIGATION_STATE_FREE5 = 7 +uint8 NAVIGATION_STATE_FREE4 = 8 +uint8 NAVIGATION_STATE_FREE3 = 9 +uint8 NAVIGATION_STATE_ACRO = 10 # Acro mode +uint8 NAVIGATION_STATE_FREE2 = 11 +uint8 NAVIGATION_STATE_DESCEND = 12 # Descend mode (no position control) +uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode +uint8 NAVIGATION_STATE_OFFBOARD = 14 +uint8 NAVIGATION_STATE_STAB = 15 # Stabilized mode +uint8 NAVIGATION_STATE_FREE1 = 16 +uint8 NAVIGATION_STATE_AUTO_TAKEOFF = 17 # Takeoff +uint8 NAVIGATION_STATE_AUTO_LAND = 18 # Land +uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19 # Auto Follow +uint8 NAVIGATION_STATE_AUTO_PRECLAND = 20 # Precision land with landing target +uint8 NAVIGATION_STATE_ORBIT = 21 # Orbit in a circle +uint8 NAVIGATION_STATE_AUTO_VTOL_TAKEOFF = 22 # Takeoff, transition, establish loiter +uint8 NAVIGATION_STATE_EXTERNAL1 = 23 +uint8 NAVIGATION_STATE_EXTERNAL2 = 24 +uint8 NAVIGATION_STATE_EXTERNAL3 = 25 +uint8 NAVIGATION_STATE_EXTERNAL4 = 26 +uint8 NAVIGATION_STATE_EXTERNAL5 = 27 +uint8 NAVIGATION_STATE_EXTERNAL6 = 28 +uint8 NAVIGATION_STATE_EXTERNAL7 = 29 +uint8 NAVIGATION_STATE_EXTERNAL8 = 30 +uint8 NAVIGATION_STATE_MAX = 31 + +uint8 executor_in_charge # Current mode executor in charge (0=Autopilot) + +uint32 valid_nav_states_mask # Bitmask for all valid nav_state values +uint32 can_set_nav_states_mask # Bitmask for all modes that a user can select + +# Bitmask of detected failures +uint16 failure_detector_status +uint16 FAILURE_NONE = 0 +uint16 FAILURE_ROLL = 1 # (1 << 0) +uint16 FAILURE_PITCH = 2 # (1 << 1) +uint16 FAILURE_ALT = 4 # (1 << 2) +uint16 FAILURE_EXT = 8 # (1 << 3) +uint16 FAILURE_ARM_ESC = 16 # (1 << 4) +uint16 FAILURE_BATTERY = 32 # (1 << 5) +uint16 FAILURE_IMBALANCED_PROP = 64 # (1 << 6) +uint16 FAILURE_MOTOR = 128 # (1 << 7) + +uint8 hil_state +uint8 HIL_STATE_OFF = 0 +uint8 HIL_STATE_ON = 1 + +# If it's a VTOL, then the value will be VEHICLE_TYPE_ROTARY_WING while flying as a multicopter, and VEHICLE_TYPE_FIXED_WING when flying as a fixed-wing +uint8 vehicle_type +uint8 VEHICLE_TYPE_UNKNOWN = 0 +uint8 VEHICLE_TYPE_ROTARY_WING = 1 +uint8 VEHICLE_TYPE_FIXED_WING = 2 +uint8 VEHICLE_TYPE_ROVER = 3 +uint8 VEHICLE_TYPE_AIRSHIP = 4 + +uint8 FAILSAFE_DEFER_STATE_DISABLED = 0 +uint8 FAILSAFE_DEFER_STATE_ENABLED = 1 +uint8 FAILSAFE_DEFER_STATE_WOULD_FAILSAFE = 2 # Failsafes deferred, but would trigger a failsafe + +bool failsafe # true if system is in failsafe state (e.g.:RTL, Hover, Terminate, ...) +bool failsafe_and_user_took_over # true if system is in failsafe state but the user took over control +uint8 failsafe_defer_state # one of FAILSAFE_DEFER_STATE_* + +# Link loss +bool gcs_connection_lost # datalink to GCS lost +uint8 gcs_connection_lost_counter # counts unique GCS connection lost events +bool high_latency_data_link_lost # Set to true if the high latency data link (eg. RockBlock Iridium 9603 telemetry module) is lost + +# VTOL flags +bool is_vtol # True if the system is VTOL capable +bool is_vtol_tailsitter # True if the system performs a 90° pitch down rotation during transition from MC to FW +bool in_transition_mode # True if VTOL is doing a transition +bool in_transition_to_fw # True if VTOL is doing a transition from MC to FW + +# MAVLink identification +uint8 system_type # system type, contains mavlink MAV_TYPE +uint8 system_id # system id, contains MAVLink's system ID field +uint8 component_id # subsystem / component id, contains MAVLink's component ID field + +bool safety_button_available # Set to true if a safety button is connected +bool safety_off # Set to true if safety is off + +bool power_input_valid # set if input power is valid +bool usb_connected # set to true (never cleared) once telemetry received from usb link + +bool open_drone_id_system_present +bool open_drone_id_system_healthy + +bool parachute_system_present +bool parachute_system_healthy + +bool avoidance_system_required # Set to true if avoidance system is enabled via COM_OBS_AVOID parameter +bool avoidance_system_valid # Status of the obstacle avoidance system + +bool rc_calibration_in_progress +bool calibration_enabled + +bool pre_flight_checks_pass # true if all checks necessary to arm pass diff --git a/msg/translation_node/translations/all_translations.h b/msg/translation_node/translations/all_translations.h index 2b3c2030b4..4369a3c58a 100644 --- a/msg/translation_node/translations/all_translations.h +++ b/msg/translation_node/translations/all_translations.h @@ -9,3 +9,5 @@ //#include "example_translation_direct_v1.h" //#include "example_translation_multi_v2.h" //#include "example_translation_service_v1.h" + +#include "translation_vehicle_status_v1.h" diff --git a/msg/translation_node/translations/translation_vehicle_status_v1.h b/msg/translation_node/translations/translation_vehicle_status_v1.h new file mode 100644 index 0000000000..2041b013d8 --- /dev/null +++ b/msg/translation_node/translations/translation_vehicle_status_v1.h @@ -0,0 +1,110 @@ +/**************************************************************************** + * Copyright (c) 2025 PX4 Development Team. + * SPDX-License-Identifier: BSD-3-Clause + ****************************************************************************/ +#pragma once + +// Translate VehicleStatus v0 <--> v1 +#include +#include + +class VehicleStatusV1Translation { +public: + using MessageOlder = px4_msgs_old::msg::VehicleStatusV0; + static_assert(MessageOlder::MESSAGE_VERSION == 0); + + using MessageNewer = px4_msgs::msg::VehicleStatus; + static_assert(MessageNewer::MESSAGE_VERSION == 1); + + static constexpr const char* kTopic = "fmu/out/vehicle_status"; + + static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) { + // Set msg_newer from msg_older + msg_newer.timestamp = msg_older.timestamp; + msg_newer.armed_time = msg_older.armed_time; + msg_newer.takeoff_time = msg_older.takeoff_time; + msg_newer.arming_state = msg_older.arming_state; + msg_newer.latest_arming_reason = msg_older.latest_arming_reason; + msg_newer.latest_disarming_reason = msg_older.latest_disarming_reason; + msg_newer.nav_state_timestamp = msg_older.nav_state_timestamp; + msg_newer.nav_state_user_intention = msg_older.nav_state_user_intention; + msg_newer.nav_state = msg_older.nav_state; + msg_newer.executor_in_charge = msg_older.executor_in_charge; + msg_newer.valid_nav_states_mask = msg_older.valid_nav_states_mask; + msg_newer.can_set_nav_states_mask = msg_older.can_set_nav_states_mask; + msg_newer.failure_detector_status = msg_older.failure_detector_status; + msg_newer.hil_state = msg_older.hil_state; + msg_newer.vehicle_type = msg_older.vehicle_type; + msg_newer.failsafe = msg_older.failsafe; + msg_newer.failsafe_and_user_took_over = msg_older.failsafe_and_user_took_over; + msg_newer.failsafe_defer_state = msg_older.failsafe_defer_state; + msg_newer.gcs_connection_lost = msg_older.gcs_connection_lost; + msg_newer.gcs_connection_lost_counter = msg_older.gcs_connection_lost_counter; + msg_newer.high_latency_data_link_lost = msg_older.high_latency_data_link_lost; + msg_newer.is_vtol = msg_older.is_vtol; + msg_newer.is_vtol_tailsitter = msg_older.is_vtol_tailsitter; + msg_newer.in_transition_mode = msg_older.in_transition_mode; + msg_newer.in_transition_to_fw = msg_older.in_transition_to_fw; + msg_newer.system_type = msg_older.system_type; + msg_newer.system_id = msg_older.system_id; + msg_newer.component_id = msg_older.component_id; + msg_newer.safety_button_available = msg_older.safety_button_available; + msg_newer.safety_off = msg_older.safety_off; + msg_newer.power_input_valid = msg_older.power_input_valid; + msg_newer.usb_connected = msg_older.usb_connected; + msg_newer.open_drone_id_system_present = msg_older.open_drone_id_system_present; + msg_newer.open_drone_id_system_healthy = msg_older.open_drone_id_system_healthy; + msg_newer.parachute_system_present = msg_older.parachute_system_present; + msg_newer.parachute_system_healthy = msg_older.parachute_system_healthy; + msg_newer.rc_calibration_in_progress = msg_older.rc_calibration_in_progress; + msg_newer.calibration_enabled = msg_older.calibration_enabled; + msg_newer.pre_flight_checks_pass = msg_older.pre_flight_checks_pass; + } + + static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) { + // Set msg_older from msg_newer + msg_older.timestamp = msg_newer.timestamp; + msg_older.armed_time = msg_newer.armed_time; + msg_older.takeoff_time = msg_newer.takeoff_time; + msg_older.arming_state = msg_newer.arming_state; + msg_older.latest_arming_reason = msg_newer.latest_arming_reason; + msg_older.latest_disarming_reason = msg_newer.latest_disarming_reason; + msg_older.nav_state_timestamp = msg_newer.nav_state_timestamp; + msg_older.nav_state_user_intention = msg_newer.nav_state_user_intention; + msg_older.nav_state = msg_newer.nav_state; + msg_older.executor_in_charge = msg_newer.executor_in_charge; + msg_older.valid_nav_states_mask = msg_newer.valid_nav_states_mask; + msg_older.can_set_nav_states_mask = msg_newer.can_set_nav_states_mask; + msg_older.failure_detector_status = msg_newer.failure_detector_status; + msg_older.hil_state = msg_newer.hil_state; + msg_older.vehicle_type = msg_newer.vehicle_type; + msg_older.failsafe = msg_newer.failsafe; + msg_older.failsafe_and_user_took_over = msg_newer.failsafe_and_user_took_over; + msg_older.failsafe_defer_state = msg_newer.failsafe_defer_state; + msg_older.gcs_connection_lost = msg_newer.gcs_connection_lost; + msg_older.gcs_connection_lost_counter = msg_newer.gcs_connection_lost_counter; + msg_older.high_latency_data_link_lost = msg_newer.high_latency_data_link_lost; + msg_older.is_vtol = msg_newer.is_vtol; + msg_older.is_vtol_tailsitter = msg_newer.is_vtol_tailsitter; + msg_older.in_transition_mode = msg_newer.in_transition_mode; + msg_older.in_transition_to_fw = msg_newer.in_transition_to_fw; + msg_older.system_type = msg_newer.system_type; + msg_older.system_id = msg_newer.system_id; + msg_older.component_id = msg_newer.component_id; + msg_older.safety_button_available = msg_newer.safety_button_available; + msg_older.safety_off = msg_newer.safety_off; + msg_older.power_input_valid = msg_newer.power_input_valid; + msg_older.usb_connected = msg_newer.usb_connected; + msg_older.open_drone_id_system_present = msg_newer.open_drone_id_system_present; + msg_older.open_drone_id_system_healthy = msg_newer.open_drone_id_system_healthy; + msg_older.parachute_system_present = msg_newer.parachute_system_present; + msg_older.parachute_system_healthy = msg_newer.parachute_system_healthy; + msg_older.avoidance_system_required = false; + msg_older.avoidance_system_valid = false; + msg_older.rc_calibration_in_progress = msg_newer.rc_calibration_in_progress; + msg_older.calibration_enabled = msg_newer.calibration_enabled; + msg_older.pre_flight_checks_pass = msg_newer.pre_flight_checks_pass; + } +}; + +REGISTER_TOPIC_TRANSLATION_DIRECT(VehicleStatusV1Translation); From 5e06ab1430eca9e55d63fb00e83aaf14f42d965f Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 12 Feb 2025 10:53:39 +0100 Subject: [PATCH 33/91] ekf-agp: do not reset to AGP if GNSS fusion is active --- .../aux_global_position.cpp | 29 +++++++++++++++---- src/modules/ekf2/EKF/estimator_interface.cpp | 24 +++++++++++++-- src/modules/ekf2/EKF/estimator_interface.h | 5 +++- 3 files changed, 49 insertions(+), 9 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp index 3307dd56b9..675bffdd3c 100644 --- a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp @@ -104,9 +104,20 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed _state = State::starting; if (ekf.global_origin_valid()) { - ekf.enableControlStatusAuxGpos(); - _reset_counters.lat_lon = sample.lat_lon_reset_counter; - _state = State::active; + const bool fused = ekf.fuseHorizontalPosition(aid_src); + bool reset = false; + + if (!fused && !ekf.isOtherSourceOfHorizontalPositionAidingThan(ekf.control_status_flags().aux_gpos)) { + ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, pos_var, sq(sample.epv)); + ekf.resetAidSourceStatusZeroInnovation(aid_src); + reset = true; + } + + if (fused || reset) { + ekf.enableControlStatusAuxGpos(); + _reset_counters.lat_lon = sample.lat_lon_reset_counter; + _state = State::active; + } } else { // Try to initialize using measurement @@ -128,12 +139,18 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed if (isTimedOut(aid_src.time_last_fuse, imu_delayed.time_us, ekf._params.no_aid_timeout_max) || (_reset_counters.lat_lon != sample.lat_lon_reset_counter)) { + if (ekf.isOnlyActiveSourceOfHorizontalPositionAiding(ekf.control_status_flags().aux_gpos)) { - ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance)); + ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance)); - ekf.resetAidSourceStatusZeroInnovation(aid_src); + ekf.resetAidSourceStatusZeroInnovation(aid_src); - _reset_counters.lat_lon = sample.lat_lon_reset_counter; + _reset_counters.lat_lon = sample.lat_lon_reset_counter; + + } else { + ekf.disableControlStatusAuxGpos(); + _state = State::stopped; + } } } else { diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 84ced94c80..22ccf958f0 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -600,12 +600,32 @@ bool EstimatorInterface::isOtherSourceOfHorizontalAidingThan(const bool aiding_f } int EstimatorInterface::getNumberOfActiveHorizontalAidingSources() const +{ + return getNumberOfActiveHorizontalPositionAidingSources() + getNumberOfActiveHorizontalVelocityAidingSources(); +} + +bool EstimatorInterface::isOnlyActiveSourceOfHorizontalPositionAiding(const bool aiding_flag) const +{ + return aiding_flag && !isOtherSourceOfHorizontalPositionAidingThan(aiding_flag); +} + +bool EstimatorInterface::isOtherSourceOfHorizontalPositionAidingThan(const bool aiding_flag) const +{ + const int nb_sources = getNumberOfActiveHorizontalPositionAidingSources(); + return aiding_flag ? nb_sources > 1 : nb_sources > 0; +} + +int EstimatorInterface::getNumberOfActiveHorizontalPositionAidingSources() const { return int(_control_status.flags.gps) - + int(_control_status.flags.opt_flow) + int(_control_status.flags.ev_pos) + + int(_control_status.flags.aux_gpos); +} + +int EstimatorInterface::getNumberOfActiveHorizontalVelocityAidingSources() const +{ + return int(_control_status.flags.opt_flow) + int(_control_status.flags.ev_vel) - + int(_control_status.flags.aux_gpos) // Combined airspeed and sideslip fusion allows sustained wind relative dead reckoning // and so is treated as a single aiding source. + int(_control_status.flags.fuse_aspd && _control_status.flags.fuse_beta); diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 1212300278..93632bc109 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -207,8 +207,8 @@ public: // set air density used by the multi-rotor specific drag force fusion void set_air_density(float air_density) { _air_density = air_density; } - // the flags considered are opt_flow, gps, ev_vel and ev_pos bool isOnlyActiveSourceOfHorizontalAiding(bool aiding_flag) const; + bool isOnlyActiveSourceOfHorizontalPositionAiding(bool aiding_flag) const; /* * Check if there are any other active source of horizontal aiding @@ -221,6 +221,7 @@ public: * @return true if an other source than aiding_flag is active */ bool isOtherSourceOfHorizontalAidingThan(bool aiding_flag) const; + bool isOtherSourceOfHorizontalPositionAidingThan(bool aiding_flag) const; // Return true if at least one source of horizontal aiding is active // the flags considered are opt_flow, gps, ev_vel and ev_pos @@ -228,6 +229,8 @@ public: bool isVerticalAidingActive() const; int getNumberOfActiveHorizontalAidingSources() const; + int getNumberOfActiveHorizontalPositionAidingSources() const; + int getNumberOfActiveHorizontalVelocityAidingSources() const; bool isOtherSourceOfVerticalPositionAidingThan(bool aiding_flag) const; bool isVerticalPositionAidingActive() const; From 412d4390a6d98f9be670aab905d66b54a0d1dada Mon Sep 17 00:00:00 2001 From: Perre <60040461+Perrrewi@users.noreply.github.com> Date: Tue, 18 Feb 2025 14:38:11 +0100 Subject: [PATCH 34/91] MC Slowmode: Yawstick for gimbal control, vehicle yaw follows gimbal (#24242) * Yawsticks on gimbal, vehicle follows gimbal in slowmode, once vehicle has taken off * Increase queue length to avoid automatically unadvertise queued publications with queue length 1 * Improve readability --------- Co-authored-by: Pernilla --- msg/GimbalManagerSetAttitude.msg | 2 + .../FlightTaskManualAccelerationSlow.cpp | 33 ++++- .../FlightTaskManualAccelerationSlow.hpp | 10 +- .../flight_task_acceleration_slow_params.c | 14 ++ .../tasks/Utility/CMakeLists.txt | 1 + .../tasks/Utility/Gimbal.cpp | 126 ++++++++++++++++++ .../tasks/Utility/Gimbal.hpp | 90 +++++++++++++ src/modules/gimbal/input_mavlink.cpp | 28 ++-- 8 files changed, 288 insertions(+), 16 deletions(-) create mode 100644 src/modules/flight_mode_manager/tasks/Utility/Gimbal.cpp create mode 100644 src/modules/flight_mode_manager/tasks/Utility/Gimbal.hpp diff --git a/msg/GimbalManagerSetAttitude.msg b/msg/GimbalManagerSetAttitude.msg index d88acca8b6..e1a174fd0b 100644 --- a/msg/GimbalManagerSetAttitude.msg +++ b/msg/GimbalManagerSetAttitude.msg @@ -20,3 +20,5 @@ float32[4] q float32 angular_velocity_x float32 angular_velocity_y float32 angular_velocity_z + +uint8 ORB_QUEUE_LENGTH = 2 diff --git a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp index 7b1c24f1dd..dd2ca1da59 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp @@ -39,6 +39,7 @@ #include using namespace time_literals; +using namespace matrix; bool FlightTaskManualAccelerationSlow::update() { @@ -128,7 +129,29 @@ bool FlightTaskManualAccelerationSlow::update() FlightTaskManualAltitude::_velocity_constraint_down = velocity_down; FlightTaskManualAcceleration::_stick_yaw.setYawspeedConstraint(yaw_rate); - return FlightTaskManualAcceleration::update(); + bool ret = FlightTaskManualAcceleration::update(); + + // Optimize input-to-video latency gimbal control + if (_gimbal.checkForTelemetry(_time_stamp_current) && haveTakenOff()) { + _gimbal.acquireGimbalControlIfNeeded(); + + // the exact same _yawspeed_setpoint is setpoint for the gimbal and vehicle feed-forward + const float pitchrate_setpoint = getInputFromSanitizedAuxParameterIndex(_param_mc_slow_map_pitch.get()) * yaw_rate; + _yawspeed_setpoint = _sticks.getYaw() * yaw_rate; + + _gimbal.publishGimbalManagerSetAttitude(Gimbal::FLAGS_ALL_AXES_LOCKED, Quatf(NAN, NAN, NAN, NAN), + Vector3f(NAN, pitchrate_setpoint, _yawspeed_setpoint)); + + if (_gimbal.allAxesLockedConfirmed()) { + // but the vehicle makes sure it stays alligned with the gimbal absolute yaw + _yaw_setpoint = _gimbal.getTelemetryYaw(); + } + + } else { + _gimbal.releaseGimbalControlIfNeeded(); + } + + return ret; } float FlightTaskManualAccelerationSlow::getInputFromSanitizedAuxParameterIndex(int parameter_value) @@ -136,3 +159,11 @@ float FlightTaskManualAccelerationSlow::getInputFromSanitizedAuxParameterIndex(i const int sanitized_index = math::constrain(parameter_value - 1, 0, 5); return _sticks.getAux()(sanitized_index); } + +bool FlightTaskManualAccelerationSlow::haveTakenOff() +{ + takeoff_status_s takeoff_status{}; + _takeoff_status_sub.copy(&takeoff_status); + + return takeoff_status.takeoff_state == takeoff_status_s::TAKEOFF_STATE_FLIGHT; +} diff --git a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp index b54d9b765d..66b2de4a8f 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp @@ -42,6 +42,7 @@ #include "FlightTaskManualAcceleration.hpp" #include "StickAccelerationXY.hpp" +#include "Gimbal.hpp" #include #include @@ -67,9 +68,15 @@ private: bool _velocity_limits_received_before{false}; + // Gimbal control + Gimbal _gimbal{this}; + uORB::Subscription _velocity_limits_sub{ORB_ID(velocity_limits)}; velocity_limits_s _velocity_limits{}; + uORB::Subscription _takeoff_status_sub{ORB_ID(takeoff_status)}; + bool haveTakenOff(); + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAcceleration, (ParamInt) _param_mc_slow_map_hvel, (ParamInt) _param_mc_slow_map_vvel, @@ -83,6 +90,7 @@ private: (ParamFloat) _param_mpc_vel_manual, (ParamFloat) _param_mpc_z_vel_max_up, (ParamFloat) _param_mpc_z_vel_max_dn, - (ParamFloat) _param_mpc_man_y_max + (ParamFloat) _param_mpc_man_y_max, + (ParamInt) _param_mc_slow_map_pitch ) }; diff --git a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/flight_task_acceleration_slow_params.c b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/flight_task_acceleration_slow_params.c index 4c643d87d8..32558d98ea 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/flight_task_acceleration_slow_params.c +++ b/src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/flight_task_acceleration_slow_params.c @@ -156,3 +156,17 @@ PARAM_DEFINE_FLOAT(MC_SLOW_DEF_VVEL, 1.f); * @group Multicopter Position Slow Mode */ PARAM_DEFINE_FLOAT(MC_SLOW_DEF_YAWR, 45.f); + +/** + * RC_MAP_AUX{N} to allow for gimbal pitch rate control in position slow mode + * + * @value 0 No pitch rate input + * @value 1 AUX1 + * @value 2 AUX2 + * @value 3 AUX3 + * @value 4 AUX4 + * @value 5 AUX5 + * @value 6 AUX6 + * @group Multicopter Position Slow Mode + */ +PARAM_DEFINE_INT32(MC_SLOW_MAP_PTCH, 0); diff --git a/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt index 6c32116237..44a5e1dba1 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt +++ b/src/modules/flight_mode_manager/tasks/Utility/CMakeLists.txt @@ -36,6 +36,7 @@ px4_add_library(FlightTaskUtility StickAccelerationXY.cpp StickTiltXY.cpp StickYaw.cpp + Gimbal.cpp ) target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis SlewRate motion_planning mathlib) diff --git a/src/modules/flight_mode_manager/tasks/Utility/Gimbal.cpp b/src/modules/flight_mode_manager/tasks/Utility/Gimbal.cpp new file mode 100644 index 0000000000..21e97f5fc3 --- /dev/null +++ b/src/modules/flight_mode_manager/tasks/Utility/Gimbal.cpp @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "Gimbal.hpp" +#include + +using namespace time_literals; +using namespace matrix; + +Gimbal::Gimbal(ModuleParams *parent) : + ModuleParams(parent) +{} + +Gimbal::~Gimbal() +{ + releaseGimbalControlIfNeeded(); +} + +bool Gimbal::checkForTelemetry(const hrt_abstime now) +{ + if (_gimbal_device_attitude_status_sub.updated()) { + gimbal_device_attitude_status_s gimbal_device_attitude_status{}; + + if (_gimbal_device_attitude_status_sub.copy(&gimbal_device_attitude_status)) { + _telemtry_timestamp = gimbal_device_attitude_status.timestamp; + _telemetry_flags = gimbal_device_attitude_status.device_flags; + _telemetry_yaw = Eulerf(Quatf(gimbal_device_attitude_status.q)).psi(); + } + } + + return now < _telemtry_timestamp + 2_s; +} + +void Gimbal::acquireGimbalControlIfNeeded() +{ + if (!_have_gimbal_control) { + _have_gimbal_control = true; + + vehicle_command_s vehicle_command{}; + vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE; + vehicle_command.param1 = _param_mav_sys_id.get(); + vehicle_command.param2 = _param_mav_comp_id.get(); + vehicle_command.param3 = -1.0f; // Leave unchanged. + vehicle_command.param4 = -1.0f; // Leave unchanged. + vehicle_command.timestamp = hrt_absolute_time(); + vehicle_command.source_system = _param_mav_sys_id.get(); + vehicle_command.source_component = _param_mav_comp_id.get(); + vehicle_command.target_system = _param_mav_sys_id.get(); + vehicle_command.target_component = _param_mav_sys_id.get(); + vehicle_command.from_external = false; + _vehicle_command_pub.publish(vehicle_command); + } +} + +void Gimbal::releaseGimbalControlIfNeeded() +{ + if (_have_gimbal_control) { + _have_gimbal_control = false; + + // Restore default flags, setting rate setpoints to NAN lead to unexpected behavior + publishGimbalManagerSetAttitude(FLAGS_ROLL_PITCH_LOCKED, + Quatf(NAN, NAN, NAN, NAN), + Vector3f(NAN, 0.f, 0.f)); + + // Release gimbal + vehicle_command_s vehicle_command{}; + vehicle_command.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE; + vehicle_command.param1 = -3.0f; // Remove control if it had it. + vehicle_command.param2 = -3.0f; // Remove control if it had it. + vehicle_command.param3 = -1.0f; // Leave unchanged. + vehicle_command.param4 = -1.0f; // Leave unchanged. + vehicle_command.timestamp = hrt_absolute_time(); + vehicle_command.from_external = false; + vehicle_command.source_system = _param_mav_sys_id.get(); + vehicle_command.source_component = _param_mav_comp_id.get(); + vehicle_command.target_system = _param_mav_sys_id.get(); + vehicle_command.target_component = _param_mav_comp_id.get(); + _vehicle_command_pub.publish(vehicle_command); + } +} + +void Gimbal::publishGimbalManagerSetAttitude(const uint16_t gimbal_flags, + const matrix::Quatf &q_gimbal_setpoint, + const matrix::Vector3f &gimbal_rates) +{ + gimbal_manager_set_attitude_s gimbal_setpoint{}; + gimbal_setpoint.origin_sysid = _param_mav_sys_id.get(); + gimbal_setpoint.origin_compid = _param_mav_comp_id.get(); + gimbal_setpoint.flags = gimbal_flags; + q_gimbal_setpoint.copyTo(gimbal_setpoint.q); + gimbal_setpoint.angular_velocity_x = gimbal_rates(0); + gimbal_setpoint.angular_velocity_y = gimbal_rates(1); + gimbal_setpoint.angular_velocity_z = gimbal_rates(2); + gimbal_setpoint.timestamp = hrt_absolute_time(); + _gimbal_manager_set_attitude_pub.publish(gimbal_setpoint); +} diff --git a/src/modules/flight_mode_manager/tasks/Utility/Gimbal.hpp b/src/modules/flight_mode_manager/tasks/Utility/Gimbal.hpp new file mode 100644 index 0000000000..938ef32aa2 --- /dev/null +++ b/src/modules/flight_mode_manager/tasks/Utility/Gimbal.hpp @@ -0,0 +1,90 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file Gimbal.hpp + * @brief Gimbal interface with flight tasks + * @author Pernilla Wikström + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "Sticks.hpp" + + +class Gimbal : public ModuleParams +{ +public: + Gimbal(ModuleParams *parent); + ~Gimbal(); + + bool checkForTelemetry(const hrt_abstime now); + void publishGimbalManagerSetAttitude(const uint16_t gimbal_flags, + const matrix::Quatf &q_gimbal_setpoint, + const matrix::Vector3f &gimbal_rates); + void acquireGimbalControlIfNeeded(); + void releaseGimbalControlIfNeeded(); + float getTelemetryYaw() { return _telemetry_yaw; } + bool allAxesLockedConfirmed() { return _telemetry_flags == FLAGS_ALL_AXES_LOCKED; } + + static constexpr uint16_t FLAGS_ALL_AXES_LOCKED = gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_ROLL_LOCK + | gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_PITCH_LOCK + | gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_YAW_LOCK; + static constexpr uint16_t FLAGS_ROLL_PITCH_LOCKED = + gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_ROLL_LOCK + | gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_PITCH_LOCK; + +private: + bool _have_gimbal_control{false}; + + uORB::Subscription _gimbal_device_attitude_status_sub{ORB_ID(gimbal_device_attitude_status)}; + hrt_abstime _telemtry_timestamp{}; + uint16_t _telemetry_flags{}; + float _telemetry_yaw{}; + + uORB::Publication _gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)}; + uORB::Publication _vehicle_command_pub{ORB_ID(vehicle_command)}; + + DEFINE_PARAMETERS( + (ParamInt) _param_mav_sys_id, + (ParamInt) _param_mav_comp_id + ) +}; diff --git a/src/modules/gimbal/input_mavlink.cpp b/src/modules/gimbal/input_mavlink.cpp index e20bf6c552..aef348ca53 100644 --- a/src/modules/gimbal/input_mavlink.cpp +++ b/src/modules/gimbal/input_mavlink.cpp @@ -43,6 +43,8 @@ #include #include +using namespace matrix; + namespace gimbal { @@ -936,21 +938,19 @@ InputMavlinkGimbalV2::UpdateResult InputMavlinkGimbalV2::_process_set_manual_con if (set_manual_control.origin_sysid == control_data.sysid_primary_control && set_manual_control.origin_compid == control_data.compid_primary_control) { - const matrix::Quatf q = - (PX4_ISFINITE(set_manual_control.pitch) && PX4_ISFINITE(set_manual_control.yaw)) - ? - matrix::Quatf( - matrix::Eulerf(0.0f, set_manual_control.pitch, set_manual_control.yaw)) - : - matrix::Quatf(NAN, NAN, NAN, NAN); + Quatf q(NAN, NAN, NAN, NAN); - const matrix::Vector3f angular_velocity = - (PX4_ISFINITE(set_manual_control.pitch_rate) && - PX4_ISFINITE(set_manual_control.yaw_rate)) ? - matrix::Vector3f(0.0f, - math::radians(_parameters.mnt_rate_pitch) * set_manual_control.pitch_rate, - math::radians(_parameters.mnt_rate_yaw) * set_manual_control.yaw_rate) : - matrix::Vector3f(NAN, NAN, NAN); + if (PX4_ISFINITE(set_manual_control.pitch) && PX4_ISFINITE(set_manual_control.yaw)) { + q = Quatf(matrix::Eulerf(0.0f, set_manual_control.pitch, set_manual_control.yaw)); + } + + Vector3f angular_velocity(NAN, NAN, NAN); + + if (PX4_ISFINITE(set_manual_control.pitch_rate) && PX4_ISFINITE(set_manual_control.yaw_rate)) { + angular_velocity = Vector3f(0.0f, + set_manual_control.pitch_rate * math::radians(_parameters.mnt_rate_pitch), + set_manual_control.yaw_rate * math::radians(_parameters.mnt_rate_yaw)); + } _set_control_data_from_set_attitude(control_data, set_manual_control.flags, q, angular_velocity, set_manual_control.timestamp); From 3c129aefa15edc9fcdea3fdeaa5cbce9c5abdab3 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 18 Feb 2025 17:15:32 +0100 Subject: [PATCH 35/91] Add IC engine control module (#24055) Internal combustion engine control module. New actuator functions and RPM based start/restart logic. Not enabled by default. --------- Signed-off-by: Silvan Fuhrer Co-authored-by: Pernilla --- ROMFS/px4fmu_common/init.d-posix/rcS | 5 + ROMFS/px4fmu_common/init.d/rcS | 5 + msg/CMakeLists.txt | 1 + msg/InternalCombustionEngineControl.msg | 8 + src/lib/mixer_module/CMakeLists.txt | 1 + .../functions/FunctionICEngineControl.hpp | 83 ++++ src/lib/mixer_module/mixer_module.cpp | 1 + src/lib/mixer_module/mixer_module.hpp | 1 + src/lib/mixer_module/output_functions.yaml | 5 + .../CMakeLists.txt | 46 ++ .../InternalCombustionEngineControl.cpp | 397 ++++++++++++++++++ .../InternalCombustionEngineControl.hpp | 159 +++++++ .../Kconfig | 5 + .../module.yaml | 120 ++++++ src/modules/logger/logged_topics.cpp | 1 + 15 files changed, 838 insertions(+) create mode 100644 msg/InternalCombustionEngineControl.msg create mode 100644 src/lib/mixer_module/functions/FunctionICEngineControl.hpp create mode 100644 src/modules/internal_combustion_engine_control/CMakeLists.txt create mode 100644 src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.cpp create mode 100644 src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.hpp create mode 100644 src/modules/internal_combustion_engine_control/Kconfig create mode 100644 src/modules/internal_combustion_engine_control/module.yaml diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index ef2907deb7..c53dbbd46a 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -337,6 +337,11 @@ then payload_deliverer start fi +if param compare -s ICE_EN 1 +then + internal_combustion_engine_control start +fi + #user defined mavlink streams for instances can be in PATH . px4-rc.mavlink diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 807a26ed03..b850985fd8 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -555,6 +555,11 @@ else payload_deliverer start fi + if param compare -s ICE_EN 1 + then + internal_combustion_engine_control start + fi + # # Optional board supplied extras: rc.board_extras # diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index af7a98cf54..5844caafec 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -114,6 +114,7 @@ set(msg_files HeaterStatus.msg HoverThrustEstimate.msg InputRc.msg + InternalCombustionEngineControl.msg InternalCombustionEngineStatus.msg IridiumsbdStatus.msg IrlockReport.msg diff --git a/msg/InternalCombustionEngineControl.msg b/msg/InternalCombustionEngineControl.msg new file mode 100644 index 0000000000..08b4198e01 --- /dev/null +++ b/msg/InternalCombustionEngineControl.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) + +bool ignition_on # activate/deactivate ignition (Spark Plug) +float32 throttle_control # [0,1] - Motor should idle with 0. Includes slew rate if enabled. +float32 choke_control # [0,1] - 1 fully closes the air inlet. +float32 starter_engine_control # [0,1] - control value for electric starter motor. + +uint8 user_request # user intent for the ICE being on/off diff --git a/src/lib/mixer_module/CMakeLists.txt b/src/lib/mixer_module/CMakeLists.txt index 783dfe3dde..3a1581118b 100644 --- a/src/lib/mixer_module/CMakeLists.txt +++ b/src/lib/mixer_module/CMakeLists.txt @@ -48,6 +48,7 @@ px4_add_library(mixer_module functions/FunctionConstantMax.hpp functions/FunctionConstantMin.hpp functions/FunctionGimbal.hpp + functions/FunctionICEngineControl.hpp functions/FunctionLandingGear.hpp functions/FunctionManualRC.hpp functions/FunctionMotors.hpp diff --git a/src/lib/mixer_module/functions/FunctionICEngineControl.hpp b/src/lib/mixer_module/functions/FunctionICEngineControl.hpp new file mode 100644 index 0000000000..0609f5cc94 --- /dev/null +++ b/src/lib/mixer_module/functions/FunctionICEngineControl.hpp @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "FunctionProviderBase.hpp" + +#include + +/** + * Functions: ICE... + */ +class FunctionICEControl : public FunctionProviderBase +{ +public: + FunctionICEControl() + { + resetAllToDisarmedValue(); + } + + static FunctionProviderBase *allocate(const Context &context) { return new FunctionICEControl(); } + + void update() override + { + internal_combustion_engine_control_s internal_combustion_engine_control; + + // map [0, 1] to [-1, 1] which is the interface for non-motor PWM channels + if (_internal_combustion_engine_control_sub.update(&internal_combustion_engine_control)) { + _data[0] = internal_combustion_engine_control.ignition_on * 2.f - 1.f; + _data[1] = internal_combustion_engine_control.throttle_control * 2.f - 1.f; + _data[2] = internal_combustion_engine_control.choke_control * 2.f - 1.f; + _data[3] = internal_combustion_engine_control.starter_engine_control * 2.f - 1.f; + } + } + + float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::IC_Engine_Ignition]; } + +private: + static constexpr int num_data_points = 4; + + void resetAllToDisarmedValue() + { + for (int i = 0; i < num_data_points; ++i) { + _data[i] = NAN; + } + } + + static_assert(num_data_points == (int)OutputFunction::IC_Engine_Starter - (int)OutputFunction::IC_Engine_Ignition + 1, + "number of functions mismatch"); + + uORB::Subscription _internal_combustion_engine_control_sub{ORB_ID(internal_combustion_engine_control)}; + float _data[num_data_points] {}; +}; diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 45f56ab540..56a2d61356 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -64,6 +64,7 @@ static const FunctionProvider all_function_providers[] = { {OutputFunction::Gripper, &FunctionGripper::allocate}, {OutputFunction::RC_Roll, OutputFunction::RC_AUXMax, &FunctionManualRC::allocate}, {OutputFunction::Gimbal_Roll, OutputFunction::Gimbal_Yaw, &FunctionGimbal::allocate}, + {OutputFunction::IC_Engine_Ignition, OutputFunction::IC_Engine_Starter, &FunctionICEControl::allocate}, }; MixingOutput::MixingOutput(const char *param_prefix, uint8_t max_num_outputs, OutputModuleInterface &interface, diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index ff400ca6d5..c55b72da4e 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -40,6 +40,7 @@ #include "functions/FunctionConstantMin.hpp" #include "functions/FunctionGimbal.hpp" #include "functions/FunctionGripper.hpp" +#include "functions/FunctionICEngineControl.hpp" #include "functions/FunctionLandingGear.hpp" #include "functions/FunctionLandingGearWheel.hpp" #include "functions/FunctionManualRC.hpp" diff --git a/src/lib/mixer_module/output_functions.yaml b/src/lib/mixer_module/output_functions.yaml index a9965a5454..52543add97 100644 --- a/src/lib/mixer_module/output_functions.yaml +++ b/src/lib/mixer_module/output_functions.yaml @@ -38,6 +38,11 @@ functions: Landing_Gear_Wheel: 440 + IC_Engine_Ignition: 450 + IC_Engine_Throttle: 451 + IC_Engine_Choke: 452 + IC_Engine_Starter: 453 + # Add your own here: #MyCustomFunction: 10000 diff --git a/src/modules/internal_combustion_engine_control/CMakeLists.txt b/src/modules/internal_combustion_engine_control/CMakeLists.txt new file mode 100644 index 0000000000..15df6ca368 --- /dev/null +++ b/src/modules/internal_combustion_engine_control/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE modules__internal_combustion_engine_control + MAIN internal_combustion_engine_control + COMPILE_FLAGS + #-DDEBUG_BUILD + SRCS + InternalCombustionEngineControl.cpp + InternalCombustionEngineControl.hpp + MODULE_CONFIG + module.yaml + DEPENDS + px4_work_queue + SlewRate +) diff --git a/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.cpp b/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.cpp new file mode 100644 index 0000000000..f4ab6a05aa --- /dev/null +++ b/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.cpp @@ -0,0 +1,397 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "InternalCombustionEngineControl.hpp" + +#include + +using namespace time_literals; + +namespace internal_combustion_engine_control +{ + +InternalCombustionEngineControl::InternalCombustionEngineControl() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) +{ + _internal_combustion_engine_control_pub.advertise(); + _internal_combustion_engine_status_pub.advertise(); +} + +InternalCombustionEngineControl::~InternalCombustionEngineControl() +{ + +} + +int InternalCombustionEngineControl::task_spawn(int argc, char *argv[]) +{ + InternalCombustionEngineControl *obj = new InternalCombustionEngineControl(); + + if (!obj) { + PX4_ERR("alloc failed"); + return -1; + } + + _object.store(obj); + _task_id = task_id_is_work_queue; + + /* Schedule a cycle to start things. */ + obj->start(); + + return 0; +} + +void InternalCombustionEngineControl::start() +{ + ScheduleOnInterval(20_ms); // 50 Hz +} + +void InternalCombustionEngineControl::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + } + + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + updateParams(); + _throttle_control_slew_rate.setSlewRate(_param_ice_thr_slew.get()); + } + + + manual_control_setpoint_s manual_control_setpoint; + _manual_control_setpoint_sub.copy(&manual_control_setpoint); + + vehicle_status_s vehicle_status; + _vehicle_status_sub.copy(&vehicle_status); + + actuator_motors_s actuator_motors; + _actuator_motors.copy(&actuator_motors); + + const float throttle_in = actuator_motors.control[0]; + + const hrt_abstime now = hrt_absolute_time(); + + UserOnOffRequest user_request = UserOnOffRequest::Off; + + switch (static_cast(_param_ice_on_source.get())) { + case ICESource::ArmingState: { + if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + user_request = UserOnOffRequest::On; + } + } + break; + + case ICESource::Aux1: { + if (manual_control_setpoint.aux1 > 0.5f) { + user_request = UserOnOffRequest::On; + } + } + break; + + case ICESource::Aux2: { + if (manual_control_setpoint.aux2 > 0.5f) { + user_request = UserOnOffRequest::On; + } + } + break; + } + + switch (_state) { + case State::Stopped: { + controlEngineStop(); + + if (user_request == UserOnOffRequest::On && !maximumAttemptsReached()) { + + _state = State::Starting; + _state_start_time = now; + PX4_INFO("ICE: Starting"); + } + } + break; + + case State::Starting: { + + if (user_request == UserOnOffRequest::Off) { + _state = State::Stopped; + _starting_retry_cycle = 0; + PX4_INFO("ICE: Stopped"); + + } else { + + switch (_starting_sub_state) { + case SubState::Rest: { + if (isStartingPermitted(now)) { + _state_start_time = now; + _starting_sub_state = SubState::Run; + } + } + break; + + case SubState::Run: + default: { + controlEngineStartup(now); + + if (isEngineRunning(now)) { + _state = State::Running; + PX4_INFO("ICE: Starting finished"); + + } else { + + if (maximumAttemptsReached()) { + _state = State::Fault; + PX4_WARN("ICE: Fault"); + + } else if (!isStartingPermitted(now)) { + controlEngineStop(); + _starting_sub_state = SubState::Rest; + } + } + + break; + } + + } + } + + } + break; + + case State::Running: { + controlEngineRunning(throttle_in); + + if (user_request == UserOnOffRequest::Off) { + _state = State::Stopped; + _starting_retry_cycle = 0; + PX4_INFO("ICE: Stopped"); + + } else if (!isEngineRunning(now) && _param_ice_running_fault_detection.get()) { + // without RPM feedback we assume the engine is running after the + // starting procedure but only switch state if fault detection is enabled + _state = State::Starting; + _state_start_time = now; + _starting_retry_cycle = 0; + PX4_WARN("ICE: Running Fault detected"); + events::send(events::ID("internal_combustion_engine_control_fault"), events::Log::Critical, + "IC engine fault detected"); + } + } + + break; + + case State::Fault: { + + // do nothing + if (user_request == UserOnOffRequest::Off) { + _state = State::Stopped; + _starting_retry_cycle = 0; + PX4_INFO("ICE: Stopped"); + + } else { + controlEngineFault(); + } + } + + + break; + } + + const float control_interval = math::constrain(static_cast((now - _last_time_run) * 1e-6f), 0.01f, 0.1f); + + _last_time_run = now; + + // slew rate limit throttle control if it's finite, otherwise just pass it through (0 throttle = NAN = disarmed) + if (PX4_ISFINITE(_throttle_control)) { + _throttle_control = _throttle_control_slew_rate.update(_throttle_control, control_interval); + + } else { + _throttle_control_slew_rate.setForcedValue(0.f); + } + + publishControl(now, user_request); +} + +void InternalCombustionEngineControl::publishControl(const hrt_abstime now, const UserOnOffRequest user_request) +{ + internal_combustion_engine_control_s ice_control{}; + ice_control.timestamp = now; + ice_control.choke_control = _choke_control; + ice_control.ignition_on = _ignition_on; + ice_control.starter_engine_control = _starter_engine_control; + ice_control.throttle_control = _throttle_control; + ice_control.user_request = static_cast(user_request); + _internal_combustion_engine_control_pub.publish(ice_control); + + internal_combustion_engine_status_s ice_status; + ice_status.state = static_cast(_state); + ice_status.timestamp = now; + _internal_combustion_engine_status_pub.publish(ice_status); +} + +bool InternalCombustionEngineControl::isEngineRunning(const hrt_abstime now) +{ + rpm_s rpm; + + if (_rpm_sub.copy(&rpm)) { + const hrt_abstime rpm_timestamp = rpm.timestamp; + + return (_param_ice_min_run_rpm.get() > FLT_EPSILON && (now < rpm_timestamp + 2_s) + && rpm.rpm_estimate > _param_ice_min_run_rpm.get()); + } + + return false; +} + +void InternalCombustionEngineControl::controlEngineRunning(float throttle_in) +{ + _ignition_on = true; + _choke_control = 0.f; + _starter_engine_control = 0.f; + _throttle_control = throttle_in; + +} + +void InternalCombustionEngineControl::controlEngineStop() +{ + _ignition_on = false; + _choke_control = _param_ice_stop_choke.get() ? 1.f : 0.f; + _starter_engine_control = 0.f; + _throttle_control = 0.f; +} + +void InternalCombustionEngineControl::controlEngineFault() +{ + _ignition_on = false; + _choke_control = _param_ice_stop_choke.get() ? 1.f : 0.f; + _starter_engine_control = 0.f; + _throttle_control = 0.f; +} + +void InternalCombustionEngineControl::controlEngineStartup(const hrt_abstime now) +{ + float ignition_delay = 0.f; + float choke_duration = 0.f; + const float starter_duration = _param_ice_strt_dur.get(); + + if (_starting_retry_cycle == 0) { + ignition_delay = math::max(_param_ice_ign_delay.get(), 0.f); + + if (_param_ice_choke_st_dur.get() > FLT_EPSILON) { + choke_duration = _param_ice_choke_st_dur.get(); + } + } + + _ignition_on = true; + _throttle_control = _param_ice_strt_thr.get(); + _choke_control = now < _state_start_time + (choke_duration + ignition_delay) * 1_s ? 1.f : 0.f; + _starter_engine_control = now > _state_start_time + (ignition_delay * 1_s) ? 1.f : 0.f; + const hrt_abstime cycle_timeout_duration = (ignition_delay + choke_duration + starter_duration) * 1_s; + + if (now > _state_start_time + cycle_timeout_duration) { + // start resting timer if engine is not running + _starting_rest_time = now; + _starting_retry_cycle++; + PX4_INFO("ICE: starting attempt %i finished", _starting_retry_cycle); + } +} + +bool InternalCombustionEngineControl::isStartingPermitted(const hrt_abstime now) +{ + return now > _starting_rest_time + DELAY_BEFORE_RESTARTING * 1_s; +} + +bool InternalCombustionEngineControl::maximumAttemptsReached() +{ + // First and only attempt + if (_param_ice_strt_attempts.get() == 0) { + return _starting_retry_cycle > 0; + } + + return _starting_retry_cycle >= _param_ice_strt_attempts.get(); +} + +int InternalCombustionEngineControl::print_usage(const char *reason) +{ + if (reason) { + PX4_ERR("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +The module controls internal combustion engine (ICE) features including: +ignition (on/off),throttle and choke level, starter engine delay, and user request. +The module publishes [InternalCombustionEngineControl.msg](../msg_docs/InternalCombustionEngineControl.md). +The architecture is as shown below.: +![Architecture](../../assets/diagrams/ice_control_diagram.png) + +### Enabling +This feature is not enabled by default needs to be configured in the +build target for your board together with the rpm capture driver: +CONFIG_MODULES_INTERNAL_COMBUSTION_ENGINE_CONTROL=y +CONFIG_DRIVERS_RPM_CAPTURE=y + +Additionally, to enable the module: +- set [ICE_EN](https://docs.px4.io/main/en/advanced_config/parameter_reference.html#ICE_EN) +to true and adjust the other module parameters ICE_ according to your needs. +- set [RPM_CAP_ENABLE](https://docs.px4.io/main/en/advanced_config/parameter_reference.html#RPM_CAP_ENABLE) to true. + +### Implementation +The ICE is implemented with a (4) state machine: +![Architecture](../../assets/diagrams/ice_control_state_machine.png) +The state machine: +- checks if [Rpm.msg](../msg_docs/Rpm.md) is updated to know if the engine is running +- allows for user inputs from +- AUX{N} +- Arming state in [VehicleStatus.msg(../msg_docs/VehicleStatus.md) + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("internal_combustion_engine_control", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + return 0; +} + +extern "C" __EXPORT int internal_combustion_engine_control_main(int argc, char *argv[]) +{ + return InternalCombustionEngineControl::main(argc, argv); +} + +} // namespace internal_combustion_engine_control diff --git a/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.hpp b/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.hpp new file mode 100644 index 0000000000..01884e4967 --- /dev/null +++ b/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.hpp @@ -0,0 +1,159 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace internal_combustion_engine_control +{ + +class InternalCombustionEngineControl : public ModuleBase, public ModuleParams, + public px4::ScheduledWorkItem +{ +public: + InternalCombustionEngineControl(); + ~InternalCombustionEngineControl() override; + + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]) + { + return print_usage("unknown command"); + } + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + void start(); + +private: + void Run() override; + + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _rpm_sub{ORB_ID(rpm)}; + uORB::Subscription _actuator_motors{ORB_ID(actuator_motors)}; + + uORB::Publication _internal_combustion_engine_control_pub{ORB_ID(internal_combustion_engine_control)}; + uORB::Publication _internal_combustion_engine_status_pub{ORB_ID(internal_combustion_engine_status)}; + + + // has to mirror internal_combustion_engine_status_s::State + enum class State { + Stopped, + Starting, + Running, + Fault + } _state{State::Stopped}; + + enum class SubState { + Run, + Rest + }; + + enum class UserOnOffRequest { + Off, + On + }; + + enum class ICESource { + ArmingState, + Aux1, + Aux2, + }; + + hrt_abstime _state_start_time{0}; + hrt_abstime _last_time_run{0}; + + bool _ignition_on{false}; + float _choke_control{1.f}; + float _starter_engine_control{0.f}; + float _throttle_control{0.f}; + + SlewRate _throttle_control_slew_rate; + + bool isEngineRunning(const hrt_abstime now); + void controlEngineRunning(float throttle_in); + void controlEngineStop(); + void controlEngineStartup(const hrt_abstime now); + void controlEngineFault(); + bool maximumAttemptsReached(); + void publishControl(const hrt_abstime now, const UserOnOffRequest user_request); + + // Starting state specifics + static constexpr float DELAY_BEFORE_RESTARTING{1.f}; + int _starting_retry_cycle{0}; + hrt_abstime _starting_rest_time{0}; + SubState _starting_sub_state{SubState::Run}; + + /** + * @brief Currently the ICE starting state is permitted after resting + * DELAY_BEFORE_RESTARTING s to reduce wear on the starter motor. + * @param now current time + * @return if true, otherwise false + */ + bool isStartingPermitted(const hrt_abstime now); + + DEFINE_PARAMETERS( + (ParamInt) _param_ice_on_source, + (ParamFloat) _param_ice_choke_st_dur, + (ParamFloat) _param_ice_strt_dur, + (ParamFloat) _param_ice_min_run_rpm, + (ParamInt) _param_ice_strt_attempts, + (ParamInt) _param_ice_running_fault_detection, + (ParamFloat) _param_ice_strt_thr, + (ParamInt) _param_ice_stop_choke, + (ParamFloat) _param_ice_thr_slew, + (ParamFloat) _param_ice_ign_delay + ) +}; + +} // namespace internal_combustion_engine_control diff --git a/src/modules/internal_combustion_engine_control/Kconfig b/src/modules/internal_combustion_engine_control/Kconfig new file mode 100644 index 0000000000..c0d0e9049f --- /dev/null +++ b/src/modules/internal_combustion_engine_control/Kconfig @@ -0,0 +1,5 @@ +menuconfig MODULES_INTERNAL_COMBUSTION_ENGINE_CONTROL + bool "internal_combustion_engine_control" + default n + ---help--- + Enable support for internal_combustion_engine_control diff --git a/src/modules/internal_combustion_engine_control/module.yaml b/src/modules/internal_combustion_engine_control/module.yaml new file mode 100644 index 0000000000..517fc89fee --- /dev/null +++ b/src/modules/internal_combustion_engine_control/module.yaml @@ -0,0 +1,120 @@ +module_name: Internal Combustion Engine Control + +parameters: + - group: ICE + definitions: + + ICE_EN: + description: + short: Enable internal combustion engine + type: boolean + default: true + + ICE_ON_SOURCE: + description: + short: Engine start/stop input source + type: enum + default: 0 + values: + 0: On arming - disarming + 1: Aux1 + 2: Aux2 + + ICE_CHOKE_ST_DUR: + description: + short: Duration of choking during startup + type: float + unit: s + min: 0 + max: 10 + decimal: 1 + increment: 0.1 + default: 5 + + ICE_STRT_DUR: + description: + short: Duration of single starting attempt (excl. choking) + long: | + Maximum expected time for startup before declaring timeout. + type: float + unit: s + min: 0 + max: 10 + decimal: 1 + increment: 0.1 + default: 5 + + ICE_MIN_RUN_RPM: + description: + short: Minimum RPM for engine to be declared running + type: float + unit: rpm + min: 0 + max: 10000 + decimal: 0 + increment: 1 + default: 2000 + + ICE_STRT_ATTEMPT: + description: + short: Number attempts for starting the engine + long: | + Number of accepted attempts for starting the engine before declaring a fault. + type: int32 + min: 0 + max: 10 + default: 3 + + ICE_RUN_FAULT_D: + description: + short: Fault detection if it stops in running state + long: | + Enables restart if a fault is detected during the running state. Otherwise + commands continues in running state until given an user request off. + type: boolean + default: true + + ICE_STRT_THR: + description: + short: Throttle value for starting engine + long: | + During the choking and the starting phase, the throttle value is set to this value. + type: float + unit: norm + min: 0 + max: 1 + decimal: 0 + increment: 0.01 + default: 0.1 + + ICE_STOP_CHOKE: + description: + short: Apply choke when stopping engine + type: boolean + default: true + + ICE_THR_SLEW: + description: + short: Throttle slew rate + long: | + Maximum rate of change of throttle value per second. + type: float + unit: 1/s + min: 0 + max: 1 + decimal: 2 + increment: 0.01 + default: 0.5 + + ICE_IGN_DELAY: + description: + short: Cold-start delay after ignition before engaging starter + long: | + In case that the ignition takes a moment to be up and running, this parameter can be set to account for that. + type: float + unit: s + min: 0 + max: 10 + decimal: 1 + increment: 0.1 + default: 0 diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 3114d83bb1..4882061d1d 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -79,6 +79,7 @@ void LoggedTopics::add_default_topics() add_topic("home_position"); add_topic("hover_thrust_estimate", 100); add_topic("input_rc", 500); + add_optional_topic("internal_combustion_engine_control", 10); add_optional_topic("internal_combustion_engine_status", 10); add_optional_topic("iridiumsbd_status", 1000); add_optional_topic("irlock_report", 1000); From 04a3c4af20df9e851e281776bfa51d02759ddee6 Mon Sep 17 00:00:00 2001 From: chfriedrich98 <125505139+chfriedrich98@users.noreply.github.com> Date: Tue, 18 Feb 2025 18:26:38 +0100 Subject: [PATCH 36/91] Differential Rover: Refactor and clean up, align with Ackermann rover(#24318) * differential: refactor code architecture * Offboard fix * fix accel/decel slew rate --- .../init.d-posix/airframes/4009_gz_r1_rover | 44 +- .../airframes/50001_aion_robotics_r1_rover | 43 +- msg/CMakeLists.txt | 3 - msg/RoverDifferentialGuidanceStatus.msg | 7 - msg/RoverDifferentialSetpoint.msg | 9 - msg/RoverDifferentialStatus.msg | 14 - src/lib/rover_control/RoverControl.cpp | 89 +++- src/lib/rover_control/RoverControl.hpp | 19 + src/lib/rover_control/module.yaml | 18 +- src/modules/logger/logged_topics.cpp | 3 - src/modules/rover_ackermann/Kconfig | 2 +- src/modules/rover_differential/CMakeLists.txt | 14 +- .../CMakeLists.txt | 10 +- .../DifferentialAttControl.cpp | 208 +++++++++ .../DifferentialAttControl.hpp | 143 ++++++ .../CMakeLists.txt | 9 +- .../DifferentialPosVelControl.cpp | 406 ++++++++++++++++++ .../DifferentialPosVelControl.hpp | 243 +++++++++++ .../DifferentialRateControl/CMakeLists.txt | 39 ++ .../DifferentialRateControl.cpp | 190 ++++++++ .../DifferentialRateControl.hpp | 144 +++++++ src/modules/rover_differential/Kconfig | 3 +- .../rover_differential/RoverDifferential.cpp | 283 ++++-------- .../rover_differential/RoverDifferential.hpp | 119 ++--- .../RoverDifferentialControl.cpp | 277 ------------ .../RoverDifferentialControl.hpp | 169 -------- .../RoverDifferentialGuidance.cpp | 234 ---------- .../RoverDifferentialGuidance.hpp | 151 ------- src/modules/rover_differential/module.yaml | 191 +------- 29 files changed, 1722 insertions(+), 1362 deletions(-) delete mode 100644 msg/RoverDifferentialGuidanceStatus.msg delete mode 100644 msg/RoverDifferentialSetpoint.msg delete mode 100644 msg/RoverDifferentialStatus.msg rename src/modules/rover_differential/{RoverDifferentialControl => DifferentialAttControl}/CMakeLists.txt (85%) create mode 100644 src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp create mode 100644 src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp rename src/modules/rover_differential/{RoverDifferentialGuidance => DifferentialPosVelControl}/CMakeLists.txt (86%) create mode 100644 src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.cpp create mode 100644 src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.hpp create mode 100644 src/modules/rover_differential/DifferentialRateControl/CMakeLists.txt create mode 100644 src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp create mode 100644 src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp delete mode 100644 src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp delete mode 100644 src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp delete mode 100644 src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp delete mode 100644 src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover index 3f96dad5f8..f05bbdcf45 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover @@ -11,29 +11,39 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=r1_rover} param set-default SIM_GZ_EN 1 # Gazebo bridge -# Rover parameters +param set-default NAV_ACC_RAD 0.5 + +# Differential Parameters param set-default RD_WHEEL_TRACK 0.3 -param set-default RD_MAX_ACCEL 5 -param set-default RD_MAX_DECEL 10 -param set-default RD_MAX_JERK 30 param set-default RD_MAX_THR_YAW_R 1.5 -param set-default RD_YAW_RATE_P 0.25 -param set-default RD_YAW_RATE_I 0.01 -param set-default RD_YAW_P 5 -param set-default RD_YAW_I 0.1 -param set-default RD_MAX_SPEED 2 -param set-default RD_MAX_THR_SPD 2.15 -param set-default RD_SPEED_P 0.1 -param set-default RD_SPEED_I 0.01 -param set-default RD_MAX_YAW_RATE 180 param set-default RD_TRANS_DRV_TRN 0.349066 param set-default RD_TRANS_TRN_DRV 0.174533 -param set-default RD_MAX_YAW_ACCEL 1000 -# Pure pursuit parameters -param set-default PP_LOOKAHD_MAX 30 -param set-default PP_LOOKAHD_MIN 2 +# Rover Control Parameters +param set-default RO_ACCEL_LIM 5 +param set-default RO_DECEL_LIM 10 +param set-default RO_JERK_LIM 30 +param set-default RO_MAX_THR_SPEED 2.1 + +# Rover Rate Control Parameters +param set-default RO_YAW_RATE_I 0.01 +param set-default RO_YAW_RATE_P 0.25 +param set-default RO_YAW_RATE_LIM 180 +param set-default RO_YAW_ACCEL_LIM 120 +param set-default RO_YAW_DECEL_LIM 1000 + +# Rover Attitude Control Parameters +param set-default RO_YAW_P 5 + +# Rover Position Control Parameters +param set-default RO_SPEED_LIM 2 +param set-default RO_SPEED_I 0.5 +param set-default RO_SPEED_P 1 + +# Pure Pursuit parameters param set-default PP_LOOKAHD_GAIN 1 +param set-default PP_LOOKAHD_MAX 10 +param set-default PP_LOOKAHD_MIN 1 # Simulated sensors param set-default SENS_EN_GPSSIM 1 diff --git a/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover b/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover index 57a3644dae..45b35ffeaa 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover +++ b/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover @@ -21,26 +21,37 @@ param set-default RBCLW_FUNC1 101 param set-default RBCLW_FUNC2 102 param set-default RBCLW_REV 1 # reverse right wheels -# Rover parameters + +param set-default NAV_ACC_RAD 0.5 + +# Differential Parameters param set-default RD_WHEEL_TRACK 0.3 -param set-default RD_MAX_ACCEL 3 -param set-default RD_MAX_DECEL 4 -param set-default RD_MAX_JERK 5 -param set-default RD_MAX_SPEED 1.6 -param set-default RD_MAX_THR_SPD 1.9 param set-default RD_MAX_THR_YAW_R 0.7 -param set-default RD_MAX_YAW_ACCEL 600 -param set-default RD_MAX_YAW_RATE 250 -param set-default RD_SPEED_P 0.1 -param set-default RD_SPEED_I 0.01 param set-default RD_TRANS_DRV_TRN 0.785398 param set-default RD_TRANS_TRN_DRV 0.139626 -param set-default RD_YAW_P 5 -param set-default RD_YAW_I 0.1 -param set-default RD_YAW_RATE_P 0.1 -param set-default RD_YAW_RATE_I 0.01 -# Pure pursuit parameters +# Rover Control Parameters +param set-default RO_ACCEL_LIM 3 +param set-default RO_DECEL_LIM 4 +param set-default RO_JERK_LIM 5 +param set-default RO_MAX_THR_SPEED 1.9 + +# Rover Rate Control Parameters +param set-default RO_YAW_RATE_I 0.01 +param set-default RO_YAW_RATE_P 0.1 +param set-default RO_YAW_RATE_LIM 250 +param set-default RO_YAW_ACCEL_LIM 600 +param set-default RO_YAW_DECEL_LIM 600 + +# Rover Attitude Control Parameters +param set-default RO_YAW_P 5 + +# Rover Position Control Parameters +param set-default RO_SPEED_LIM 1.6 +param set-default RO_SPEED_I 0.01 +param set-default RO_SPEED_P 0.1 + +# Pure Pursuit parameters +param set-default PP_LOOKAHD_GAIN 1 param set-default PP_LOOKAHD_MAX 10 param set-default PP_LOOKAHD_MIN 1 -param set-default PP_LOOKAHD_GAIN 1 diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 5844caafec..ac59212f40 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -179,9 +179,6 @@ set(msg_files RoverRateStatus.msg RoverSteeringSetpoint.msg RoverThrottleSetpoint.msg - RoverDifferentialGuidanceStatus.msg - RoverDifferentialSetpoint.msg - RoverDifferentialStatus.msg RoverMecanumGuidanceStatus.msg RoverMecanumSetpoint.msg RoverMecanumStatus.msg diff --git a/msg/RoverDifferentialGuidanceStatus.msg b/msg/RoverDifferentialGuidanceStatus.msg deleted file mode 100644 index ce3d375111..0000000000 --- a/msg/RoverDifferentialGuidanceStatus.msg +++ /dev/null @@ -1,7 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller -float32 heading_error_deg # [deg] Heading error of the pure pursuit controller -uint8 state_machine # Driving state of the rover [0: SPOT_TURNING, 1: DRIVING, 2: GOAL_REACHED] - -# TOPICS rover_differential_guidance_status diff --git a/msg/RoverDifferentialSetpoint.msg b/msg/RoverDifferentialSetpoint.msg deleted file mode 100644 index 100cc6b3cd..0000000000 --- a/msg/RoverDifferentialSetpoint.msg +++ /dev/null @@ -1,9 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 forward_speed_setpoint # [m/s] Desired forward speed for the rover -float32 forward_speed_setpoint_normalized # [-1, 1] Normalized forward speed for the rover -float32 yaw_rate_setpoint # [rad/s] Desired yaw rate for the rover (Overriden by yaw controller if yaw_setpoint is used) -float32 speed_diff_setpoint_normalized # [-1, 1] Normalized speed difference between the left and right wheels -float32 yaw_setpoint # [rad] Desired yaw (heading) for the rover - -# TOPICS rover_differential_setpoint diff --git a/msg/RoverDifferentialStatus.msg b/msg/RoverDifferentialStatus.msg deleted file mode 100644 index 767474fb5b..0000000000 --- a/msg/RoverDifferentialStatus.msg +++ /dev/null @@ -1,14 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 measured_forward_speed # [m/s] Measured speed in body x direction. Forwards: positiv, Backwards: negativ -float32 adjusted_forward_speed_setpoint # [m/s] Speed setpoint after applying slew rate -float32 measured_yaw # [rad] Measured yaw -float32 adjusted_yaw_setpoint # [rad] Yaw setpoint after applying slew rate -float32 clyaw_yaw_rate_setpoint # [rad/s] Yaw rate setpoint output by the closed loop yaw controller -float32 measured_yaw_rate # [rad/s] Measured yaw rate -float32 adjusted_yaw_rate_setpoint # [rad/s] Yaw rate setpoint from the closed loop yaw controller -float32 pid_yaw_integral # Integral of the PID for the closed loop yaw controller -float32 pid_yaw_rate_integral # Integral of the PID for the closed loop yaw rate controller -float32 pid_throttle_integral # Integral of the PID for the closed loop speed controller - -# TOPICS rover_differential_status diff --git a/src/lib/rover_control/RoverControl.cpp b/src/lib/rover_control/RoverControl.cpp index 7b0864a428..0eafcaa9f8 100644 --- a/src/lib/rover_control/RoverControl.cpp +++ b/src/lib/rover_control/RoverControl.cpp @@ -48,26 +48,24 @@ float throttleControl(SlewRate &motor_setpoint, const float throttle_setp if (accelerating && max_accel > FLT_EPSILON && max_thr_spd > FLT_EPSILON) { // Acceleration slew rate motor_setpoint.setSlewRate(max_accel / max_thr_spd); + motor_setpoint.update(throttle_setpoint, dt); // Reinitialize slew rate if current value is closer to setpoint than post slew rate value if (fabsf(motor_setpoint.getState() - current_motor_setpoint) > fabsf(throttle_setpoint - current_motor_setpoint)) { - motor_setpoint.setForcedValue(current_motor_setpoint); + motor_setpoint.setForcedValue(throttle_setpoint); } - motor_setpoint.update(throttle_setpoint, dt); - } else if (!accelerating && max_decel > FLT_EPSILON && max_thr_spd > FLT_EPSILON) { // Deceleration slew rate motor_setpoint.setSlewRate(max_decel / max_thr_spd); + motor_setpoint.update(throttle_setpoint, dt); // Reinitialize slew rate if current value is closer to setpoint than post slew rate value if (fabsf(motor_setpoint.getState() - current_motor_setpoint) > fabsf(throttle_setpoint - current_motor_setpoint)) { - motor_setpoint.setForcedValue(current_motor_setpoint); + motor_setpoint.setForcedValue(throttle_setpoint); } - motor_setpoint.update(throttle_setpoint, dt); - } else { // Fallthrough if slew rate parameters are not configured motor_setpoint.setForcedValue(throttle_setpoint); } @@ -88,14 +86,13 @@ float attitudeControl(SlewRateYaw &adjusted_yaw_setpoint, PID &pid_yaw, if (yaw_slew_rate > FLT_EPSILON) { // Apply slew rate if configured adjusted_yaw_setpoint.setSlewRate(yaw_slew_rate); + adjusted_yaw_setpoint.update(yaw_setpoint, dt); // Reinitialize slew rate if current value is closer to setpoint than post slew rate value if (fabsf(wrap_pi(adjusted_yaw_setpoint.getState() - vehicle_yaw)) > fabsf(wrap_pi(yaw_setpoint - vehicle_yaw))) { - adjusted_yaw_setpoint.setForcedValue(vehicle_yaw); + adjusted_yaw_setpoint.setForcedValue(yaw_setpoint); } - adjusted_yaw_setpoint.update(yaw_setpoint, dt); - } else { adjusted_yaw_setpoint.setForcedValue(yaw_setpoint); } @@ -118,26 +115,24 @@ float speedControl(SlewRate &speed_with_rate_limit, PID &pid_speed, const // Apply acceleration and deceleration limit if (fabsf(speed_setpoint) >= fabsf(vehicle_speed) && max_accel > FLT_EPSILON) { speed_with_rate_limit.setSlewRate(max_accel); + speed_with_rate_limit.update(speed_setpoint, dt); // Reinitialize slew rate if current value is closer to setpoint than post slew rate value if (fabsf(speed_with_rate_limit.getState() - vehicle_speed) > fabsf( speed_setpoint - vehicle_speed)) { - speed_with_rate_limit.setForcedValue(vehicle_speed); + speed_with_rate_limit.setForcedValue(speed_setpoint); } - speed_with_rate_limit.update(speed_setpoint, dt); - } else if (fabsf(speed_setpoint) < fabsf(vehicle_speed) && max_decel > FLT_EPSILON) { speed_with_rate_limit.setSlewRate(max_decel); + speed_with_rate_limit.update(speed_setpoint, dt); // Reinitialize slew rate if current value is closer to setpoint than post slew rate value if (fabsf(speed_with_rate_limit.getState() - vehicle_speed) > fabsf( speed_setpoint - vehicle_speed)) { - speed_with_rate_limit.setForcedValue(vehicle_speed); + speed_with_rate_limit.setForcedValue(speed_setpoint); } - speed_with_rate_limit.update(speed_setpoint, dt); - } else { // Fallthrough if slew rate is not configured speed_with_rate_limit.setForcedValue(speed_setpoint); } @@ -153,12 +148,72 @@ float speedControl(SlewRate &speed_with_rate_limit, PID &pid_speed, const } // Feedback control - pid_speed.setSetpoint(speed_with_rate_limit.getState()); - forward_speed_normalized += pid_speed.update(vehicle_speed, dt); + if (fabsf(speed_with_rate_limit.getState()) > FLT_EPSILON) { + pid_speed.setSetpoint(speed_with_rate_limit.getState()); + forward_speed_normalized += pid_speed.update(vehicle_speed, dt); + + } else { + pid_speed.resetIntegral(); + } + return math::constrain(forward_speed_normalized, -1.f, 1.f); } +float rateControl(SlewRate &adjusted_yaw_rate_setpoint, PID &pid_yaw_rate, const float yaw_rate_setpoint, + const float vehicle_yaw_rate, const float max_thr_yaw_r, const float max_yaw_accel, const float max_yaw_decel, + const float wheel_track, const float dt) +{ + // Apply acceleration and deceleration limit + if (fabsf(yaw_rate_setpoint) >= fabsf(vehicle_yaw_rate) && max_yaw_accel > FLT_EPSILON) { + adjusted_yaw_rate_setpoint.setSlewRate(max_yaw_accel); + adjusted_yaw_rate_setpoint.update(yaw_rate_setpoint, dt); + + // Reinitialize slew rate if current value is closer to setpoint than post slew rate value + if (fabsf(adjusted_yaw_rate_setpoint.getState() - vehicle_yaw_rate) > fabsf( + yaw_rate_setpoint - vehicle_yaw_rate)) { + adjusted_yaw_rate_setpoint.setForcedValue(yaw_rate_setpoint); + } + + + } else if (fabsf(yaw_rate_setpoint) < fabsf(vehicle_yaw_rate) && max_yaw_decel > FLT_EPSILON) { + adjusted_yaw_rate_setpoint.setSlewRate(max_yaw_decel); + adjusted_yaw_rate_setpoint.update(yaw_rate_setpoint, dt); + + // Reinitialize slew rate if current value is closer to setpoint than post slew rate value + if (fabsf(adjusted_yaw_rate_setpoint.getState() - vehicle_yaw_rate) > fabsf( + yaw_rate_setpoint - vehicle_yaw_rate)) { + adjusted_yaw_rate_setpoint.setForcedValue(yaw_rate_setpoint); + } + + + } else { // Fallthrough if slew rate is not configured + adjusted_yaw_rate_setpoint.setForcedValue(yaw_rate_setpoint); + } + + // Transform yaw rate into speed difference + float speed_diff_normalized{0.f}; + + if (wheel_track > FLT_EPSILON && max_thr_yaw_r > FLT_EPSILON) { // Feedforward + const float speed_diff = adjusted_yaw_rate_setpoint.getState() * wheel_track / + 2.f; + speed_diff_normalized = math::interpolate(speed_diff, -max_thr_yaw_r, + max_thr_yaw_r, -1.f, 1.f); + } + + // Feedback control + if (fabsf(adjusted_yaw_rate_setpoint.getState()) > FLT_EPSILON) { + pid_yaw_rate.setSetpoint(adjusted_yaw_rate_setpoint.getState()); + speed_diff_normalized += pid_yaw_rate.update(vehicle_yaw_rate, dt); + + } else { + pid_yaw_rate.resetIntegral(); + } + + + return math::constrain(speed_diff_normalized, -1.f, 1.f); +} + void globalToLocalSetpointTriplet(Vector2f &curr_wp_ned, Vector2f &prev_wp_ned, Vector2f &next_wp_ned, position_setpoint_triplet_s position_setpoint_triplet, Vector2f &curr_pos_ned, Vector2d &home_pos, MapProjection &global_ned_proj_ref) diff --git a/src/lib/rover_control/RoverControl.hpp b/src/lib/rover_control/RoverControl.hpp index 7f76710225..1196e04650 100644 --- a/src/lib/rover_control/RoverControl.hpp +++ b/src/lib/rover_control/RoverControl.hpp @@ -92,6 +92,25 @@ float attitudeControl(SlewRateYaw &adjusted_yaw_setpoint, PID &pid_yaw, f float speedControl(SlewRate &speed_with_rate_limit, PID &pid_speed, float speed_setpoint, float vehicle_speed, float max_accel, float max_decel, float max_thr_speed, float dt); +/** + * Applies yaw acceleration slew rate to a yaw rate setpoint and calculates the necessary speed diff setpoint + * using a feedforward term and/or a PID controller. + * Note: This function is only for rovers that control the rate through a speed difference between the left/right wheels. + * @param adjusted_yaw_rate_setpoint Yaw rate setpoint with applied slew rate [-1, 1] (Updated by this function). + * @param pid_yaw_rate Yaw rate PID (Updated by this function). + * @param yaw_rate_setpoint Yaw rate setpoint [rad/s]. + * @param vehicle_yaw_rate Measured vehicle yaw rate [rad/s]. + * @param max_thr_yaw_r Yaw rate turning left/right wheels at max speed in opposite directions [m/s]. + * @param max_yaw_accel Maximum allowed yaw acceleration [rad/s^2]. + * @param max_yaw_decel Maximum allowed yaw deceleration [rad/s^2]. + * @param wheel_track Distance from the center of the right wheel to the center of the left wheel [m]. + * @param dt Time since last update [s]. + * @return Normalized speed difference setpoint [-1, 1]. + */ +float rateControl(SlewRate &adjusted_yaw_rate_setpoint, PID &pid_yaw_rate, + float yaw_rate_setpoint, float vehicle_yaw_rate, float max_thr_yaw_r, float max_yaw_accel, float max_yaw_decel, + float wheel_track, float dt); + /** * Projects positionSetpointTriplet waypoints from global to ned frame. * @param curr_wp_ned Current waypoint in NED frame (Updated by this function) diff --git a/src/lib/rover_control/module.yaml b/src/lib/rover_control/module.yaml index 8450a03b06..a786a32c4b 100644 --- a/src/lib/rover_control/module.yaml +++ b/src/lib/rover_control/module.yaml @@ -133,7 +133,23 @@ parameters: RO_YAW_ACCEL_LIM: description: short: Yaw acceleration limit - long: Set to -1 to disable. + long: | + Used to cap how quickly the magnitude of yaw rate setpoints can increase. + Set to -1 to disable. + type: float + unit: deg/s^2 + min: -1 + max: 10000 + increment: 0.01 + decimal: 2 + default: -1 + + RO_YAW_DECEL_LIM: + description: + short: Yaw deceleration limit + long: | + Used to cap how quickly the magnitude of yaw rate setpoints can decrease. + Set to -1 to disable. type: float unit: deg/s^2 min: -1 diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 4882061d1d..b98d185aff 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -110,9 +110,6 @@ void LoggedTopics::add_default_topics() add_optional_topic("rover_rate_status", 100); add_optional_topic("rover_steering_setpoint", 100); add_optional_topic("rover_throttle_setpoint", 100); - add_optional_topic("rover_differential_guidance_status", 100); - add_optional_topic("rover_differential_setpoint", 100); - add_optional_topic("rover_differential_status", 100); add_optional_topic("rover_mecanum_guidance_status", 100); add_optional_topic("rover_mecanum_setpoint", 100); add_optional_topic("rover_mecanum_status", 100); diff --git a/src/modules/rover_ackermann/Kconfig b/src/modules/rover_ackermann/Kconfig index a6acd00251..bfcff4a895 100644 --- a/src/modules/rover_ackermann/Kconfig +++ b/src/modules/rover_ackermann/Kconfig @@ -2,4 +2,4 @@ menuconfig MODULES_ROVER_ACKERMANN bool "rover_ackermann" default n ---help--- - Enable support for rover_ackermann + Enable support for ackermann rovers diff --git a/src/modules/rover_differential/CMakeLists.txt b/src/modules/rover_differential/CMakeLists.txt index 8805893160..8fafc30bab 100644 --- a/src/modules/rover_differential/CMakeLists.txt +++ b/src/modules/rover_differential/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2024 PX4 Development Team. All rights reserved. +# Copyright (c) 2025 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -31,8 +31,9 @@ # ############################################################################ -add_subdirectory(RoverDifferentialGuidance) -add_subdirectory(RoverDifferentialControl) +add_subdirectory(DifferentialRateControl) +add_subdirectory(DifferentialAttControl) +add_subdirectory(DifferentialPosVelControl) px4_add_module( MODULE modules__rover_differential @@ -41,10 +42,11 @@ px4_add_module( RoverDifferential.cpp RoverDifferential.hpp DEPENDS - RoverDifferentialGuidance - RoverDifferentialControl + DifferentialRateControl + DifferentialAttControl + DifferentialPosVelControl px4_work_queue - pure_pursuit + rover_control MODULE_CONFIG module.yaml ) diff --git a/src/modules/rover_differential/RoverDifferentialControl/CMakeLists.txt b/src/modules/rover_differential/DifferentialAttControl/CMakeLists.txt similarity index 85% rename from src/modules/rover_differential/RoverDifferentialControl/CMakeLists.txt rename to src/modules/rover_differential/DifferentialAttControl/CMakeLists.txt index 7464de4f5b..456b21489d 100644 --- a/src/modules/rover_differential/RoverDifferentialControl/CMakeLists.txt +++ b/src/modules/rover_differential/DifferentialAttControl/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2024 PX4 Development Team. All rights reserved. +# Copyright (c) 2025 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -31,9 +31,9 @@ # ############################################################################ -px4_add_library(RoverDifferentialControl - RoverDifferentialControl.cpp +px4_add_library(DifferentialAttControl + DifferentialAttControl.cpp ) -target_link_libraries(RoverDifferentialControl PUBLIC PID) -target_include_directories(RoverDifferentialControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(DifferentialAttControl PUBLIC PID) +target_include_directories(DifferentialAttControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp new file mode 100644 index 0000000000..a36ed41ada --- /dev/null +++ b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.cpp @@ -0,0 +1,208 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "DifferentialAttControl.hpp" + +using namespace time_literals; + +DifferentialAttControl::DifferentialAttControl(ModuleParams *parent) : ModuleParams(parent) +{ + _rover_rate_setpoint_pub.advertise(); + _rover_throttle_setpoint_pub.advertise(); + _rover_attitude_setpoint_pub.advertise(); + _rover_attitude_status_pub.advertise(); + updateParams(); +} + +void DifferentialAttControl::updateParams() +{ + ModuleParams::updateParams(); + + if (_param_ro_yaw_rate_limit.get() > FLT_EPSILON) { + _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + } + + _pid_yaw.setGains(_param_ro_yaw_p.get(), 0.f, 0.f); + _pid_yaw.setIntegralLimit(_max_yaw_rate); + _pid_yaw.setOutputLimit(_max_yaw_rate); + _adjusted_yaw_setpoint.setSlewRate(_max_yaw_rate); +} + +void DifferentialAttControl::updateAttControl() +{ + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); + } + + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); + matrix::Quatf vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); + _vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi(); + } + + if (_vehicle_control_mode.flag_control_attitude_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) { + + if (_vehicle_control_mode.flag_control_manual_enabled || _vehicle_control_mode.flag_control_offboard_enabled) { + generateAttitudeSetpoint(); + } + + generateRateSetpoint(); + + } else { // Reset pid and slew rate when attitude control is not active + _pid_yaw.resetIntegral(); + _adjusted_yaw_setpoint.setForcedValue(0.f); + } + + // Publish attitude controller status (logging only) + rover_attitude_status_s rover_attitude_status; + rover_attitude_status.timestamp = _timestamp; + rover_attitude_status.measured_yaw = _vehicle_yaw; + rover_attitude_status.adjusted_yaw_setpoint = _adjusted_yaw_setpoint.getState(); + _rover_attitude_status_pub.publish(rover_attitude_status); + +} + +void DifferentialAttControl::generateAttitudeSetpoint() +{ + const bool stab_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled + && !_vehicle_control_mode.flag_control_position_enabled && _vehicle_control_mode.flag_control_attitude_enabled; + + if (stab_mode_enabled && _manual_control_setpoint_sub.updated()) { // Stab Mode + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + rover_throttle_setpoint.throttle_body_y = 0.f; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + + const float yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, + _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + + if (fabsf(yaw_rate_setpoint) > FLT_EPSILON + || fabsf(rover_throttle_setpoint.throttle_body_x) < FLT_EPSILON) { // Closed loop yaw rate control + _stab_yaw_ctl = false; + _adjusted_yaw_setpoint.setForcedValue(0.f); + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = yaw_rate_setpoint; + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + + } else { // Closed loop yaw control if the yaw rate input is zero (keep current yaw) + if (!_stab_yaw_ctl) { + _stab_yaw_setpoint = _vehicle_yaw; + _stab_yaw_ctl = true; + } + + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = _stab_yaw_setpoint; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + } + + + } + + } else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard attitude control + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + + if (_offboard_control_mode_sub.updated()) { + _offboard_control_mode_sub.copy(&_offboard_control_mode); + } + + const bool offboard_att_control = _offboard_control_mode.attitude && !_offboard_control_mode.position + && !_offboard_control_mode.velocity; + + if (offboard_att_control && PX4_ISFINITE(trajectory_setpoint.yaw)) { + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = trajectory_setpoint.yaw; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + } + } +} + +void DifferentialAttControl::generateRateSetpoint() +{ + if (_rover_attitude_setpoint_sub.updated()) { + _rover_attitude_setpoint_sub.copy(&_rover_attitude_setpoint); + } + + if (_rover_rate_setpoint_sub.updated()) { + _rover_rate_setpoint_sub.copy(&_rover_rate_setpoint); + } + + // Check if a new rate setpoint was already published from somewhere else + if (_rover_rate_setpoint.timestamp > _last_rate_setpoint_update + && _rover_rate_setpoint.timestamp > _rover_attitude_setpoint.timestamp) { + return; + } + + const float yaw_rate_setpoint = RoverControl::attitudeControl(_adjusted_yaw_setpoint, _pid_yaw, _max_yaw_rate, + _vehicle_yaw, _rover_attitude_setpoint.yaw_setpoint, _dt); + + _last_rate_setpoint_update = _timestamp; + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = math::constrain(yaw_rate_setpoint, -_max_yaw_rate, _max_yaw_rate); + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); +} + +bool DifferentialAttControl::runSanityChecks() +{ + bool ret = true; + + if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_ro_yaw_p.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("differential_att_control_conf_invalid_yaw_p"), events::Log::Error, + "Invalid configuration of necessary parameter RO_YAW_P", _param_ro_yaw_p.get()); + } + } + + _prev_param_check_passed = ret; + return ret; +} diff --git a/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp new file mode 100644 index 0000000000..e4198384c0 --- /dev/null +++ b/src/modules/rover_differential/DifferentialAttControl/DifferentialAttControl.hpp @@ -0,0 +1,143 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +// PX4 includes +#include +#include + +// Libraries +#include +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * @brief Class for differential attitude control. + */ +class DifferentialAttControl : public ModuleParams +{ +public: + /** + * @brief Constructor for DifferentialAttControl. + * @param parent The parent ModuleParams object. + */ + DifferentialAttControl(ModuleParams *parent); + ~DifferentialAttControl() = default; + + /** + * @brief Update attitude controller. + */ + void updateAttControl(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + /** + * @brief Generate and publish roverAttitudeSetpoint from manualControlSetpoint (Stab Mode) + * or trajectorySetpoint (Offboard attitude control). + */ + void generateAttitudeSetpoint(); + + /** + * @brief Generate and publish roverRateSetpoint from roverAttitudeSetpoint. + */ + void generateRateSetpoint(); + + /** + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. + */ + bool runSanityChecks(); + + // uORB subscriptions + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + uORB::Subscription _rover_attitude_setpoint_sub{ORB_ID(rover_attitude_setpoint)}; + uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)}; + vehicle_control_mode_s _vehicle_control_mode{}; + rover_attitude_setpoint_s _rover_attitude_setpoint{}; + rover_rate_setpoint_s _rover_rate_setpoint{}; + offboard_control_mode_s _offboard_control_mode{}; + + // uORB publications + uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; + uORB::Publication _rover_attitude_status_pub{ORB_ID(rover_attitude_status)}; + + // Variables + hrt_abstime _timestamp{0}; + hrt_abstime _last_rate_setpoint_update{0}; + float _vehicle_yaw{0.f}; + float _dt{0.f}; + float _max_yaw_rate{0.f}; + float _stab_yaw_setpoint{0.f}; // Yaw setpoint if rover is doing yaw control in stab mode + bool _stab_yaw_ctl{false}; // Indicates if rover is doing yaw control in stab mode + bool _prev_param_check_passed{true}; + + // Controllers + PID _pid_yaw; + SlewRateYaw _adjusted_yaw_setpoint; + + // Parameters + DEFINE_PARAMETERS( + (ParamFloat) _param_ro_yaw_rate_limit, + (ParamFloat) _param_ro_yaw_p, + (ParamFloat) _param_ro_yaw_stick_dz + ) +}; diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt b/src/modules/rover_differential/DifferentialPosVelControl/CMakeLists.txt similarity index 86% rename from src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt rename to src/modules/rover_differential/DifferentialPosVelControl/CMakeLists.txt index 81f350d484..a4a4795be7 100644 --- a/src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt +++ b/src/modules/rover_differential/DifferentialPosVelControl/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. +# Copyright (c) 2025 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -31,8 +31,9 @@ # ############################################################################ -px4_add_library(RoverDifferentialGuidance - RoverDifferentialGuidance.cpp +px4_add_library(DifferentialPosVelControl + DifferentialPosVelControl.cpp ) -target_include_directories(RoverDifferentialGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(DifferentialPosVelControl PUBLIC PID) +target_include_directories(DifferentialPosVelControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.cpp b/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.cpp new file mode 100644 index 0000000000..32532f4c56 --- /dev/null +++ b/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.cpp @@ -0,0 +1,406 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "DifferentialPosVelControl.hpp" + +using namespace time_literals; + +DifferentialPosVelControl::DifferentialPosVelControl(ModuleParams *parent) : ModuleParams(parent) +{ + _rover_rate_setpoint_pub.advertise(); + _rover_throttle_setpoint_pub.advertise(); + _rover_attitude_setpoint_pub.advertise(); + _rover_velocity_status_pub.advertise(); + updateParams(); +} + +void DifferentialPosVelControl::updateParams() +{ + ModuleParams::updateParams(); + _pid_speed.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f); + _pid_speed.setIntegralLimit(1.f); + _pid_speed.setOutputLimit(1.f); + _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + + if (_param_ro_accel_limit.get() > FLT_EPSILON) { + _speed_setpoint.setSlewRate(_param_ro_accel_limit.get()); + } +} + +void DifferentialPosVelControl::updatePosVelControl() +{ + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + updateSubscriptions(); + + if ((_vehicle_control_mode.flag_control_position_enabled || _vehicle_control_mode.flag_control_velocity_enabled) + && _vehicle_control_mode.flag_armed && runSanityChecks()) { + generateAttitudeSetpoint(); + + if (_param_ro_max_thr_speed.get() > FLT_EPSILON) { + + const float speed_body_x_setpoint_normalized = math::interpolate(_speed_body_x_setpoint, + -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f); + + if (_rover_steering_setpoint_sub.updated()) { + _rover_steering_setpoint_sub.copy(&_rover_steering_setpoint); + } + + if (fabsf(speed_body_x_setpoint_normalized) > 1.f - + fabsf(_rover_steering_setpoint.normalized_speed_diff)) { // Adjust speed setpoint if it is infeasible due to the desired speed difference of the left/right wheels + _speed_body_x_setpoint = sign(_speed_body_x_setpoint) * + math::interpolate(1.f - fabsf(_rover_steering_setpoint.normalized_speed_diff), -1.f, 1.f, + -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); + } + } + + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + _speed_body_x_setpoint = fabsf(_speed_body_x_setpoint) > _param_ro_speed_th.get() ? _speed_body_x_setpoint : 0.f; + rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_speed_setpoint, _pid_speed, + _speed_body_x_setpoint, _vehicle_speed_body_x, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), + _param_ro_max_thr_speed.get(), _dt); + rover_throttle_setpoint.throttle_body_y = 0.f; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + + } else { // Reset controller and slew rate when position control is not active + _pid_speed.resetIntegral(); + _speed_setpoint.setForcedValue(0.f); + } + + // Publish position controller status (logging only) + rover_velocity_status_s rover_velocity_status; + rover_velocity_status.timestamp = _timestamp; + rover_velocity_status.measured_speed_body_x = _vehicle_speed_body_x; + rover_velocity_status.speed_body_x_setpoint = _speed_body_x_setpoint; + rover_velocity_status.adjusted_speed_body_x_setpoint = _speed_setpoint.getState(); + rover_velocity_status.measured_speed_body_y = _vehicle_speed_body_y; + rover_velocity_status.speed_body_y_setpoint = NAN; + rover_velocity_status.adjusted_speed_body_y_setpoint = NAN; + rover_velocity_status.pid_throttle_body_x_integral = _pid_speed.getIntegral(); + rover_velocity_status.pid_throttle_body_y_integral = NAN; + _rover_velocity_status_pub.publish(rover_velocity_status); +} + +void DifferentialPosVelControl::updateSubscriptions() +{ + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); + } + + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); + _vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); + _vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi(); + } + + if (_vehicle_local_position_sub.updated()) { + vehicle_local_position_s vehicle_local_position{}; + _vehicle_local_position_sub.copy(&vehicle_local_position); + + if (!_global_ned_proj_ref.isInitialized() + || (_global_ned_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.ref_timestamp)) { + _global_ned_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon, + vehicle_local_position.ref_timestamp); + } + + _curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y); + const Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); + const Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); + _vehicle_speed_body_x = fabsf(velocity_in_body_frame(0)) > _param_ro_speed_th.get() ? velocity_in_body_frame(0) : 0.f; + _vehicle_speed_body_y = fabsf(velocity_in_body_frame(1)) > _param_ro_speed_th.get() ? velocity_in_body_frame(1) : 0.f; + } + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + _vehicle_status_sub.copy(&vehicle_status); + _nav_state = vehicle_status.nav_state; + } + +} + +void DifferentialPosVelControl::generateAttitudeSetpoint() +{ + if (_vehicle_control_mode.flag_control_manual_enabled + && _vehicle_control_mode.flag_control_position_enabled) { // Position Mode + manualPositionMode(); + + } else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard Control + if (_offboard_control_mode_sub.updated()) { + _offboard_control_mode_sub.copy(&_offboard_control_mode); + } + + if (_offboard_control_mode.position) { + offboardPositionMode(); + + } else if (_offboard_control_mode.velocity) { + offboardVelocityMode(); + } + + } else if (_vehicle_control_mode.flag_control_auto_enabled) { // Auto Mode + autoPositionMode(); + } +} + +void DifferentialPosVelControl::manualPositionMode() +{ + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + _speed_body_x_setpoint = math::interpolate(manual_control_setpoint.throttle, + -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get()); + const float yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, + _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + + if (fabsf(yaw_rate_setpoint) > FLT_EPSILON + || fabsf(_speed_body_x_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control + _course_control = false; + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = yaw_rate_setpoint; + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + + } else { // Course control if the steering input is zero (keep driving on a straight line) + if (!_course_control) { + _pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw)); + _pos_ctl_start_position_ned = _curr_pos_ned; + _course_control = true; + } + + // Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover + const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(), + 2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment(); + const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign(_speed_body_x_setpoint) * + vector_scaling * _pos_ctl_course_direction; + float yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _pos_ctl_start_position_ned, + _curr_pos_ned, fabsf(_speed_body_x_setpoint)); + yaw_setpoint = _speed_body_x_setpoint > FLT_EPSILON ? yaw_setpoint : matrix::wrap_pi(yaw_setpoint + M_PI_F); + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = yaw_setpoint; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + } + } +} + +void DifferentialPosVelControl::offboardPositionMode() +{ + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + + // Translate trajectory setpoint to rover setpoints + const Vector2f target_waypoint_ned(trajectory_setpoint.position[0], trajectory_setpoint.position[1]); + const float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm(); + + if (target_waypoint_ned.isAllFinite() && distance_to_target > _param_nav_acc_rad.get()) { + const float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(), + _param_ro_decel_limit.get(), distance_to_target, 0.f); + _speed_body_x_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get()); + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _curr_pos_ned, + _curr_pos_ned, fabsf(_speed_body_x_setpoint)); + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + + } else { + _speed_body_x_setpoint = 0.f; + } +} + +void DifferentialPosVelControl::offboardVelocityMode() +{ + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + + const Vector2f velocity_in_local_frame(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]); + + if (velocity_in_local_frame.isAllFinite()) { + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = atan2f(velocity_in_local_frame(1), velocity_in_local_frame(0)); + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + _speed_body_x_setpoint = velocity_in_local_frame.norm(); + } +} + +void DifferentialPosVelControl::autoPositionMode() +{ + updateAutoSubscriptions(); + + // Distances to waypoints + const float distance_to_curr_wp = sqrt(powf(_curr_pos_ned(0) - _curr_wp_ned(0), + 2) + powf(_curr_pos_ned(1) - _curr_wp_ned(1), 2)); + + if (_nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { // Check RTL arrival + _mission_finished = distance_to_curr_wp < _param_nav_acc_rad.get(); + } + + // State machine + float yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, + fabsf(_vehicle_speed_body_x)); + const float heading_error = matrix::wrap_pi(yaw_setpoint - _vehicle_yaw); + + if (!_mission_finished && distance_to_curr_wp > _param_nav_acc_rad.get()) { + if (_currentState == GuidanceState::STOPPED) { + _currentState = GuidanceState::DRIVING; + } + + if (_currentState == GuidanceState::DRIVING && fabsf(heading_error) > _param_rd_trans_drv_trn.get()) { + _currentState = GuidanceState::SPOT_TURNING; + + } else if (_currentState == GuidanceState::SPOT_TURNING && fabsf(heading_error) < _param_rd_trans_trn_drv.get()) { + _currentState = GuidanceState::DRIVING; + } + + } else { // Mission finished or delay command + _currentState = GuidanceState::STOPPED; + } + + // Guidance logic + switch (_currentState) { + case GuidanceState::DRIVING: { + // Calculate desired speed in body x direction + _speed_body_x_setpoint = calcSpeedSetpoint(_cruising_speed, distance_to_curr_wp, _param_ro_decel_limit.get(), + _param_ro_jerk_limit.get(), _waypoint_transition_angle, _param_ro_speed_limit.get(), _param_rd_trans_drv_trn.get(), + _param_rd_miss_spd_gain.get()); + + } break; + + case GuidanceState::SPOT_TURNING: + if (fabsf(_vehicle_speed_body_x) > 0.f) { + yaw_setpoint = _vehicle_yaw; // Wait for the rover to stop + + } + + _speed_body_x_setpoint = 0.f; + break; + + case GuidanceState::STOPPED: + default: + yaw_setpoint = _vehicle_yaw; + _speed_body_x_setpoint = 0.f; + break; + + } + + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = yaw_setpoint; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); +} + +void DifferentialPosVelControl::updateAutoSubscriptions() +{ + if (_home_position_sub.updated()) { + home_position_s home_position{}; + _home_position_sub.copy(&home_position); + _home_position = Vector2d(home_position.lat, home_position.lon); + } + + if (_position_setpoint_triplet_sub.updated()) { + position_setpoint_triplet_s position_setpoint_triplet{}; + _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); + + RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet, + _curr_pos_ned, _home_position, _global_ned_proj_ref); + + _waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned); + + // Waypoint cruising speed + _cruising_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain( + position_setpoint_triplet.current.cruising_speed, 0.f, _param_ro_speed_limit.get()) : _param_ro_speed_limit.get(); + } + + if (_mission_result_sub.updated()) { + mission_result_s mission_result{}; + _mission_result_sub.copy(&mission_result); + _mission_finished = mission_result.finished; + } +} + +float DifferentialPosVelControl::calcSpeedSetpoint(const float cruising_speed, const float distance_to_curr_wp, + const float max_decel, const float max_jerk, const float waypoint_transition_angle, const float max_speed, + const float trans_drv_trn, const float miss_spd_gain) +{ + float speed_body_x_setpoint = cruising_speed; + + if (_waypoint_transition_angle < M_PI_F - trans_drv_trn && max_jerk > FLT_EPSILON && max_decel > FLT_EPSILON) { + speed_body_x_setpoint = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_decel, distance_to_curr_wp, 0.0f); + + } else if (_waypoint_transition_angle >= M_PI_F - trans_drv_trn && max_jerk > FLT_EPSILON && max_decel > FLT_EPSILON + && miss_spd_gain > FLT_EPSILON) { + const float speed_reduction = math::constrain(miss_spd_gain * math::interpolate(M_PI_F - _waypoint_transition_angle, + 0.f, M_PI_F, 0.f, 1.f), 0.f, 1.f); + speed_body_x_setpoint = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_decel, distance_to_curr_wp, + max_speed * (1.f - speed_reduction)); + } + + return math::constrain(speed_body_x_setpoint, -cruising_speed, cruising_speed); + +} + +bool DifferentialPosVelControl::runSanityChecks() +{ + bool ret = true; + + if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_ro_speed_limit.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("differential_posVel_control_conf_invalid_speed_lim"), events::Log::Error, + "Invalid configuration of necessary parameter RO_SPEED_LIM", _param_ro_speed_limit.get()); + } + + } + + if (_param_ro_max_thr_speed.get() < FLT_EPSILON && _param_ro_speed_p.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("differential_posVel_control_conf_invalid_speed_control"), events::Log::Error, + "Invalid configuration for speed control: Neither feed forward (RO_MAX_THR_SPEED) nor feedback (RO_SPEED_P) is setup", + _param_ro_max_thr_speed.get(), + _param_ro_speed_p.get()); + } + } + + _prev_param_check_passed = ret; + return ret; +} diff --git a/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.hpp b/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.hpp new file mode 100644 index 0000000000..0ae8a24eae --- /dev/null +++ b/src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.hpp @@ -0,0 +1,243 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +// PX4 includes +#include +#include + +// Libraries +#include +#include +#include +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace matrix; + +/** + * @brief Enum class for the different states of guidance. + */ +enum class GuidanceState { + SPOT_TURNING, // The vehicle is currently turning on the spot. + DRIVING, // The vehicle is currently driving. + STOPPED // The vehicle is stopped. +}; + +/** + * @brief Class for differential position/velocity control. + */ +class DifferentialPosVelControl : public ModuleParams +{ +public: + /** + * @brief Constructor for DifferentialPosVelControl. + * @param parent The parent ModuleParams object. + */ + DifferentialPosVelControl(ModuleParams *parent); + ~DifferentialPosVelControl() = default; + + /** + * @brief Update position controller. + */ + void updatePosVelControl(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + /** + * @brief Update uORB subscriptions used in position controller. + */ + void updateSubscriptions(); + + /** + * @brief Generate and publish roverAttitudeSetpoint from manualControlSetpoint (Position Mode) or + * trajcetorySetpoint (Offboard position or velocity control) or + * positionSetpointTriplet (Auto Mode). + */ + void generateAttitudeSetpoint(); + + /** + * @brief Generate and publish roverThrottleSetpoint and roverAttitudeSetpoint or roverRateSetpoint from manualControlSetpoint. + */ + void manualPositionMode(); + + /** + * @brief Generate and publish roverAttitudeSetpoint from position of trajectorySetpoint. + */ + void offboardPositionMode(); + + /** + * @brief Generate and publish roverAttitudeSetpoint from velocity of trajectorySetpoint. + */ + void offboardVelocityMode(); + + /** + * @brief Generate and publish roverThrottleSetpoint and roverAttitudeSetpoint from positionSetpointTriplet. + */ + void autoPositionMode(); + + /** + * @brief Update uORB subscriptions used for auto modes. + */ + void updateAutoSubscriptions(); + + /** + * @brief Calculate the speed setpoint. During waypoint transition the speed is restricted to + * Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN). + * On straight lines it is based on a speed trajectory such that the rover will arrive at the next waypoint transition + * with the desired waypoiny transition speed under consideration of the maximum deceleration and jerk. + * @param cruising_speed Cruising speed [m/s]. + * @param distance_to_curr_wp Distance to the current waypoint [m]. + * @param max_decel Maximum allowed deceleration [m/s^2]. + * @param max_jerk Maximum allowed jerk [m/s^3]. + * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + * @param max_speed Maximum speed setpoint [m/s] + * @param trans_drv_trn Heading error threshold to switch from driving to turning [rad]. + * @param miss_spd_gain Tuning parameter for the speed reduction during waypoint transition. + * @return Speed setpoint [m/s]. + */ + float calcSpeedSetpoint(float cruising_speed, float distance_to_curr_wp, float max_decel, float max_jerk, + float waypoint_transition_angle, float max_speed, float trans_drv_trn, float miss_spd_gain); + + + /** + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. + */ + bool runSanityChecks(); + + // uORB subscriptions + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _mission_result_sub{ORB_ID(mission_result)}; + uORB::Subscription _home_position_sub{ORB_ID(home_position)}; + uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)}; + vehicle_control_mode_s _vehicle_control_mode{}; + offboard_control_mode_s _offboard_control_mode{}; + rover_steering_setpoint_s _rover_steering_setpoint{}; + + // uORB publications + uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; + uORB::Publication _rover_velocity_status_pub{ORB_ID(rover_velocity_status)}; + uORB::Publication _position_controller_status_pub{ORB_ID(position_controller_status)}; + + // Variables + hrt_abstime _timestamp{0}; + Quatf _vehicle_attitude_quaternion{}; + Vector2f _curr_pos_ned{}; + Vector2f _pos_ctl_course_direction{}; + Vector2f _pos_ctl_start_position_ned{}; + float _vehicle_speed_body_x{0.f}; + float _vehicle_speed_body_y{0.f}; + float _vehicle_yaw{0.f}; + float _max_yaw_rate{0.f}; + float _speed_body_x_setpoint{0.f}; + float _dt{0.f}; + int _nav_state{0}; + bool _course_control{false}; // Indicates if the rover is doing course control in manual position mode. + bool _mission_finished{false}; + + // Waypoint variables + Vector2d _home_position{}; + Vector2f _curr_wp_ned{}; + Vector2f _prev_wp_ned{}; + Vector2f _next_wp_ned{}; + float _cruising_speed{0.f}; + float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + bool _prev_param_check_passed{true}; + + // Controllers + PID _pid_speed; + SlewRate _speed_setpoint; + + // Class Instances + PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library + MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates + GuidanceState _currentState{GuidanceState::DRIVING}; // The current state of the guidance. + + DEFINE_PARAMETERS( + (ParamFloat) _param_rd_trans_trn_drv, + (ParamFloat) _param_rd_trans_drv_trn, + (ParamFloat) _param_rd_miss_spd_gain, + (ParamFloat) _param_ro_max_thr_speed, + (ParamFloat) _param_ro_speed_p, + (ParamFloat) _param_ro_speed_i, + (ParamFloat) _param_ro_yaw_stick_dz, + (ParamFloat) _param_ro_accel_limit, + (ParamFloat) _param_ro_decel_limit, + (ParamFloat) _param_ro_jerk_limit, + (ParamFloat) _param_ro_speed_limit, + (ParamFloat) _param_ro_speed_th, + (ParamFloat) _param_pp_lookahd_max, + (ParamFloat) _param_ro_yaw_rate_limit, + (ParamFloat) _param_nav_acc_rad + + ) +}; diff --git a/src/modules/rover_differential/DifferentialRateControl/CMakeLists.txt b/src/modules/rover_differential/DifferentialRateControl/CMakeLists.txt new file mode 100644 index 0000000000..0182e89d30 --- /dev/null +++ b/src/modules/rover_differential/DifferentialRateControl/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(DifferentialRateControl + DifferentialRateControl.cpp +) + +target_link_libraries(DifferentialRateControl PUBLIC PID) +target_include_directories(DifferentialRateControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp new file mode 100644 index 0000000000..fe62328cd9 --- /dev/null +++ b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.cpp @@ -0,0 +1,190 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "DifferentialRateControl.hpp" + +using namespace time_literals; + +DifferentialRateControl::DifferentialRateControl(ModuleParams *parent) : ModuleParams(parent) +{ + _rover_rate_setpoint_pub.advertise(); + _rover_throttle_setpoint_pub.advertise(); + _rover_steering_setpoint_pub.advertise(); + _rover_rate_status_pub.advertise(); + updateParams(); +} + +void DifferentialRateControl::updateParams() +{ + ModuleParams::updateParams(); + _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + _max_yaw_accel = _param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F; + _max_yaw_decel = _param_ro_yaw_decel_limit.get() * M_DEG_TO_RAD_F; + _pid_yaw_rate.setGains(_param_ro_yaw_rate_p.get(), _param_ro_yaw_rate_i.get(), 0.f); + _pid_yaw_rate.setIntegralLimit(1.f); + _pid_yaw_rate.setOutputLimit(1.f); + _adjusted_yaw_rate_setpoint.setSlewRate(_max_yaw_accel); +} + +void DifferentialRateControl::updateRateControl() +{ + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); + } + + if (_vehicle_angular_velocity_sub.updated()) { + vehicle_angular_velocity_s vehicle_angular_velocity{}; + _vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity); + _vehicle_yaw_rate = fabsf(vehicle_angular_velocity.xyz[2]) > _param_ro_yaw_rate_th.get() * M_DEG_TO_RAD_F ? + vehicle_angular_velocity.xyz[2] : 0.f; + } + + if (_vehicle_control_mode.flag_control_rates_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) { + if (_vehicle_control_mode.flag_control_manual_enabled || _vehicle_control_mode.flag_control_offboard_enabled) { + generateRateSetpoint(); + } + + generateSteeringSetpoint(); + + } else { // Reset controller and slew rate when rate control is not active + _pid_yaw_rate.resetIntegral(); + _adjusted_yaw_rate_setpoint.setForcedValue(0.f); + } + + // Publish rate controller status (logging only) + rover_rate_status_s rover_rate_status; + rover_rate_status.timestamp = _timestamp; + rover_rate_status.measured_yaw_rate = _vehicle_yaw_rate; + rover_rate_status.adjusted_yaw_rate_setpoint = _adjusted_yaw_rate_setpoint.getState(); + rover_rate_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral(); + _rover_rate_status_pub.publish(rover_rate_status); + +} + +void DifferentialRateControl::generateRateSetpoint() +{ + const bool acro_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled + && !_vehicle_control_mode.flag_control_position_enabled && !_vehicle_control_mode.flag_control_attitude_enabled; + + if (acro_mode_enabled && _manual_control_setpoint_sub.updated()) { // Acro Mode + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + rover_throttle_setpoint.throttle_body_y = 0.f; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = math::interpolate (manual_control_setpoint.roll, -1.f, 1.f, + -_max_yaw_rate, _max_yaw_rate); + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + } + + } else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard rate control + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + + if (_offboard_control_mode_sub.updated()) { + _offboard_control_mode_sub.copy(&_offboard_control_mode); + } + + const bool offboard_rate_control = _offboard_control_mode.body_rate && !_offboard_control_mode.position + && !_offboard_control_mode.velocity && !_offboard_control_mode.attitude; + + if (offboard_rate_control && PX4_ISFINITE(trajectory_setpoint.yawspeed)) { + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = trajectory_setpoint.yawspeed; + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + } + } +} + +void DifferentialRateControl::generateSteeringSetpoint() +{ + if (_rover_rate_setpoint_sub.updated()) { + _rover_rate_setpoint_sub.copy(&_rover_rate_setpoint); + + } + + float speed_diff_normalized{0.f}; + + if (PX4_ISFINITE(_rover_rate_setpoint.yaw_rate_setpoint) && PX4_ISFINITE(_vehicle_yaw_rate)) { + const float yaw_rate_setpoint = fabsf(_rover_rate_setpoint.yaw_rate_setpoint) > _param_ro_yaw_rate_th.get() * + M_DEG_TO_RAD_F ? + _rover_rate_setpoint.yaw_rate_setpoint : 0.f; + speed_diff_normalized = RoverControl::rateControl(_adjusted_yaw_rate_setpoint, _pid_yaw_rate, + yaw_rate_setpoint, _vehicle_yaw_rate, _param_rd_max_thr_yaw_r.get(), _max_yaw_accel, + _max_yaw_decel, _param_rd_wheel_track.get(), _dt); + } + + rover_steering_setpoint_s rover_steering_setpoint{}; + rover_steering_setpoint.timestamp = _timestamp; + rover_steering_setpoint.normalized_speed_diff = speed_diff_normalized; + _rover_steering_setpoint_pub.publish(rover_steering_setpoint); +} + +bool DifferentialRateControl::runSanityChecks() +{ + bool ret = true; + + if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("differential_rate_control_conf_invalid_yaw_rate_lim"), events::Log::Error, + "Invalid configuration of necessary parameter RO_YAW_RATE_LIM", _param_ro_yaw_rate_limit.get()); + } + + } + + if ((_param_rd_wheel_track.get() < FLT_EPSILON || _param_rd_max_thr_yaw_r.get() < FLT_EPSILON) + && _param_ro_yaw_rate_p.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("differential_rate_control_conf_invalid_rate_control"), events::Log::Error, + "Invalid configuration for rate control: Neither feed forward nor feedback is setup", _param_rd_wheel_track.get(), + _param_rd_max_thr_yaw_r.get(), _param_ro_yaw_rate_p.get()); + } + } + + _prev_param_check_passed = ret; + return ret; +} diff --git a/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp new file mode 100644 index 0000000000..5b9bd6d632 --- /dev/null +++ b/src/modules/rover_differential/DifferentialRateControl/DifferentialRateControl.hpp @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +// PX4 includes +#include +#include + +// Libraries +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * @brief Class for differential rate control. + */ +class DifferentialRateControl : public ModuleParams +{ +public: + /** + * @brief Constructor for DifferentialRateControl. + * @param parent The parent ModuleParams object. + */ + DifferentialRateControl(ModuleParams *parent); + ~DifferentialRateControl() = default; + + /** + * @brief Update rate controller. + */ + void updateRateControl(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + + /** + * @brief Generate and publish roverRateSetpoint from manualControlSetpoint (Acro Mode) + * or trajectorySetpoint (Offboard rate control). + */ + void generateRateSetpoint(); + + /** + * @brief Generate and publish roverSteeringSetpoint from RoverRateSetpoint. + */ + void generateSteeringSetpoint(); + + /** + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. + */ + bool runSanityChecks(); + + // uORB subscriptions + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; + uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)}; + uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + vehicle_control_mode_s _vehicle_control_mode{}; + offboard_control_mode_s _offboard_control_mode{}; + rover_rate_setpoint_s _rover_rate_setpoint{}; + + // uORB publications + uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; + uORB::Publication _rover_rate_status_pub{ORB_ID(rover_rate_status)}; + + // Variables + hrt_abstime _timestamp{0}; + float _max_yaw_rate{0.f}; + float _max_yaw_accel{0.f}; + float _max_yaw_decel{0.f}; + float _vehicle_yaw_rate{0.f}; + float _dt{0.f}; // Time since last update [s]. + bool _prev_param_check_passed{true}; + + // Controllers + PID _pid_yaw_rate; + SlewRate _adjusted_yaw_rate_setpoint{0.f}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_rd_wheel_track, + (ParamFloat) _param_rd_max_thr_yaw_r, + (ParamFloat) _param_ro_yaw_rate_limit, + (ParamFloat) _param_ro_yaw_rate_th, + (ParamFloat) _param_ro_yaw_rate_p, + (ParamFloat) _param_ro_yaw_rate_i, + (ParamFloat) _param_ro_yaw_accel_limit, + (ParamFloat) _param_ro_yaw_decel_limit + ) +}; diff --git a/src/modules/rover_differential/Kconfig b/src/modules/rover_differential/Kconfig index 840e2cdbf9..ed62f619b7 100644 --- a/src/modules/rover_differential/Kconfig +++ b/src/modules/rover_differential/Kconfig @@ -1,6 +1,5 @@ menuconfig MODULES_ROVER_DIFFERENTIAL bool "rover_differential" default n - depends on MODULES_CONTROL_ALLOCATOR ---help--- - Enable support for control of differential rovers + Enable support for differential rovers diff --git a/src/modules/rover_differential/RoverDifferential.cpp b/src/modules/rover_differential/RoverDifferential.cpp index c5fe294cd3..5ad4439597 100644 --- a/src/modules/rover_differential/RoverDifferential.cpp +++ b/src/modules/rover_differential/RoverDifferential.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * Copyright (c) 2025 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,12 +33,15 @@ #include "RoverDifferential.hpp" +using namespace time_literals; + RoverDifferential::RoverDifferential() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) { + _rover_throttle_setpoint_pub.advertise(); + _rover_steering_setpoint_pub.advertise(); updateParams(); - _rover_differential_setpoint_pub.advertise(); } bool RoverDifferential::init() @@ -50,219 +53,103 @@ bool RoverDifferential::init() void RoverDifferential::updateParams() { ModuleParams::updateParams(); - _max_yaw_rate = _param_rd_max_yaw_rate.get() * M_DEG_TO_RAD_F; + + if (_param_ro_accel_limit.get() > FLT_EPSILON && _param_ro_max_thr_speed.get() > FLT_EPSILON) { + _throttle_body_x_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get()); + } } void RoverDifferential::Run() { - if (should_exit()) { - ScheduleClear(); - exit_and_cleanup(); - return; - } - - updateSubscriptions(); - - // Generate and publish attitude, rate and speed setpoints - hrt_abstime timestamp = hrt_absolute_time(); - - switch (_nav_state) { - case vehicle_status_s::NAVIGATION_STATE_MANUAL: { - manual_control_setpoint_s manual_control_setpoint{}; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - rover_differential_setpoint_s rover_differential_setpoint{}; - rover_differential_setpoint.timestamp = timestamp; - rover_differential_setpoint.forward_speed_setpoint = NAN; - rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle; - rover_differential_setpoint.yaw_setpoint = NAN; - - if (_max_yaw_rate > FLT_EPSILON && _param_rd_max_thr_yaw_r.get() > FLT_EPSILON) { - const float scaled_yaw_rate_input = math::interpolate(manual_control_setpoint.roll, - -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - const float speed_diff = scaled_yaw_rate_input * _param_rd_wheel_track.get() / 2.f; - rover_differential_setpoint.speed_diff_setpoint_normalized = math::interpolate(speed_diff, - -_param_rd_max_thr_yaw_r.get(), _param_rd_max_thr_yaw_r.get(), -1.f, 1.f); - - } else { - rover_differential_setpoint.speed_diff_setpoint_normalized = manual_control_setpoint.roll; - - } - - rover_differential_setpoint.yaw_rate_setpoint = NAN; - _rover_differential_setpoint_pub.publish(rover_differential_setpoint); - } - } break; - - case vehicle_status_s::NAVIGATION_STATE_ACRO: { - manual_control_setpoint_s manual_control_setpoint{}; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - rover_differential_setpoint_s rover_differential_setpoint{}; - rover_differential_setpoint.timestamp = timestamp; - rover_differential_setpoint.forward_speed_setpoint = NAN; - rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle; - rover_differential_setpoint.yaw_rate_setpoint = math::interpolate(manual_control_setpoint.roll, - -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - rover_differential_setpoint.speed_diff_setpoint_normalized = NAN; - rover_differential_setpoint.yaw_setpoint = NAN; - _rover_differential_setpoint_pub.publish(rover_differential_setpoint); - } - - } break; - - case vehicle_status_s::NAVIGATION_STATE_STAB: { - manual_control_setpoint_s manual_control_setpoint{}; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - rover_differential_setpoint_s rover_differential_setpoint{}; - rover_differential_setpoint.timestamp = timestamp; - rover_differential_setpoint.forward_speed_setpoint = NAN; - rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle; - rover_differential_setpoint.yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, - STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - rover_differential_setpoint.speed_diff_setpoint_normalized = NAN; - rover_differential_setpoint.yaw_setpoint = NAN; - - if (fabsf(rover_differential_setpoint.yaw_rate_setpoint) > FLT_EPSILON - || fabsf(rover_differential_setpoint.forward_speed_setpoint_normalized) < FLT_EPSILON) { // Closed loop yaw rate control - _yaw_ctl = false; - - - } else { // Closed loop yaw control if the yaw rate input is zero (keep current yaw) - if (!_yaw_ctl) { - _stab_desired_yaw = _vehicle_yaw; - _yaw_ctl = true; - } - - rover_differential_setpoint.yaw_setpoint = _stab_desired_yaw; - rover_differential_setpoint.yaw_rate_setpoint = NAN; - - } - - _rover_differential_setpoint_pub.publish(rover_differential_setpoint); - } - - } break; - - case vehicle_status_s::NAVIGATION_STATE_POSCTL: { - manual_control_setpoint_s manual_control_setpoint{}; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - rover_differential_setpoint_s rover_differential_setpoint{}; - rover_differential_setpoint.timestamp = timestamp; - rover_differential_setpoint.forward_speed_setpoint = math::interpolate(manual_control_setpoint.throttle, - -1.f, 1.f, -_param_rd_max_speed.get(), _param_rd_max_speed.get()); - rover_differential_setpoint.forward_speed_setpoint_normalized = NAN; - rover_differential_setpoint.yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, - STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - rover_differential_setpoint.speed_diff_setpoint_normalized = NAN; - rover_differential_setpoint.yaw_setpoint = NAN; - - if (fabsf(rover_differential_setpoint.yaw_rate_setpoint) > FLT_EPSILON - || fabsf(rover_differential_setpoint.forward_speed_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control - _yaw_ctl = false; - - - } else { // Course control if the yaw rate input is zero (keep driving on a straight line) - if (!_yaw_ctl) { - _pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw)); - _pos_ctl_start_position_ned = _curr_pos_ned; - _yaw_ctl = true; - } - - // Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover - const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(), - 2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment(); - const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign( - rover_differential_setpoint.forward_speed_setpoint) * - vector_scaling * _pos_ctl_course_direction; - // Calculate yaw setpoint - const float yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, - _pos_ctl_start_position_ned, _curr_pos_ned, fabsf(_vehicle_forward_speed)); - rover_differential_setpoint.yaw_setpoint = sign(rover_differential_setpoint.forward_speed_setpoint) >= 0 ? - yaw_setpoint : matrix::wrap_pi(M_PI_F + yaw_setpoint); // Flip yaw setpoint when driving backwards - rover_differential_setpoint.yaw_rate_setpoint = NAN; - - } - - _rover_differential_setpoint_pub.publish(rover_differential_setpoint); - } - - } break; - - case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: - case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: - _rover_differential_guidance.computeGuidance(_vehicle_yaw, _vehicle_forward_speed, _nav_state); - break; - - default: // Unimplemented nav states will stop the rover - _rover_differential_control.resetControllers(); - _yaw_ctl = false; - rover_differential_setpoint_s rover_differential_setpoint{}; - rover_differential_setpoint.forward_speed_setpoint = NAN; - rover_differential_setpoint.forward_speed_setpoint_normalized = 0.f; - rover_differential_setpoint.yaw_rate_setpoint = NAN; - rover_differential_setpoint.speed_diff_setpoint_normalized = 0.f; - rover_differential_setpoint.yaw_setpoint = NAN; - _rover_differential_setpoint_pub.publish(rover_differential_setpoint); - break; - } - - if (!_armed) { // Reset when disarmed - _rover_differential_control.resetControllers(); - _yaw_ctl = false; - } - - _rover_differential_control.computeMotorCommands(_vehicle_yaw, _vehicle_yaw_rate, _vehicle_forward_speed); - -} - -void RoverDifferential::updateSubscriptions() -{ - if (_parameter_update_sub.updated()) { - parameter_update_s parameter_update; - _parameter_update_sub.copy(¶meter_update); updateParams(); } - if (_vehicle_status_sub.updated()) { - vehicle_status_s vehicle_status{}; - _vehicle_status_sub.copy(&vehicle_status); + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - if (vehicle_status.nav_state != _nav_state) { // Reset on mode change - _rover_differential_control.resetControllers(); - _yaw_ctl = false; - } + _differential_pos_vel_control.updatePosVelControl(); + _differential_att_control.updateAttControl(); + _differential_rate_control.updateRateControl(); - _nav_state = vehicle_status.nav_state; - _armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED; + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); } - if (_vehicle_angular_velocity_sub.updated()) { - vehicle_angular_velocity_s vehicle_angular_velocity{}; - _vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity); - _vehicle_yaw_rate = fabsf(vehicle_angular_velocity.xyz[2]) > YAW_RATE_THRESHOLD ? vehicle_angular_velocity.xyz[2] : 0.f; + const bool full_manual_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled + && !_vehicle_control_mode.flag_control_position_enabled && !_vehicle_control_mode.flag_control_attitude_enabled + && !_vehicle_control_mode.flag_control_rates_enabled; + + if (full_manual_mode_enabled) { // Manual mode + generateSteeringSetpoint(); } - if (_vehicle_attitude_sub.updated()) { - vehicle_attitude_s vehicle_attitude{}; - _vehicle_attitude_sub.copy(&vehicle_attitude); - _vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); - _vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi(); + if (_vehicle_control_mode.flag_armed) { + generateActuatorSetpoint(); + } - if (_vehicle_local_position_sub.updated()) { - vehicle_local_position_s vehicle_local_position{}; - _vehicle_local_position_sub.copy(&vehicle_local_position); - _curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y); - Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); - Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); - _vehicle_forward_speed = fabsf(velocity_in_body_frame(0)) > SPEED_THRESHOLD ? velocity_in_body_frame(0) : 0.f; +} + +void RoverDifferential::generateSteeringSetpoint() +{ + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + rover_steering_setpoint_s rover_steering_setpoint{}; + rover_steering_setpoint.timestamp = _timestamp; + rover_steering_setpoint.normalized_speed_diff = manual_control_setpoint.roll; + _rover_steering_setpoint_pub.publish(rover_steering_setpoint); + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + rover_throttle_setpoint.throttle_body_y = 0.f; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); } } +void RoverDifferential::generateActuatorSetpoint() +{ + if (_rover_throttle_setpoint_sub.updated()) { + _rover_throttle_setpoint_sub.copy(&_rover_throttle_setpoint); + } + + if (_actuator_motors_sub.updated()) { + actuator_motors_s actuator_motors{}; + _actuator_motors_sub.copy(&actuator_motors); + _current_throttle_body_x = (actuator_motors.control[0] + actuator_motors.control[1]) / 2.f; + } + + if (_rover_steering_setpoint_sub.updated()) { + _rover_steering_setpoint_sub.copy(&_rover_steering_setpoint); + } + + const float throttle_body_x = RoverControl::throttleControl(_throttle_body_x_setpoint, + _rover_throttle_setpoint.throttle_body_x, _current_throttle_body_x, _param_ro_accel_limit.get(), + _param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), _dt); + actuator_motors_s actuator_motors{}; + actuator_motors.reversible_flags = _param_r_rev.get(); + computeInverseKinematics(throttle_body_x, + _rover_steering_setpoint.normalized_speed_diff).copyTo(actuator_motors.control); + actuator_motors.timestamp = _timestamp; + _actuator_motors_pub.publish(actuator_motors); +} + +Vector2f RoverDifferential::computeInverseKinematics(float throttle_body_x, const float speed_diff_normalized) +{ + float max_motor_command = fabsf(throttle_body_x) + fabsf(speed_diff_normalized); + + if (max_motor_command > 1.0f) { // Prioritize yaw rate if a normalized motor command exceeds limit of 1 + float excess = fabsf(max_motor_command - 1.0f); + throttle_body_x -= sign(throttle_body_x) * excess; + } + + // Calculate the left and right wheel speeds + return Vector2f(throttle_body_x - speed_diff_normalized, + throttle_body_x + speed_diff_normalized); +} + int RoverDifferential::task_spawn(int argc, char *argv[]) { RoverDifferential *instance = new RoverDifferential(); @@ -288,7 +175,7 @@ int RoverDifferential::task_spawn(int argc, char *argv[]) int RoverDifferential::custom_command(int argc, char *argv[]) { - return print_usage("unk_timestampn command"); + return print_usage("unknown command"); } int RoverDifferential::print_usage(const char *reason) @@ -300,7 +187,7 @@ int RoverDifferential::print_usage(const char *reason) PRINT_MODULE_DESCRIPTION( R"DESCR_STR( ### Description -Rover Differential controller. +Rover differential module. )DESCR_STR"); PRINT_MODULE_USAGE_NAME("rover_differential", "controller"); diff --git a/src/modules/rover_differential/RoverDifferential.hpp b/src/modules/rover_differential/RoverDifferential.hpp index 3fa8f4891c..9c943627b0 100644 --- a/src/modules/rover_differential/RoverDifferential.hpp +++ b/src/modules/rover_differential/RoverDifferential.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * Copyright (c) 2025 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -39,40 +39,34 @@ #include #include #include -#include + +// Libraries +#include +#include // uORB includes -#include #include -#include +#include +#include #include -#include -#include -#include -#include -#include - -// Standard libraries -#include +#include +#include +#include +#include +#include // Local includes -#include "RoverDifferentialGuidance/RoverDifferentialGuidance.hpp" -#include "RoverDifferentialControl/RoverDifferentialControl.hpp" - -using namespace time_literals; - -// Constants -static constexpr float STICK_DEADZONE = - 0.1f; // [0, 1] Percentage of stick input range that will be interpreted as zero around the stick centered value -static constexpr float YAW_RATE_THRESHOLD = - 0.02f; // [rad/s] The minimum threshold for the yaw rate measurement not to be interpreted as zero -static constexpr float SPEED_THRESHOLD = - 0.1f; // [m/s] The minimum threshold for the speed measurement not to be interpreted as zero +#include "DifferentialRateControl/DifferentialRateControl.hpp" +#include "DifferentialAttControl/DifferentialAttControl.hpp" +#include "DifferentialPosVelControl/DifferentialPosVelControl.hpp" class RoverDifferential : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: + /** + * @brief Constructor for RoverDifferential + */ RoverDifferential(); ~RoverDifferential() override = default; @@ -88,51 +82,66 @@ public: bool init(); protected: + /** + * @brief Update the parameters of the module. + */ void updateParams() override; private: void Run() override; /** - * @brief Update uORB subscriptions. + * @brief Generate and publish roverSteeringSetpoint from manualControlSetpoint (Manual Mode). */ - void updateSubscriptions(); + void generateSteeringSetpoint(); - // uORB Subscriptions - uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + /** + * @brief Generate and publish actuatorMotors setpoints from roverThrottleSetpoint/roverSteeringSetpoint. + */ + void generateActuatorSetpoint(); + + /** + * @brief Compute normalized motor commands based on normalized setpoints. + * @param throttle_body_x Normalized speed in body x direction [-1, 1]. + * @param speed_diff_normalized Speed difference between left and right wheels [-1, 1]. + * @return Motor speeds for the right and left motors [-1, 1]. + */ + Vector2f computeInverseKinematics(float throttle_body_x, float speed_diff_normalized); + + // uORB subscriptions uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; - uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; - uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; - uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)}; + uORB::Subscription _rover_throttle_setpoint_sub{ORB_ID(rover_throttle_setpoint)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + vehicle_control_mode_s _vehicle_control_mode{}; + rover_steering_setpoint_s _rover_steering_setpoint{}; + rover_throttle_setpoint_s _rover_throttle_setpoint{}; - // uORB Publications - uORB::Publication _rover_differential_setpoint_pub{ORB_ID(rover_differential_setpoint)}; + // uORB publications + uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; - // Instances - RoverDifferentialGuidance _rover_differential_guidance{this}; - RoverDifferentialControl _rover_differential_control{this}; - PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library + // Class instances + DifferentialRateControl _differential_rate_control{this}; + DifferentialAttControl _differential_att_control{this}; + DifferentialPosVelControl _differential_pos_vel_control{this}; // Variables - Vector2f _curr_pos_ned{}; - matrix::Quatf _vehicle_attitude_quaternion{}; - float _vehicle_yaw_rate{0.f}; - float _vehicle_forward_speed{0.f}; - float _vehicle_yaw{0.f}; - float _max_yaw_rate{0.f}; - int _nav_state{0}; - bool _armed{false}; - bool _yaw_ctl{false}; // Indicates if the rover is doing yaw or yaw rate control in Stabilized and Position mode - float _stab_desired_yaw{0.f}; // Yaw setpoint for Stabilized mode - Vector2f _pos_ctl_course_direction{}; // Course direction for Position mode - Vector2f _pos_ctl_start_position_ned{}; // Initial rover position for course control in Position mode + hrt_abstime _timestamp{0}; + float _dt{0.f}; + float _current_throttle_body_x{0.f}; + // Controllers + SlewRate _throttle_body_x_setpoint{0.f}; + + // Parameters DEFINE_PARAMETERS( - (ParamFloat) _param_rd_wheel_track, - (ParamFloat) _param_rd_max_yaw_rate, - (ParamFloat) _param_rd_max_thr_yaw_r, - (ParamFloat) _param_rd_max_speed, - (ParamFloat) _param_pp_lookahd_max + (ParamInt) _param_r_rev, + (ParamFloat) _param_ro_accel_limit, + (ParamFloat) _param_ro_decel_limit, + (ParamFloat) _param_ro_max_thr_speed ) }; diff --git a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp deleted file mode 100644 index a378de3b94..0000000000 --- a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp +++ /dev/null @@ -1,277 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2024 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "RoverDifferentialControl.hpp" - -#include - -using namespace matrix; -using namespace time_literals; - -RoverDifferentialControl::RoverDifferentialControl(ModuleParams *parent) : ModuleParams(parent) -{ - updateParams(); - _rover_differential_status_pub.advertise(); -} - -void RoverDifferentialControl::updateParams() -{ - ModuleParams::updateParams(); - _max_yaw_rate = _param_rd_max_yaw_rate.get() * M_DEG_TO_RAD_F; - _max_yaw_accel = _param_rd_max_yaw_accel.get() * M_DEG_TO_RAD_F; - - // Update PID - _pid_yaw_rate.setGains(_param_rd_yaw_rate_p.get(), _param_rd_yaw_rate_i.get(), 0.f); - _pid_yaw_rate.setIntegralLimit(1.f); - _pid_yaw_rate.setOutputLimit(1.f); - _pid_throttle.setGains(_param_rd_p_gain_speed.get(), _param_rd_i_gain_speed.get(), 0.f); - _pid_throttle.setIntegralLimit(1.f); - _pid_throttle.setOutputLimit(1.f); - _pid_yaw.setGains(_param_rd_p_gain_yaw.get(), _param_rd_i_gain_yaw.get(), 0.f); - _pid_yaw.setIntegralLimit(_max_yaw_rate); - _pid_yaw.setOutputLimit(_max_yaw_rate); - - // Update slew rates - if (_max_yaw_rate > FLT_EPSILON) { - _yaw_setpoint_with_yaw_rate_limit.setSlewRate(_max_yaw_rate); - } - - if (_max_yaw_accel > FLT_EPSILON) { - _yaw_rate_with_accel_limit.setSlewRate(_max_yaw_accel); - } - -} - -void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, const float vehicle_yaw_rate, - const float vehicle_forward_speed) -{ - // Timestamps - hrt_abstime timestamp_prev = _timestamp; - _timestamp = hrt_absolute_time(); - const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - - // Update differential setpoint - _rover_differential_setpoint_sub.update(&_rover_differential_setpoint); - - // Closed loop yaw control (Overrides yaw rate setpoint) - if (PX4_ISFINITE(_rover_differential_setpoint.yaw_setpoint)) { - _yaw_setpoint_with_yaw_rate_limit.update(matrix::wrap_pi(_rover_differential_setpoint.yaw_setpoint), dt); - _rover_differential_status.adjusted_yaw_setpoint = matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState()); - _pid_yaw.setSetpoint( - matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState() - - vehicle_yaw)); // error as setpoint to take care of wrapping - _rover_differential_setpoint.yaw_rate_setpoint = _pid_yaw.update(0.f, dt); - _rover_differential_status.clyaw_yaw_rate_setpoint = _rover_differential_setpoint.yaw_rate_setpoint; - - } else { - _yaw_setpoint_with_yaw_rate_limit.setForcedValue(vehicle_yaw); - } - - // Yaw rate control - float speed_diff_normalized{0.f}; - - if (PX4_ISFINITE(_rover_differential_setpoint.yaw_rate_setpoint)) { // Closed loop yaw rate control - speed_diff_normalized = calcNormalizedSpeedDiff(_rover_differential_setpoint.yaw_rate_setpoint, vehicle_yaw_rate, - _param_rd_max_thr_yaw_r.get(), _max_yaw_accel, _param_rd_wheel_track.get(), dt, _yaw_rate_with_accel_limit, - _pid_yaw_rate, false); - - } else { // Use normalized setpoint - speed_diff_normalized = calcNormalizedSpeedDiff(_rover_differential_setpoint.speed_diff_setpoint_normalized, - vehicle_yaw_rate, - _param_rd_max_thr_yaw_r.get(), _max_yaw_accel, _param_rd_wheel_track.get(), dt, _yaw_rate_with_accel_limit, - _pid_yaw_rate, true); - } - - // Speed control - float forward_speed_normalized{0.f}; - - if (PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint)) { - if (_param_rd_max_thr_spd.get() > FLT_EPSILON) { - - forward_speed_normalized = math::interpolate(_rover_differential_setpoint.forward_speed_setpoint, - -_param_rd_max_thr_spd.get(), _param_rd_max_thr_spd.get(), -1.f, 1.f); - - if (fabsf(forward_speed_normalized) > 1.f - - fabsf(speed_diff_normalized)) { // Adjust forward speed setpoint if it is infeasible due to the desired speed difference of the left/right wheels - _rover_differential_setpoint.forward_speed_setpoint = sign(_rover_differential_setpoint.forward_speed_setpoint) * - math::interpolate(1.f - fabsf(speed_diff_normalized), -1.f, 1.f, - -_param_rd_max_thr_spd.get(), _param_rd_max_thr_spd.get()); - } - } - - - forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_differential_setpoint.forward_speed_setpoint, - vehicle_forward_speed, _param_rd_max_thr_spd.get(), _forward_speed_setpoint_with_accel_limit, _pid_throttle, - _param_rd_max_accel.get(), _param_rd_max_decel.get(), dt, false); - - } else if (PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint_normalized)) { // Use normalized setpoint - forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_differential_setpoint.forward_speed_setpoint_normalized, - vehicle_forward_speed, _param_rd_max_thr_spd.get(), _forward_speed_setpoint_with_accel_limit, _pid_throttle, - _param_rd_max_accel.get(), _param_rd_max_decel.get(), dt, true); - - } - - // Publish rover differential status (logging) - _rover_differential_status.timestamp = _timestamp; - _rover_differential_status.measured_forward_speed = vehicle_forward_speed; - _rover_differential_status.measured_yaw = vehicle_yaw; - _rover_differential_status.measured_yaw_rate = vehicle_yaw_rate; - _rover_differential_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral(); - _rover_differential_status.pid_throttle_integral = _pid_throttle.getIntegral(); - _rover_differential_status.pid_yaw_integral = _pid_yaw.getIntegral(); - _rover_differential_status_pub.publish(_rover_differential_status); - - // Publish to motors - actuator_motors_s actuator_motors{}; - actuator_motors.reversible_flags = _param_r_rev.get(); - computeInverseKinematics(forward_speed_normalized, speed_diff_normalized).copyTo(actuator_motors.control); - actuator_motors.timestamp = _timestamp; - _actuator_motors_pub.publish(actuator_motors); -} - -float RoverDifferentialControl::calcNormalizedSpeedDiff(const float yaw_rate_setpoint, const float vehicle_yaw_rate, - const float max_thr_yaw_r, - const float max_yaw_accel, const float wheel_track, const float dt, SlewRate &yaw_rate_with_accel_limit, - PID &pid_yaw_rate, const bool normalized) -{ - float slew_rate_normalization{1.f}; - - if (normalized) { // Slew rate needs to be normalized if the setpoint is normalized - slew_rate_normalization = max_thr_yaw_r > FLT_EPSILON ? max_thr_yaw_r : 0.f; - } - - if (max_yaw_accel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) { - yaw_rate_with_accel_limit.setSlewRate(max_yaw_accel / slew_rate_normalization); - yaw_rate_with_accel_limit.update(yaw_rate_setpoint, dt); - - } else { - yaw_rate_with_accel_limit.setForcedValue(yaw_rate_setpoint); - } - - // Transform yaw rate into speed difference - float speed_diff_normalized{0.f}; - - if (normalized) { - speed_diff_normalized = yaw_rate_with_accel_limit.getState(); - - } else { - _rover_differential_status.adjusted_yaw_rate_setpoint = yaw_rate_with_accel_limit.getState(); - - if (wheel_track > FLT_EPSILON && max_thr_yaw_r > FLT_EPSILON) { // Feedforward - const float speed_diff = yaw_rate_with_accel_limit.getState() * wheel_track / - 2.f; - speed_diff_normalized = math::interpolate(speed_diff, -max_thr_yaw_r, - max_thr_yaw_r, -1.f, 1.f); - } - - pid_yaw_rate.setSetpoint(yaw_rate_with_accel_limit.getState()); - speed_diff_normalized += pid_yaw_rate.update(vehicle_yaw_rate, dt); - } - - return math::constrain(speed_diff_normalized, -1.f, 1.f); - -} - -float RoverDifferentialControl::calcNormalizedSpeedSetpoint(const float forward_speed_setpoint, - const float vehicle_forward_speed, const float max_thr_spd, SlewRate &forward_speed_setpoint_with_accel_limit, - PID &pid_throttle, const float max_accel, const float max_decel, const float dt, const bool normalized) -{ - float slew_rate_normalization{1.f}; - - if (normalized) { // Slew rate needs to be normalized if the setpoint is normalized - slew_rate_normalization = max_thr_spd > FLT_EPSILON ? max_thr_spd : 0.f; - } - - // Apply acceleration and deceleration limit - if (fabsf(forward_speed_setpoint) >= fabsf(forward_speed_setpoint_with_accel_limit.getState())) { - if (max_accel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) { - forward_speed_setpoint_with_accel_limit.setSlewRate(max_accel / slew_rate_normalization); - forward_speed_setpoint_with_accel_limit.update(forward_speed_setpoint, dt); - - } else { - forward_speed_setpoint_with_accel_limit.setForcedValue(forward_speed_setpoint); - - } - - } else if (max_decel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) { - forward_speed_setpoint_with_accel_limit.setSlewRate(max_decel / slew_rate_normalization); - forward_speed_setpoint_with_accel_limit.update(forward_speed_setpoint, dt); - - } else { - forward_speed_setpoint_with_accel_limit.setForcedValue(forward_speed_setpoint); - } - - // Calculate normalized forward speed setpoint - float forward_speed_normalized{0.f}; - - if (normalized) { - forward_speed_normalized = _forward_speed_setpoint_with_accel_limit.getState(); - - } else { // Closed loop speed control - _rover_differential_status.adjusted_forward_speed_setpoint = _forward_speed_setpoint_with_accel_limit.getState(); - - if (_param_rd_max_thr_spd.get() > FLT_EPSILON) { // Feedforward - forward_speed_normalized = math::interpolate(_forward_speed_setpoint_with_accel_limit.getState(), - -max_thr_spd, max_thr_spd, - -1.f, 1.f); - } - - pid_throttle.setSetpoint(_forward_speed_setpoint_with_accel_limit.getState()); - forward_speed_normalized += pid_throttle.update(vehicle_forward_speed, dt); - } - - return math::constrain(forward_speed_normalized, -1.f, 1.f); - -} - -matrix::Vector2f RoverDifferentialControl::computeInverseKinematics(float forward_speed_normalized, - const float speed_diff_normalized) -{ - float max_motor_command = fabsf(forward_speed_normalized) + fabsf(speed_diff_normalized); - - if (max_motor_command > 1.0f) { // Prioritize yaw rate if a normalized motor command exceeds limit of 1 - float excess = fabsf(max_motor_command - 1.0f); - forward_speed_normalized -= sign(forward_speed_normalized) * excess; - } - - // Calculate the left and right wheel speeds - return Vector2f(forward_speed_normalized - speed_diff_normalized, - forward_speed_normalized + speed_diff_normalized); -} - -void RoverDifferentialControl::resetControllers() -{ - _pid_throttle.resetIntegral(); - _pid_yaw_rate.resetIntegral(); - _pid_yaw.resetIntegral(); -} diff --git a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp deleted file mode 100644 index e08153356b..0000000000 --- a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp +++ /dev/null @@ -1,169 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2024 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -// PX4 includes -#include - -// uORB includes -#include -#include -#include -#include -#include -#include -#include -#include - -// Standard libraries -#include -#include -#include -#include -#include - -using namespace matrix; - -/** - * @brief Class for differential rover guidance. - */ -class RoverDifferentialControl : public ModuleParams -{ -public: - /** - * @brief Constructor for RoverDifferentialControl. - * @param parent The parent ModuleParams object. - */ - RoverDifferentialControl(ModuleParams *parent); - ~RoverDifferentialControl() = default; - - /** - * @brief Compute motor commands based on setpoints - */ - void computeMotorCommands(float vehicle_yaw, float vehicle_yaw_rate, float vehicle_forward_speed); - - /** - * @brief Reset PID controllers - */ - void resetControllers(); - -protected: - /** - * @brief Update the parameters of the module. - */ - void updateParams() override; - -private: - - /** - * @brief Compute normalized speed diff setpoint between the left and right wheels and apply slew rates. - * @param yaw_rate_setpoint Yaw rate setpoint [rad/s or normalized [-1, 1]]. - * @param vehicle_yaw_rate Measured yaw rate [rad/s]. - * @param max_thr_yaw_r Yaw rate turning left/right wheels at max speed in opposite directions [m/s]. - * @param max_yaw_accel Maximum allowed yaw acceleration for the rover [rad/s^2]. - * @param wheel_track Wheel track [m]. - * @param dt Time since last update [s]. - * @param yaw_rate_with_accel_limit Yaw rate slew rate. - * @param pid_yaw_rate Yaw rate PID - * @param normalized Indicates if the forward speed setpoint is already normalized. - * @return Normalized speed differece setpoint with applied slew rates [-1, 1]. - */ - float calcNormalizedSpeedDiff(float yaw_rate_setpoint, float vehicle_yaw_rate, float max_thr_yaw_r, float max_yaw_accel, - float wheel_track, float dt, SlewRate &yaw_rate_with_accel_limit, PID &pid_yaw_rate, bool normalized); - /** - * @brief Compute normalized forward speed setpoint and apply slew rates. - * to the forward speed setpoint and doing closed loop speed control if enabled. - * @param forward_speed_setpoint Forward speed setpoint [m/s]. - * @param vehicle_forward_speed Actual forward speed [m/s]. - * @param max_thr_spd Speed the rover drives at maximum throttle [m/s]. - * @param forward_speed_setpoint_with_accel_limit Speed slew rate. - * @param pid_throttle Throttle PID - * @param max_accel Maximum acceleration [m/s^2] - * @param max_decel Maximum deceleration [m/s^2] - * @param dt Time since last update [s]. - * @param normalized Indicates if the forward speed setpoint is already normalized. - * @return Normalized forward speed setpoint with applied slew rates [-1, 1]. - */ - float calcNormalizedSpeedSetpoint(float forward_speed_setpoint, float vehicle_forward_speed, float max_thr_spd, - SlewRate &forward_speed_setpoint_with_accel_limit, PID &pid_throttle, float max_accel, float max_decel, - float dt, bool normalized); - - /** - * @brief Compute normalized motor commands based on normalized setpoints. - * @param forward_speed_normalized Normalized forward speed [-1, 1]. - * @param speed_diff_normalized Speed difference between left and right wheels [-1, 1]. - * @return matrix::Vector2f Motor velocities for the right and left motors [-1, 1]. - */ - matrix::Vector2f computeInverseKinematics(float forward_speed_normalized, const float speed_diff_normalized); - - // uORB subscriptions - uORB::Subscription _rover_differential_setpoint_sub{ORB_ID(rover_differential_setpoint)}; - - // uORB publications - uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; - uORB::Publication _rover_differential_status_pub{ORB_ID(rover_differential_status)}; - rover_differential_status_s _rover_differential_status{}; - - // Variables - rover_differential_setpoint_s _rover_differential_setpoint{}; - hrt_abstime _timestamp{0}; - float _max_yaw_rate{0.f}; - float _max_yaw_accel{0.f}; - - // Controllers - PID _pid_throttle; // Closed loop speed control - PID _pid_yaw; // Closed loop yaw control - PID _pid_yaw_rate; // Closed loop yaw rate control - SlewRate _forward_speed_setpoint_with_accel_limit{0.f}; - SlewRate _yaw_rate_with_accel_limit{0.f}; - SlewRateYaw _yaw_setpoint_with_yaw_rate_limit; - - // Parameters - DEFINE_PARAMETERS( - (ParamFloat) _param_rd_wheel_track, - (ParamFloat) _param_rd_max_thr_spd, - (ParamFloat) _param_rd_max_accel, - (ParamFloat) _param_rd_max_decel, - (ParamFloat) _param_rd_max_thr_yaw_r, - (ParamFloat) _param_rd_max_yaw_rate, - (ParamFloat) _param_rd_max_yaw_accel, - (ParamFloat) _param_rd_yaw_rate_p, - (ParamFloat) _param_rd_yaw_rate_i, - (ParamFloat) _param_rd_p_gain_speed, - (ParamFloat) _param_rd_i_gain_speed, - (ParamFloat) _param_rd_p_gain_yaw, - (ParamFloat) _param_rd_i_gain_yaw, - (ParamInt) _param_r_rev - ) -}; diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp deleted file mode 100644 index bb45e6b7b9..0000000000 --- a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp +++ /dev/null @@ -1,234 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2024 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "RoverDifferentialGuidance.hpp" - -#include - -using namespace matrix; - -RoverDifferentialGuidance::RoverDifferentialGuidance(ModuleParams *parent) : ModuleParams(parent) -{ - updateParams(); - _max_forward_speed = _param_rd_max_speed.get(); - _rover_differential_guidance_status_pub.advertise(); -} - -void RoverDifferentialGuidance::updateParams() -{ - ModuleParams::updateParams(); -} - -void RoverDifferentialGuidance::computeGuidance(const float vehicle_yaw, const float forward_speed, const int nav_state) -{ - updateSubscriptions(); - - // Catch return to launch - const float distance_to_curr_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), - _curr_wp(0), _curr_wp(1)); - - if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { - _mission_finished = distance_to_curr_wp < _param_nav_acc_rad.get(); - } - - // State machine - float desired_yaw = _pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, - math::max(forward_speed, 0.f)); - const float heading_error = matrix::wrap_pi(desired_yaw - vehicle_yaw); - - if (!_mission_finished && distance_to_curr_wp > _param_nav_acc_rad.get()) { - if (_currentState == GuidanceState::STOPPED) { - _currentState = GuidanceState::DRIVING; - } - - if (_currentState == GuidanceState::DRIVING && fabsf(heading_error) > _param_rd_trans_drv_trn.get()) { - _currentState = GuidanceState::SPOT_TURNING; - - } else if (_currentState == GuidanceState::SPOT_TURNING && fabsf(heading_error) < _param_rd_trans_trn_drv.get()) { - _currentState = GuidanceState::DRIVING; - } - - } else { // Mission finished or delay command - _currentState = GuidanceState::STOPPED; - } - - // Guidance logic - float desired_forward_speed{0.f}; - - switch (_currentState) { - case GuidanceState::DRIVING: { - // Calculate desired forward speed - desired_forward_speed = _max_forward_speed; - - if (_waypoint_transition_angle < M_PI_F - _param_rd_trans_drv_trn.get()) { - if (_param_rd_max_jerk.get() > FLT_EPSILON && _param_rd_max_decel.get() > FLT_EPSILON) { - desired_forward_speed = math::trajectory::computeMaxSpeedFromDistance(_param_rd_max_jerk.get(), - _param_rd_max_decel.get(), distance_to_curr_wp, 0.0f); - desired_forward_speed = math::constrain(desired_forward_speed, -_max_forward_speed, _max_forward_speed); - } - - } else if (_param_rd_max_jerk.get() > FLT_EPSILON && _param_rd_max_decel.get() > FLT_EPSILON - && _param_rd_miss_spd_gain.get() > FLT_EPSILON) { - const float speed_reduction = math::constrain(_param_rd_miss_spd_gain.get() * math::interpolate( - M_PI_F - _waypoint_transition_angle, 0.f, - M_PI_F, 0.f, 1.f), 0.f, 1.f); - desired_forward_speed = math::trajectory::computeMaxSpeedFromDistance(_param_rd_max_jerk.get(), - _param_rd_max_decel.get(), distance_to_curr_wp, _max_forward_speed * (1.f - speed_reduction)); - desired_forward_speed = math::constrain(desired_forward_speed, -_max_forward_speed, - _max_forward_speed); - } - - } break; - - case GuidanceState::SPOT_TURNING: - if (forward_speed > TURN_MAX_VELOCITY) { - desired_yaw = vehicle_yaw; // Wait for the rover to stop - - } - - break; - - case GuidanceState::STOPPED: - default: - desired_yaw = vehicle_yaw; - break; - - } - - // Publish differential guidance status (logging) - hrt_abstime timestamp = hrt_absolute_time(); - rover_differential_guidance_status_s rover_differential_guidance_status{}; - rover_differential_guidance_status.timestamp = timestamp; - rover_differential_guidance_status.lookahead_distance = _pure_pursuit.getLookaheadDistance(); - rover_differential_guidance_status.heading_error_deg = M_RAD_TO_DEG_F * heading_error; - rover_differential_guidance_status.state_machine = (uint8_t) _currentState; - _rover_differential_guidance_status_pub.publish(rover_differential_guidance_status); - - // Publish setpoints - rover_differential_setpoint_s rover_differential_setpoint{}; - rover_differential_setpoint.timestamp = timestamp; - rover_differential_setpoint.forward_speed_setpoint = desired_forward_speed; - rover_differential_setpoint.forward_speed_setpoint_normalized = NAN; - rover_differential_setpoint.yaw_rate_setpoint = NAN; - rover_differential_setpoint.speed_diff_setpoint_normalized = NAN; - rover_differential_setpoint.yaw_setpoint = desired_yaw; - _rover_differential_setpoint_pub.publish(rover_differential_setpoint); -} - -void RoverDifferentialGuidance::updateSubscriptions() -{ - if (_vehicle_global_position_sub.updated()) { - vehicle_global_position_s vehicle_global_position{}; - _vehicle_global_position_sub.copy(&vehicle_global_position); - _curr_pos = Vector2d(vehicle_global_position.lat, vehicle_global_position.lon); - } - - if (_local_position_sub.updated()) { - vehicle_local_position_s local_position{}; - _local_position_sub.copy(&local_position); - - if (!_global_ned_proj_ref.isInitialized() - || (_global_ned_proj_ref.getProjectionReferenceTimestamp() != local_position.ref_timestamp)) { - _global_ned_proj_ref.initReference(local_position.ref_lat, local_position.ref_lon, local_position.ref_timestamp); - } - - _curr_pos_ned = Vector2f(local_position.x, local_position.y); - } - - if (_position_setpoint_triplet_sub.updated()) { - updateWaypoints(); - } - - if (_mission_result_sub.updated()) { - mission_result_s mission_result{}; - _mission_result_sub.copy(&mission_result); - _mission_finished = mission_result.finished; - } - - if (_home_position_sub.updated()) { - home_position_s home_position{}; - _home_position_sub.copy(&home_position); - _home_position = Vector2d(home_position.lat, home_position.lon); - } -} - -void RoverDifferentialGuidance::updateWaypoints() -{ - position_setpoint_triplet_s position_setpoint_triplet{}; - _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); - - // Global waypoint coordinates - if (position_setpoint_triplet.current.valid && PX4_ISFINITE(position_setpoint_triplet.current.lat) - && PX4_ISFINITE(position_setpoint_triplet.current.lon)) { - _curr_wp = Vector2d(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon); - - } else { - _curr_wp = Vector2d(0, 0); - } - - if (position_setpoint_triplet.previous.valid && PX4_ISFINITE(position_setpoint_triplet.previous.lat) - && PX4_ISFINITE(position_setpoint_triplet.previous.lon)) { - _prev_wp = Vector2d(position_setpoint_triplet.previous.lat, position_setpoint_triplet.previous.lon); - - } else { - _prev_wp = _curr_pos; - } - - if (position_setpoint_triplet.next.valid && PX4_ISFINITE(position_setpoint_triplet.next.lat) - && PX4_ISFINITE(position_setpoint_triplet.next.lon)) { - _next_wp = Vector2d(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon); - - } else { - _next_wp = _home_position; - } - - // NED waypoint coordinates - _curr_wp_ned = _global_ned_proj_ref.project(_curr_wp(0), _curr_wp(1)); - _prev_wp_ned = _global_ned_proj_ref.project(_prev_wp(0), _prev_wp(1)); - _next_wp_ned = _global_ned_proj_ref.project(_next_wp(0), _next_wp(1)); - - // Distances - const Vector2f curr_to_next_wp_ned = _next_wp_ned - _curr_wp_ned; - const Vector2f curr_to_prev_wp_ned = _prev_wp_ned - _curr_wp_ned; - float cosin = curr_to_prev_wp_ned.unit_or_zero() * curr_to_next_wp_ned.unit_or_zero(); - cosin = math::constrain(cosin, -1.f, 1.f); // Protect against float precision problem - _waypoint_transition_angle = acosf(cosin); - - // Waypoint cruising speed - if (position_setpoint_triplet.current.cruising_speed > 0.f) { - _max_forward_speed = math::constrain(position_setpoint_triplet.current.cruising_speed, 0.f, _param_rd_max_speed.get()); - - } else { - _max_forward_speed = _param_rd_max_speed.get(); - } -} diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp deleted file mode 100644 index cc1b033635..0000000000 --- a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp +++ /dev/null @@ -1,151 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2024 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -// PX4 includes -#include -#include - -// uORB includes -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// Standard libraries -#include -#include -#include -#include -#include - -using namespace matrix; - -/** - * @brief Enum class for the different states of guidance. - */ -enum class GuidanceState { - SPOT_TURNING, // The vehicle is currently turning on the spot. - DRIVING, // The vehicle is currently driving. - STOPPED // The vehicle is stopped. -}; - -/** - * @brief Class for differential rover guidance. - */ -class RoverDifferentialGuidance : public ModuleParams -{ -public: - /** - * @brief Constructor for RoverDifferentialGuidance. - * @param parent The parent ModuleParams object. - */ - RoverDifferentialGuidance(ModuleParams *parent); - ~RoverDifferentialGuidance() = default; - - /** - * @brief Compute and publish attitude and velocity setpoints based on the mission plan. - * @param vehicle_yaw The yaw of the vehicle [rad]. - * @param forward_speed The forward speed of the vehicle [m/s]. - * @param nav_state Navigation state of the rover. - */ - void computeGuidance(float vehicle_yaw, float forward_speed, int nav_state); - -protected: - /** - * @brief Update the parameters of the module. - */ - void updateParams() override; - -private: - /** - * @brief Update uORB subscriptions. - */ - void updateSubscriptions(); - - /** - * @brief Update global/ned waypoint coordinates - */ - void updateWaypoints(); - - // uORB subscriptions - uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; - uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; - uORB::Subscription _mission_result_sub{ORB_ID(mission_result)}; - uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _home_position_sub{ORB_ID(home_position)}; - - // uORB publications - uORB::Publication _rover_differential_setpoint_pub{ORB_ID(rover_differential_setpoint)}; - uORB::Publication _rover_differential_guidance_status_pub{ORB_ID(rover_differential_guidance_status)}; - - // Variables - MapProjection _global_ned_proj_ref{}; // Transform global to ned coordinates. - GuidanceState _currentState{GuidanceState::DRIVING}; // The current state of the guidance. - PurePursuit _pure_pursuit{this}; // Pure pursuit library - bool _mission_finished{false}; - - // Waypoints - Vector2d _curr_pos{}; - Vector2f _curr_pos_ned{}; - Vector2d _prev_wp{}; - Vector2f _prev_wp_ned{}; - Vector2d _curr_wp{}; - Vector2f _curr_wp_ned{}; - Vector2d _next_wp{}; - Vector2f _next_wp_ned{}; - Vector2d _home_position{}; - float _max_forward_speed{0.f}; // Maximum forward speed for the rover [m/s] - float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad] - - // Constants - static constexpr float TURN_MAX_VELOCITY = 0.2f; // Velocity threshhold for starting the spot turn [m/s] - - // Parameters - DEFINE_PARAMETERS( - (ParamFloat) _param_nav_acc_rad, - (ParamFloat) _param_rd_max_jerk, - (ParamFloat) _param_rd_max_decel, - (ParamFloat) _param_rd_max_speed, - (ParamFloat) _param_rd_trans_trn_drv, - (ParamFloat) _param_rd_trans_drv_trn, - (ParamFloat) _param_rd_miss_spd_gain - ) -}; diff --git a/src/modules/rover_differential/module.yaml b/src/modules/rover_differential/module.yaml index 6297f3aafd..eca54e798e 100644 --- a/src/modules/rover_differential/module.yaml +++ b/src/modules/rover_differential/module.yaml @@ -3,176 +3,17 @@ module_name: Rover Differential parameters: - group: Rover Differential definitions: - RD_WHEEL_TRACK: - description: - short: Wheel track - long: Distance from the center of the right wheel to the center of the left wheel - type: float - unit: m - min: 0.001 - max: 100 - increment: 0.001 - decimal: 3 - default: 0.5 - - RD_YAW_P: - description: - short: Proportional gain for closed loop yaw controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 1 - - RD_YAW_I: - description: - short: Integral gain for closed loop yaw controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 0 - - RD_SPEED_P: - description: - short: Proportional gain for closed loop forward speed controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 1 - - RD_SPEED_I: - description: - short: Integral gain for closed loop forward speed controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 0 - - RD_YAW_RATE_P: - description: - short: Proportional gain for closed loop yaw rate controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 1 - - RD_YAW_RATE_I: - description: - short: Integral gain for closed loop yaw rate controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 0 - - RD_MAX_JERK: - description: - short: Maximum jerk - long: Limit for forwards acc/deceleration change. - type: float - unit: m/s^3 - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 0.5 - - RD_MAX_ACCEL: - description: - short: Maximum acceleration - long: Maximum acceleration is used to limit the acceleration of the rover - type: float - unit: m/s^2 - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 0.5 - - RD_MAX_DECEL: - description: - short: Maximum deceleration - long: | - Maximum decelaration is used to limit the deceleration of the rover. - Set to -1 to disable, causing the rover to decelerate as fast as possible. - Caution: This disables the slow down effect in auto modes. - type: float - unit: m/s^2 - min: -1 - max: 100 - increment: 0.01 - decimal: 2 - default: -1 - - RD_MAX_SPEED: - description: - short: Maximum speed setpoint - long: | - This parameter is used to cap desired forward speed and map controller inputs to desired speeds in Position mode. - type: float - unit: m/s - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 1 - - RD_MAX_THR_SPD: - description: - short: Speed the rover drives at maximum throttle - long: | - This parameter is used to calculate the feedforward term of the closed loop speed control which linearly - maps desired speeds to normalized motor commands [-1. 1]. - A good starting point is the observed ground speed when the rover drives at maximum throttle in manual mode. - Increase this parameter if the rover is faster than the setpoint, and decrease if the rover is slower. - type: float - unit: m/s - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 1 - - RD_MAX_YAW_RATE: - description: - short: Maximum allowed yaw rate for the rover - long: | - This parameter is used to cap desired yaw rates and map controller inputs to desired yaw rates - in Acro,Stabilized and Position mode. - type: float - unit: deg/s - min: 0.01 - max: 1000 - increment: 0.01 - decimal: 2 - default: 90 - - RD_MAX_YAW_ACCEL: - description: - short: Maximum allowed yaw acceleration for the rover - long: | - This parameter is used to cap desired yaw acceleration. This is used to adjust incoming yaw rate setpoints - to a feasible yaw rate setpoint based on the physical limitation on how fast the yaw rate can change. - This leads to a smooth setpoint trajectory for the closed loop yaw rate controller to track. - Set to -1 to disable. - type: float - unit: deg/s^2 - min: -1 - max: 1000 - increment: 0.01 - decimal: 2 - default: -1 + description: + short: Wheel track + long: Distance from the center of the right wheel to the center of the left wheel. + type: float + unit: m + min: 0 + max: 100 + increment: 0.001 + decimal: 3 + default: 0 RD_MAX_THR_YAW_R: description: @@ -181,7 +22,7 @@ parameters: This parameter is used to calculate the feedforward term of the closed loop yaw rate control. The controller first calculates the required speed difference between the left and right motor to achieve the desired yaw rate. This desired speed difference is then linearly mapped to normalized motor commands. - A good starting point is twice the speed the rover drives at maximum throttle (RD_MAX_THR_SPD)). + A good starting point is half the speed the rover drives at maximum throttle (RD_MAX_THR_SPD)). Increase this parameter if the rover turns faster than the setpoint, and decrease if the rover turns slower. type: float unit: m/s @@ -189,11 +30,14 @@ parameters: max: 100 increment: 0.01 decimal: 2 - default: 2 + default: 0 RD_TRANS_TRN_DRV: description: short: Yaw error threshhold to switch from spot turning to driving + long: | + This threshold is used for the state machine to switch from turning to driving based on the + error between the desired and actual yaw. type: float unit: rad min: 0.001 @@ -227,9 +71,10 @@ parameters: The normalized transition angle is the angle between the line segment from prev-curr WP and curr-next WP interpolated from [0, 180] -> [0, 1]. Higher value -> More speed reduction during waypoint transitions. + Set to -1 to disable any speed reduction during waypoint transition. type: float - min: 0.05 + min: -1 max: 100 increment: 0.01 decimal: 2 - default: 1 + default: -1 From 73dbecadf1c3e01970d6836aae83ef3c9cc03f90 Mon Sep 17 00:00:00 2001 From: Eric Katzfey <53063038+katzfey@users.noreply.github.com> Date: Wed, 19 Feb 2025 07:59:45 -0800 Subject: [PATCH 37/91] Qurt: changed the mutex for I2C for the Qurt platform from a single mutex to one for each bus. (#23531) --- src/lib/drivers/device/qurt/I2C.cpp | 40 ++++++++++++++++++++++++----- src/lib/drivers/device/qurt/I2C.hpp | 10 +++++++- 2 files changed, 42 insertions(+), 8 deletions(-) diff --git a/src/lib/drivers/device/qurt/I2C.cpp b/src/lib/drivers/device/qurt/I2C.cpp index c2b70f66d3..a459dbbe81 100644 --- a/src/lib/drivers/device/qurt/I2C.cpp +++ b/src/lib/drivers/device/qurt/I2C.cpp @@ -55,7 +55,16 @@ I2C::_config_i2c_bus_func_t I2C::_config_i2c_bus = NULL; I2C::_set_i2c_address_func_t I2C::_set_i2c_address = NULL; I2C::_i2c_transfer_func_t I2C::_i2c_transfer = NULL; -pthread_mutex_t I2C::_mutex = PTHREAD_MUTEX_INITIALIZER; +// There is a mutex per I2C bus. A mutex isn't required with normal +// use since per bus I2C accesses are made via work item tied to a per bus thread. +// But it is here anyways in case someone decides to use the bus in some different +// custom code that has a separate thread. +struct I2C::_bus_mutex_t I2C::_bus_mutex[I2C::MAX_I2C_BUS] = { + {1, PTHREAD_MUTEX_INITIALIZER}, + {2, PTHREAD_MUTEX_INITIALIZER}, + {4, PTHREAD_MUTEX_INITIALIZER}, + {5, PTHREAD_MUTEX_INITIALIZER} +}; I2C::I2C(uint8_t device_type, const char *name, const int bus, const uint16_t address, const uint32_t frequency) : CDev(name, nullptr), @@ -91,10 +100,27 @@ I2C::init() goto out; } - pthread_mutex_lock(&_mutex); + if (_mutex == nullptr) { + for (int i = 0; i < MAX_I2C_BUS; i++) { + if (get_device_bus() == _bus_mutex[i]._bus) { + _mutex = &_bus_mutex[i]._mutex; + break; + } + } + + if (_mutex == nullptr) { + PX4_ERR("NULL i2c bus mutex"); + goto out; + + } else { + PX4_INFO("Set up I2C bus mutex for bus %d", get_device_bus()); + } + } + + pthread_mutex_lock(_mutex); // Open the actual I2C device _i2c_fd = _config_i2c_bus(get_device_bus(), get_device_address(), _frequency); - pthread_mutex_unlock(&_mutex); + pthread_mutex_unlock(_mutex); if (_i2c_fd == PX4_ERROR) { PX4_ERR("i2c init failed"); @@ -131,9 +157,9 @@ I2C::set_device_address(int address) if ((_i2c_fd != PX4_ERROR) && (_set_i2c_address != NULL)) { PX4_INFO("Set i2c address 0x%x, fd %d", address, _i2c_fd); - pthread_mutex_lock(&_mutex); + pthread_mutex_lock(_mutex); _set_i2c_address(_i2c_fd, address); - pthread_mutex_unlock(&_mutex); + pthread_mutex_unlock(_mutex); Device::set_device_address(address); } @@ -150,9 +176,9 @@ I2C::transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const do { // PX4_INFO("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); - pthread_mutex_lock(&_mutex); + pthread_mutex_lock(_mutex); ret = _i2c_transfer(_i2c_fd, send, send_len, recv, recv_len); - pthread_mutex_unlock(&_mutex); + pthread_mutex_unlock(_mutex); if (ret != PX4_ERROR) { break; } diff --git a/src/lib/drivers/device/qurt/I2C.hpp b/src/lib/drivers/device/qurt/I2C.hpp index 7f1d59bc0a..ccf5e1f5df 100644 --- a/src/lib/drivers/device/qurt/I2C.hpp +++ b/src/lib/drivers/device/qurt/I2C.hpp @@ -124,10 +124,18 @@ protected: private: uint32_t _frequency{0}; int _i2c_fd{-1}; + pthread_mutex_t *_mutex{nullptr}; + + static const int MAX_I2C_BUS{4}; + static _config_i2c_bus_func_t _config_i2c_bus; static _set_i2c_address_func_t _set_i2c_address; static _i2c_transfer_func_t _i2c_transfer; - static pthread_mutex_t _mutex; + + static struct _bus_mutex_t { + int _bus; + pthread_mutex_t _mutex{PTHREAD_MUTEX_INITIALIZER}; + } _bus_mutex[MAX_I2C_BUS]; }; } // namespace device From 98ceb0ce799031202b43248521617f7aa0be3409 Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Sun, 9 Feb 2025 14:21:46 -0800 Subject: [PATCH 38/91] voxl_esc: Added GPIO control feature voxl_esc: Removed obsolete modal_io_data UORB topic --- msg/Buffer128.msg | 8 - msg/CMakeLists.txt | 1 - src/drivers/actuators/voxl_esc/voxl_esc.cpp | 243 ++++++++++++------ src/drivers/actuators/voxl_esc/voxl_esc.hpp | 31 ++- .../actuators/voxl_esc/voxl_esc_params.c | 14 + 5 files changed, 195 insertions(+), 102 deletions(-) delete mode 100644 msg/Buffer128.msg diff --git a/msg/Buffer128.msg b/msg/Buffer128.msg deleted file mode 100644 index 4ff939a87d..0000000000 --- a/msg/Buffer128.msg +++ /dev/null @@ -1,8 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -uint8 len # length of data -uint32 MAX_BUFLEN = 128 - -uint8[128] data # data - -# TOPICS voxl2_io_data diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index ac59212f40..a88567b286 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -48,7 +48,6 @@ set(msg_files AirspeedValidated.msg AirspeedWind.msg AutotuneAttitudeControlStatus.msg - Buffer128.msg ButtonEvent.msg CameraCapture.msg CameraStatus.msg diff --git a/src/drivers/actuators/voxl_esc/voxl_esc.cpp b/src/drivers/actuators/voxl_esc/voxl_esc.cpp index f5568d7eeb..740211c4e8 100644 --- a/src/drivers/actuators/voxl_esc/voxl_esc.cpp +++ b/src/drivers/actuators/voxl_esc/voxl_esc.cpp @@ -90,13 +90,13 @@ VoxlEsc::~VoxlEsc() int VoxlEsc::init() { - PX4_INFO("VOXL_ESC: Starting VOXL ESC driver"); + PX4_ERR("Starting VOXL ESC driver"); /* Getting initial parameter values */ int ret = update_params(); if (ret != OK) { - PX4_ERR("VOXL_ESC: Failed to update params during init"); + PX4_ERR("Failed to update params during init"); return ret; } @@ -124,13 +124,13 @@ int VoxlEsc::device_init() // Open serial port if (!_uart_port.isOpen()) { - PX4_INFO("VOXL_ESC: Opening UART ESC device %s, baud rate %" PRIi32, _device, _parameters.baud_rate); + PX4_ERR("Opening UART ESC device %s, baud rate %" PRIi32, _device, _parameters.baud_rate); #ifndef __PX4_QURT //warn user that unless DMA is enabled for UART RX, data can be lost due to high frequency of per char cpu interrupts //at least at 2mbit, there are definitely losses, did not test other baud rates to find the cut off if (_parameters.baud_rate > 250000) { - PX4_WARN("VOXL_ESC: Baud rate is too high for non-DMA based UART, this can lead to loss of RX data"); + PX4_WARN("Baud rate is too high for non-DMA based UART, this can lead to loss of RX data"); } #endif @@ -165,7 +165,7 @@ int VoxlEsc::device_init() } // Detect ESCs - PX4_INFO("VOXL_ESC: Detecting ESCs..."); + PX4_ERR("Detecting ESCs..."); qc_esc_packet_init(&_fb_packet); //request extended version info from each ESC and wait for reply @@ -174,7 +174,7 @@ int VoxlEsc::device_init() cmd.len = qc_esc_create_extended_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf)); if (_uart_port.write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("VOXL_ESC: Could not write version request packet to UART port"); + PX4_ERR("Could not write version request packet to UART port"); return -1; } @@ -201,17 +201,17 @@ int VoxlEsc::device_init() QC_ESC_EXTENDED_VERSION_INFO ver; memcpy(&ver, _fb_packet.buffer, packet_size); - PX4_INFO("VOXL_ESC: \tESC ID : %i", ver.id); - PX4_INFO("VOXL_ESC: \tBoard Type : %i: %s", ver.hw_version, board_id_to_name(ver.hw_version)); + PX4_ERR("\tESC ID : %i", ver.id); + PX4_ERR("\tBoard Type : %i: %s", ver.hw_version, board_id_to_name(ver.hw_version)); uint8_t *u = &ver.unique_id[0]; - PX4_INFO("VOXL_ESC: \tUnique ID : 0x%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X", - u[11], u[10], u[9], u[8], u[7], u[6], u[5], u[4], u[3], u[2], u[1], u[0]); + PX4_ERR("\tUnique ID : 0x%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X", + u[11], u[10], u[9], u[8], u[7], u[6], u[5], u[4], u[3], u[2], u[1], u[0]); - PX4_INFO("VOXL_ESC: \tFirmware : version %4d, hash %.12s", ver.sw_version, ver.firmware_git_version); - PX4_INFO("VOXL_ESC: \tBootloader : version %4d, hash %.12s", ver.bootloader_version, ver.bootloader_git_version); - PX4_INFO("VOXL_ESC: \tReply time : %" PRIu32 "us", (uint32_t)response_time); - PX4_INFO("VOXL_ESC:"); + PX4_ERR("\tFirmware : version %4d, hash %.12s", ver.sw_version, ver.firmware_git_version); + PX4_ERR("\tBootloader : version %4d, hash %.12s", ver.bootloader_version, ver.bootloader_git_version); + PX4_ERR("\tReply time : %" PRIu32 "us", (uint32_t)response_time); + PX4_INFO(""); if (ver.id == esc_id) { memcpy(&_version_info[esc_id], &ver, sizeof(ver)); @@ -223,7 +223,7 @@ int VoxlEsc::device_init() } if (!got_response) { - PX4_ERR("VOXL_ESC: ESC %d version info response timeout", esc_id); + PX4_ERR("ESC %d version info response timeout", esc_id); } } @@ -232,7 +232,7 @@ int VoxlEsc::device_init() for (int esc_id = 0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; esc_id++) { if (_version_info[esc_id].sw_version == 0) { - PX4_ERR("VOXL_ESC: ESC ID %d was not detected", esc_id); + PX4_ERR("ESC ID %d was not detected", esc_id); esc_detection_fault = true; } } @@ -240,7 +240,7 @@ int VoxlEsc::device_init() //check the firmware hashes to make sure they are the same. Firmware hash has 8 chars plus optional "*" for (int esc_id = 1; esc_id < VOXL_ESC_OUTPUT_CHANNELS; esc_id++) { if (strncmp(_version_info[0].firmware_git_version, _version_info[esc_id].firmware_git_version, 9) != 0) { - PX4_ERR("VOXL_ESC: ESC %d Firmware hash does not match ESC 0 firmware hash: (%.12s) != (%.12s)", + PX4_ERR("ESC %d Firmware hash does not match ESC 0 firmware hash: (%.12s) != (%.12s)", esc_id, _version_info[esc_id].firmware_git_version, _version_info[0].firmware_git_version); esc_detection_fault = true; } @@ -256,13 +256,13 @@ int VoxlEsc::device_init() } if (esc_detection_fault) { - PX4_ERR("VOXL_ESC: Critical error during ESC initialization"); + PX4_ERR("Critical error during ESC initialization"); return -1; } - PX4_INFO("VOXL_ESC: Use extened rpm packet : %d", _extended_rpm); + PX4_ERR("Use extened rpm packet : %d", _extended_rpm); - PX4_INFO("VOXL_ESC: All ESCs successfully detected"); + PX4_ERR("All ESCs successfully detected"); _device_initialized = true; @@ -309,45 +309,53 @@ int VoxlEsc::load_params(voxl_esc_params_t *params, ch_assign_t *map) param_get(param_find("VOXL_ESC_T_WARN"), ¶ms->esc_warn_temp_threshold); param_get(param_find("VOXL_ESC_T_OVER"), ¶ms->esc_over_temp_threshold); + param_get(param_find("VOXL_ESC_GPIO_CH"), ¶ms->gpio_ctl_channel); + if (params->rpm_min >= params->rpm_max) { - PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_RPM_MIN. Please verify parameters."); + PX4_ERR("Invalid parameter VOXL_ESC_RPM_MIN. Please verify parameters."); params->rpm_min = 0; ret = PX4_ERROR; } if (params->turtle_motor_percent < 0 || params->turtle_motor_percent > 100) { - PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_T_PERC. Please verify parameters."); + PX4_ERR("Invalid parameter VOXL_ESC_T_PERC. Please verify parameters."); params->turtle_motor_percent = 0; ret = PX4_ERROR; } if (params->turtle_motor_deadband < 0 || params->turtle_motor_deadband > 100) { - PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_T_DEAD. Please verify parameters."); + PX4_ERR("Invalid parameter VOXL_ESC_T_DEAD. Please verify parameters."); params->turtle_motor_deadband = 0; ret = PX4_ERROR; } if (params->turtle_motor_expo < 0 || params->turtle_motor_expo > 100) { - PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_T_EXPO. Please verify parameters."); + PX4_ERR("Invalid parameter VOXL_ESC_T_EXPO. Please verify parameters."); params->turtle_motor_expo = 0; ret = PX4_ERROR; } if (params->turtle_stick_minf < 0.0f || params->turtle_stick_minf > 100.0f) { - PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_T_MINF. Please verify parameters."); + PX4_ERR("Invalid parameter VOXL_ESC_T_MINF. Please verify parameters."); params->turtle_stick_minf = 0.0f; ret = PX4_ERROR; } if (params->turtle_cosphi < 0.0f || params->turtle_cosphi > 100.0f) { - PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_T_COSP. Please verify parameters."); + PX4_ERR("Invalid parameter VOXL_ESC_T_COSP. Please verify parameters."); params->turtle_cosphi = 0.0f; ret = PX4_ERROR; } + if (params->gpio_ctl_channel < 0 || params->gpio_ctl_channel > 6) { + PX4_ERR("Invalid parameter VOXL_ESC_GPIO_CH. Please verify parameters."); + params->gpio_ctl_channel = 0; + ret = PX4_ERROR; + } + for (int i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++) { if (params->function_map[i] < (int)OutputFunction::Motor1 || params->function_map[i] > (int)OutputFunction::Motor4) { - PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_FUNCX. Only supports motors 1-4. Please verify parameters."); + PX4_ERR("Invalid parameter VOXL_ESC_FUNCX. Only supports motors 1-4. Please verify parameters."); params->function_map[i] = 0; ret = PX4_ERROR; @@ -365,7 +373,7 @@ int VoxlEsc::load_params(voxl_esc_params_t *params, ch_assign_t *map) if (params->motor_map[i] == VOXL_ESC_OUTPUT_DISABLED || params->motor_map[i] < -(VOXL_ESC_OUTPUT_CHANNELS) || params->motor_map[i] > VOXL_ESC_OUTPUT_CHANNELS) { - PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_MOTORX. Please verify parameters."); + PX4_ERR("Invalid parameter VOXL_ESC_MOTORX. Please verify parameters."); params->motor_map[i] = 0; ret = PX4_ERROR; } @@ -471,9 +479,8 @@ int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback) uint32_t voltage = fb.voltage; int32_t current = fb.current * 8; int32_t temperature = fb.temperature / 100; - PX4_INFO("VOXL_ESC: [%" PRId64 "] ID_RAW=%d ID=%d, RPM=%5d, PWR=%3d%%, V=%5dmV, I=%+5dmA, T=%+3dC", tnow, (int)id, - motor_idx + 1, - (int)rpm, (int)power, (int)voltage, (int)current, (int)temperature); + PX4_ERR("[%" PRId64 "] ID_RAW=%d ID=%d, RPM=%5d, PWR=%3d%%, V=%5dmV, I=%+5dmA, T=%+3dC", tnow, (int)id, + motor_idx + 1, (int)rpm, (int)power, (int)voltage, (int)current, (int)temperature); } _esc_chans[id].rate_meas = fb.rpm; @@ -541,24 +548,24 @@ int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback) QC_ESC_VERSION_INFO ver; memcpy(&ver, _fb_packet.buffer, packet_size); - PX4_INFO("VOXL_ESC: ESC ID: %i", ver.id); - PX4_INFO("VOXL_ESC: HW Version: %i", ver.hw_version); - PX4_INFO("VOXL_ESC: SW Version: %i", ver.sw_version); - PX4_INFO("VOXL_ESC: Unique ID: %i", (int)ver.unique_id); + PX4_ERR("ESC ID: %i", ver.id); + PX4_ERR("HW Version: %i", ver.hw_version); + PX4_ERR("SW Version: %i", ver.sw_version); + PX4_ERR("Unique ID: %i", (int)ver.unique_id); } else if (packet_type == ESC_PACKET_TYPE_VERSION_EXT_RESPONSE && packet_size == sizeof(QC_ESC_EXTENDED_VERSION_INFO)) { QC_ESC_EXTENDED_VERSION_INFO ver; memcpy(&ver, _fb_packet.buffer, packet_size); - PX4_INFO("VOXL_ESC: \tESC ID : %i", ver.id); - PX4_INFO("VOXL_ESC: \tBoard : %i", ver.hw_version); - PX4_INFO("VOXL_ESC: \tSW Version : %i", ver.sw_version); + PX4_ERR("\tESC ID : %i", ver.id); + PX4_ERR("\tBoard : %i", ver.hw_version); + PX4_ERR("\tSW Version : %i", ver.sw_version); uint8_t *u = &ver.unique_id[0]; - PX4_INFO("VOXL_ESC: \tUnique ID : 0x%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X", - u[11], u[10], u[9], u[8], u[7], u[6], u[5], u[4], u[3], u[2], u[1], u[0]); + PX4_ERR("\tUnique ID : 0x%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X", + u[11], u[10], u[9], u[8], u[7], u[6], u[5], u[4], u[3], u[2], u[1], u[0]); - PX4_INFO("VOXL_ESC: \tFirmware : version %4d, hash %.12s", ver.sw_version, ver.firmware_git_version); - PX4_INFO("VOXL_ESC: \tBootloader : version %4d, hash %.12s", ver.bootloader_version, ver.bootloader_git_version); + PX4_ERR("\tFirmware : version %4d, hash %.12s", ver.sw_version, ver.firmware_git_version); + PX4_ERR("\tBootloader : version %4d, hash %.12s", ver.bootloader_version, ver.bootloader_git_version); } else if (packet_type == ESC_PACKET_TYPE_FB_POWER_STATUS && packet_size == sizeof(QC_ESC_FB_POWER_STATUS)) { QC_ESC_FB_POWER_STATUS packet; @@ -671,7 +678,7 @@ int VoxlEsc::custom_command(int argc, char *argv[]) } if (!is_running()) { - PX4_INFO("VOXL_ESC:Not running"); + PX4_INFO("Not running"); return -1; } @@ -730,7 +737,7 @@ int VoxlEsc::custom_command(int argc, char *argv[]) if (!strcmp(verb, "reset")) { if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) { - PX4_INFO("VOXL_ESC: Reset ESC: %i", esc_id); + PX4_ERR("Reset ESC: %i", esc_id); cmd.len = qc_esc_create_reset_packet(esc_id, cmd.buf, sizeof(cmd.buf)); cmd.response = false; return get_instance()->send_cmd_thread_safe(&cmd); @@ -742,7 +749,7 @@ int VoxlEsc::custom_command(int argc, char *argv[]) } else if (!strcmp(verb, "version")) { if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) { - PX4_INFO("VOXL_ESC: Request version for ESC: %i", esc_id); + PX4_ERR("Request version for ESC: %i", esc_id); cmd.len = qc_esc_create_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf)); cmd.response = true; cmd.resp_delay_us = 2000; @@ -755,7 +762,7 @@ int VoxlEsc::custom_command(int argc, char *argv[]) } else if (!strcmp(verb, "version-ext")) { if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) { - PX4_INFO("VOXL_ESC: Request extended version for ESC: %i", esc_id); + PX4_ERR("Request extended version for ESC: %i", esc_id); cmd.len = qc_esc_create_extended_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf)); cmd.response = true; cmd.resp_delay_us = 5000; @@ -768,7 +775,7 @@ int VoxlEsc::custom_command(int argc, char *argv[]) } else if (!strcmp(verb, "tone")) { if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) { - PX4_INFO("VOXL_ESC: Request tone for ESC mask: %i", esc_id); + PX4_ERR("Request tone for ESC mask: %i", esc_id); cmd.len = qc_esc_create_sound_packet(period, duration, power, esc_id, cmd.buf, sizeof(cmd.buf)); cmd.response = false; return get_instance()->send_cmd_thread_safe(&cmd); @@ -782,7 +789,7 @@ int VoxlEsc::custom_command(int argc, char *argv[]) if (led_mask <= 0x0FFF) { get_instance()->_led_rsc.test = true; get_instance()->_led_rsc.breath_en = false; - PX4_INFO("VOXL_ESC: Request LED control for ESCs with mask: %i", led_mask); + PX4_ERR("Request LED control for ESCs with mask: %i", led_mask); get_instance()->_esc_chans[0].led = (led_mask & 0x0007); get_instance()->_esc_chans[1].led = (led_mask & 0x0038) >> 3; @@ -797,7 +804,7 @@ int VoxlEsc::custom_command(int argc, char *argv[]) } else if (!strcmp(verb, "rpm")) { if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) { - PX4_INFO("VOXL_ESC: Request RPM for ESC ID: %i - RPM: %i", esc_id, rate); + PX4_ERR("Request RPM for ESC ID: %i - RPM: %i", esc_id, rate); int16_t rate_req[VOXL_ESC_OUTPUT_CHANNELS] = {0, 0, 0, 0}; uint8_t id_fb = 0; @@ -831,8 +838,8 @@ int VoxlEsc::custom_command(int argc, char *argv[]) cmd.repeat_delay_us = repeat_delay_us; cmd.print_feedback = true; - PX4_INFO("VOXL_ESC: Feedback id debug: %i", id_fb); - PX4_INFO("VOXL_ESC: Sending UART ESC RPM command %i", rate); + PX4_ERR("Feedback id debug: %i", id_fb); + PX4_ERR("Sending UART ESC RPM command %i", rate); return get_instance()->send_cmd_thread_safe(&cmd); @@ -843,7 +850,7 @@ int VoxlEsc::custom_command(int argc, char *argv[]) } else if (!strcmp(verb, "pwm")) { if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) { - PX4_INFO("VOXL_ESC: Request PWM for ESC ID: %i - PWM: %i", esc_id, rate); + PX4_ERR("Request PWM for ESC ID: %i - PWM: %i", esc_id, rate); int16_t rate_req[VOXL_ESC_OUTPUT_CHANNELS] = {0, 0, 0, 0}; uint8_t id_fb = 0; @@ -876,8 +883,8 @@ int VoxlEsc::custom_command(int argc, char *argv[]) cmd.repeat_delay_us = repeat_delay_us; cmd.print_feedback = true; - PX4_INFO("VOXL_ESC: Feedback id debug: %i", id_fb); - PX4_INFO("VOXL_ESC: Sending UART ESC power command %i", rate); + PX4_ERR("Feedback id debug: %i", id_fb); + PX4_ERR("Sending UART ESC power command %i", rate); return get_instance()->send_cmd_thread_safe(&cmd); @@ -1242,14 +1249,58 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], _extended_rpm); if (_uart_port.write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("VOXL_ESC: Failed to send packet"); + PX4_ERR("Failed to send packet"); return false; } + // Track and manage gpio command writes + bool write_gpio_command = false; + + if (_gpio_ctl_en) { + if (_gpio_ctl_high != _prev_gpio_ctl_high) { + _gpio_write_counter = 0; + } + + if (_gpio_write_counter < 10) { + write_gpio_command = true; + _gpio_write_counter++; + } + + _prev_gpio_ctl_high = _gpio_ctl_high; + + if (write_gpio_command) { + Command gpio_cmd; + const int ESC_PACKET_TYPE_GPIO_CMD = 15; + uint8_t data[5]; + + int esc_id = 0; // In future un-hardcode + int val = 0; + + if (_gpio_ctl_high) { + val = 1; + } + + data[0] = esc_id; // esc id + data[1] = 80; // 01010000 : pin F0 + data[2] = 0; // 0: output, 1: input + data[3] = val; //cmd LSB + data[4] = 0; // cmd MSB + + // type, data, size + gpio_cmd.len = qc_esc_create_packet(ESC_PACKET_TYPE_GPIO_CMD, (uint8_t *) & (data[0]), 5, gpio_cmd.buf, + sizeof(gpio_cmd.buf)); + + if (_uart_port.write(gpio_cmd.buf, gpio_cmd.len) != gpio_cmd.len) { + PX4_ERR("Failed to send gpio packet"); + return false; + } + } + + } + // increment ESC id from which to request feedback in round robin order _fb_idx = (_fb_idx + 1) % VOXL_ESC_OUTPUT_CHANNELS; - /* * Here we read and parse response from ESCs. Since the latest command has just been sent out, * the response packet we may read here is probabaly from previous iteration, but it is totally ok. @@ -1283,22 +1334,6 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], _esc_status_pub.publish(_esc_status); - // If any extra external modal io data has been received then - // send it over as well - while (_voxl2_io_data_sub.updated()) { - buffer128_s io_data{}; - _voxl2_io_data_sub.copy(&io_data); - - // PX4_INFO("Got Modal IO data: %u bytes", io_data.len); - // PX4_INFO(" 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x", - // io_data.data[0], io_data.data[1], io_data.data[2], io_data.data[3], - // io_data.data[4], io_data.data[5], io_data.data[6], io_data.data[7]); - if (_uart_port.write(io_data.data, io_data.len) != io_data.len) { - PX4_ERR("VOXL_ESC: Failed to send modal io data to esc"); - return false; - } - } - perf_count(_output_update_perf); return true; @@ -1308,7 +1343,7 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], void VoxlEsc::Run() { if (should_exit()) { - PX4_ERR("VOXL_ESC: Stopping the module"); + PX4_ERR("Stopping the module"); ScheduleClear(); _mixing_output.unregister(); @@ -1328,12 +1363,12 @@ void VoxlEsc::Run() int dev_init_ret = device_init(); if (dev_init_ret != 0) { - PX4_WARN("VOXL_ESC: Failed to initialize device, retries left %d", retries_left); + PX4_WARN("Failed to initialize device, retries left %d", retries_left); } } if (!_device_initialized) { - PX4_ERR("VOXL_ESC: Failed to initialize device, exiting the module"); + PX4_ERR("Failed to initialize device, exiting the module"); ScheduleClear(); _mixing_output.unregister(); exit_and_cleanup(); @@ -1374,11 +1409,12 @@ void VoxlEsc::Run() update_leds(_led_rsc.mode, _led_rsc.control); } - if (_parameters.mode > 0) { - /* if turtle mode enabled, we go straight to the sticks, no mix */ - if (_manual_control_setpoint_sub.updated()) { + /* check whether sticks have been updated */ + if (_manual_control_setpoint_sub.updated()) { + _manual_control_setpoint_sub.copy(&_manual_control_setpoint); - _manual_control_setpoint_sub.copy(&_manual_control_setpoint); + // if turtle mode enabled, we go straight to the sticks, no mix + if (_parameters.mode > 0) { if (!_outputs_on) { @@ -1400,6 +1436,45 @@ void VoxlEsc::Run() } } + // check if gpio control is enabled + if (_parameters.gpio_ctl_channel > 0) { + + _gpio_ctl_en = true; + float gpio_setpoint = VOXL_ESC_GPIO_CTL_DISABLED_SETPOINT; + + switch (_parameters.gpio_ctl_channel) { + case VOXL_ESC_GPIO_CTL_AUX1: + gpio_setpoint = _manual_control_setpoint.aux1; + break; + + case VOXL_ESC_GPIO_CTL_AUX2: + gpio_setpoint = _manual_control_setpoint.aux2; + break; + + case VOXL_ESC_GPIO_CTL_AUX3: + gpio_setpoint = _manual_control_setpoint.aux3; + break; + + case VOXL_ESC_GPIO_CTL_AUX4: + gpio_setpoint = _manual_control_setpoint.aux4; + break; + + case VOXL_ESC_GPIO_CTL_AUX5: + gpio_setpoint = _manual_control_setpoint.aux5; + break; + + case VOXL_ESC_GPIO_CTL_AUX6: + gpio_setpoint = _manual_control_setpoint.aux6; + break; + } + + if (gpio_setpoint > VOXL_ESC_GPIO_CTL_THRESHOLD) { + _gpio_ctl_high = false; + + } else { + _gpio_ctl_high = true; + } + } } if (!_outputs_on) { @@ -1431,19 +1506,19 @@ void VoxlEsc::Run() } else { if (_current_cmd.retries == 0) { _current_cmd.clear(); - PX4_ERR("VOXL_ESC: Failed to send command, errno: %i", errno); + PX4_ERR("Failed to send command, errno: %i", errno); } else { _current_cmd.retries--; - PX4_ERR("VOXL_ESC: Failed to send command, errno: %i", errno); + PX4_ERR("Failed to send command, errno: %i", errno); } } px4_usleep(_current_cmd.repeat_delay_us); } while (_current_cmd.repeats-- > 0); - PX4_INFO("VOXL_ESC: RX packet count: %d", (int)_rx_packet_count); - PX4_INFO("VOXL_ESC: CRC error count: %d", (int)_rx_crc_error_count); + PX4_ERR("RX packet count: %d", (int)_rx_packet_count); + PX4_ERR("CRC error count: %d", (int)_rx_crc_error_count); } else { Command *new_cmd = _pending_cmd.load(); @@ -1550,6 +1625,8 @@ void VoxlEsc::print_params() PX4_INFO("Params: VOXL_ESC_T_WARN: %" PRId32, _parameters.esc_warn_temp_threshold); PX4_INFO("Params: VOXL_ESC_T_OVER: %" PRId32, _parameters.esc_over_temp_threshold); + + PX4_INFO("Params: VOXL_ESC_GPIO_CH: %" PRId32, _parameters.gpio_ctl_channel); } int VoxlEsc::print_status() diff --git a/src/drivers/actuators/voxl_esc/voxl_esc.hpp b/src/drivers/actuators/voxl_esc/voxl_esc.hpp index d894cb9326..0fe6f9fd42 100644 --- a/src/drivers/actuators/voxl_esc/voxl_esc.hpp +++ b/src/drivers/actuators/voxl_esc/voxl_esc.hpp @@ -49,7 +49,6 @@ #include #include #include -#include #include @@ -129,17 +128,24 @@ private: static constexpr uint32_t VOXL_ESC_MODE_TURTLE_AUX1 = 1; static constexpr uint32_t VOXL_ESC_MODE_TURTLE_AUX2 = 2; - static constexpr uint16_t VOXL_ESC_EXT_RPM = - 39; // minimum firmware version for extended RPM command support - static constexpr uint16_t VOXL_ESC_RPM_MAX = INT16_MAX - - 1; // 32K, Limit max standard range RPM to prevent overflow (rpm packet packing function accepts int32_t) - static constexpr uint16_t VOXL_ESC_RPM_MAX_EXT = UINT16_MAX - - 5; // 65K, Limit max extended range RPM to prevent overflow (rpm packet packing function accepts int32_t) + // minimum firmware version for extended RPM command support + static constexpr uint16_t VOXL_ESC_EXT_RPM = 39; + // 32K, Limit max standard range RPM to prevent overflow (rpm packet packing function accepts int32_t) + static constexpr uint16_t VOXL_ESC_RPM_MAX = INT16_MAX - 1; + // 65K, Limit max extended range RPM to prevent overflow (rpm packet packing function accepts int32_t) + static constexpr uint16_t VOXL_ESC_RPM_MAX_EXT = UINT16_MAX - 5; static constexpr uint16_t VOXL_ESC_NUM_INIT_RETRIES = 3; - //static constexpr uint16_t max_pwm(uint16_t pwm) { return math::min(pwm, VOXL_ESC_PWM_MAX); } - //static constexpr uint16_t max_rpm(uint16_t rpm) { return math::min(rpm, VOXL_ESC_RPM_MAX); } + static constexpr float VOXL_ESC_GPIO_CTL_DISABLED_SETPOINT = -0.1f; + static constexpr float VOXL_ESC_GPIO_CTL_THRESHOLD = 0.0f; + + static constexpr uint32_t VOXL_ESC_GPIO_CTL_AUX1 = 1; + static constexpr uint32_t VOXL_ESC_GPIO_CTL_AUX2 = 2; + static constexpr uint32_t VOXL_ESC_GPIO_CTL_AUX3 = 3; + static constexpr uint32_t VOXL_ESC_GPIO_CTL_AUX4 = 4; + static constexpr uint32_t VOXL_ESC_GPIO_CTL_AUX5 = 5; + static constexpr uint32_t VOXL_ESC_GPIO_CTL_AUX6 = 6; Serial _uart_port{}; @@ -161,6 +167,7 @@ private: int32_t publish_battery_status{0}; int32_t esc_warn_temp_threshold{0}; int32_t esc_over_temp_threshold{0}; + int32_t gpio_ctl_channel{0}; } voxl_esc_params_t; struct EscChan { @@ -205,7 +212,6 @@ private: uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _actuator_test_sub{ORB_ID(actuator_test)}; uORB::Subscription _led_update_sub{ORB_ID(led_control)}; - uORB::Subscription _voxl2_io_data_sub{ORB_ID(voxl2_io_data)}; uORB::Publication _outputs_debug_pub{ORB_ID(actuator_outputs_debug)}; uORB::Publication _esc_status_pub{ORB_ID(esc_status)}; @@ -224,6 +230,11 @@ private: int32_t _rpm_fullscale{0}; manual_control_setpoint_s _manual_control_setpoint{}; + int32_t _gpio_write_counter{0}; + bool _gpio_ctl_en{false}; + bool _gpio_ctl_high{true}; + bool _prev_gpio_ctl_high{true}; + uint16_t _cmd_id{0}; Command _current_cmd; px4::atomic _pending_cmd{nullptr}; diff --git a/src/drivers/actuators/voxl_esc/voxl_esc_params.c b/src/drivers/actuators/voxl_esc/voxl_esc_params.c index 2752d9d744..b0526ecf1f 100644 --- a/src/drivers/actuators/voxl_esc/voxl_esc_params.c +++ b/src/drivers/actuators/voxl_esc/voxl_esc_params.c @@ -262,3 +262,17 @@ PARAM_DEFINE_INT32(VOXL_ESC_T_WARN, 0); * @max 200 */ PARAM_DEFINE_INT32(VOXL_ESC_T_OVER, 0); + + +/** + * GPIO Control Channel + * + * + * @reboot_required true + * + * @group VOXL ESC + * @value 0 - Disabled + * @min 0 + * @max 6 + */ +PARAM_DEFINE_INT32(VOXL_ESC_GPIO_CH, 0); From af311c8d45192c398fe77ad2e02c915fdbefd5ee Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Tue, 18 Feb 2025 17:24:14 -0900 Subject: [PATCH 39/91] gz: update submodule --- Tools/simulation/gz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/simulation/gz b/Tools/simulation/gz index db4af69088..183cbee9e2 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit db4af69088cccef4549cf3a5c195d5cd97d6b36a +Subproject commit 183cbee9e2250d1ca5517cd76c5d9a7e47cc0b7a From 786c2a7f397a525c5d0e6666144a566f9ad65ff4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 20 Feb 2025 06:09:54 +1300 Subject: [PATCH 40/91] drivers/imu/invensense/mpu6000: include cstdlib for size_t --- .../imu/invensense/mpu6000/InvenSense_MPU6000_registers.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/imu/invensense/mpu6000/InvenSense_MPU6000_registers.hpp b/src/drivers/imu/invensense/mpu6000/InvenSense_MPU6000_registers.hpp index 768f4a3de9..b53a6dab43 100644 --- a/src/drivers/imu/invensense/mpu6000/InvenSense_MPU6000_registers.hpp +++ b/src/drivers/imu/invensense/mpu6000/InvenSense_MPU6000_registers.hpp @@ -41,6 +41,7 @@ #pragma once #include +#include // TODO: move to a central header static constexpr uint8_t Bit0 = (1 << 0); From 1db50cb331f1464bdd6bbd53b3690e36a9d022dc Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Tue, 18 Feb 2025 10:01:32 -0900 Subject: [PATCH 41/91] mavlink: add missing fields for DISTANCE_SENSOR --- src/modules/mavlink/streams/DISTANCE_SENSOR.hpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/modules/mavlink/streams/DISTANCE_SENSOR.hpp b/src/modules/mavlink/streams/DISTANCE_SENSOR.hpp index 9042403a48..ddabecfba6 100644 --- a/src/modules/mavlink/streams/DISTANCE_SENSOR.hpp +++ b/src/modules/mavlink/streams/DISTANCE_SENSOR.hpp @@ -94,6 +94,22 @@ private: msg.min_distance = dist_sensor.min_distance * 1e2f; // m to cm msg.orientation = dist_sensor.orientation; msg.covariance = dist_sensor.variance * 1e4f; // m^2 to cm^2 + msg.horizontal_fov = dist_sensor.h_fov; + msg.vertical_fov = dist_sensor.v_fov; + msg.quaternion[0] = dist_sensor.q[0]; + msg.quaternion[1] = dist_sensor.q[1]; + msg.quaternion[2] = dist_sensor.q[2]; + msg.quaternion[3] = dist_sensor.q[3]; + + if (dist_sensor.signal_quality < 0) { + msg.signal_quality = 0; + + } else if (dist_sensor.signal_quality == 0) { + msg.signal_quality = 1; + + } else { + msg.signal_quality = dist_sensor.signal_quality; + } mavlink_msg_distance_sensor_send_struct(_mavlink->get_channel(), &msg); From 86bfac7c8f9bc865f17c073bb15bbc21391e66c2 Mon Sep 17 00:00:00 2001 From: GuillaumeLaine Date: Wed, 19 Feb 2025 11:38:26 +0100 Subject: [PATCH 42/91] fix Jenkins: add copy safeguard and copy to unified folder --- Jenkinsfile | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Jenkinsfile b/Jenkinsfile index 9e9f5787c8..fb092cd83d 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -235,9 +235,9 @@ pipeline { sh('rm -f px4_msgs/srv/*.srv') sh('rm -f px4_msgs/srv/versioned/*.srv') sh('cp msg/*.msg px4_msgs/msg/') - sh('mkdir -p px4_msgs/msg/versioned && cp msg/versioned/*.msg px4_msgs/msg/versioned/') + sh('cp msg/versioned/*.msg px4_msgs/msg/ || true') sh('cp srv/*.srv px4_msgs/srv/') - sh('mkdir -p px4_msgs/srv/versioned && cp srv/versioned/*.srv px4_msgs/srv/versioned/') + sh('cp srv/versioned/*.srv px4_msgs/srv/ || true') sh('cd px4_msgs; git status; git add .; git commit -a -m "Update message definitions `date`" || true') sh('cd px4_msgs; git push origin main || true') sh('rm -rf px4_msgs') From 44d5bfc42c9726948c40beb5c545cc4045e97b29 Mon Sep 17 00:00:00 2001 From: GuillaumeLaine Date: Wed, 19 Feb 2025 17:36:46 +0100 Subject: [PATCH 43/91] Tools: in px4_msgs copy versioned msgs to common root folder --- Tools/copy_to_ros_ws.sh | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/Tools/copy_to_ros_ws.sh b/Tools/copy_to_ros_ws.sh index ef663aaaa9..48fcaa0d1f 100755 --- a/Tools/copy_to_ros_ws.sh +++ b/Tools/copy_to_ros_ws.sh @@ -29,6 +29,5 @@ then rm -rf "${PX4_MSGS_DIR}"/srv/*.srv fi cp -ar "${PX4_SRC_DIR}"/msg/*.msg "${PX4_MSGS_DIR}"/msg -mkdir -p "${PX4_MSGS_DIR}"/msg/versioned -cp -ar "${PX4_SRC_DIR}"/msg/versioned/*.msg "${PX4_MSGS_DIR}"/msg/versioned +cp -ar "${PX4_SRC_DIR}"/msg/versioned/*.msg "${PX4_MSGS_DIR}"/msg cp -ar "${PX4_SRC_DIR}"/srv/*.srv "${PX4_MSGS_DIR}"/srv From 29317d90c338229720bebb369a5fe4a96c7383e6 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 20 Feb 2025 09:19:43 +0100 Subject: [PATCH 44/91] RPM capture: improve description of param RPM_CAP_ENABLE (#24368) * RPM capture: improve description of param RPM_CAP_ENABLE --------- Signed-off-by: Silvan Fuhrer Co-authored-by: Matthias Grob --- src/drivers/rpm_capture/rpm_capture_params.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/rpm_capture/rpm_capture_params.c b/src/drivers/rpm_capture/rpm_capture_params.c index 13bd3fb7a7..5402d089ab 100644 --- a/src/drivers/rpm_capture/rpm_capture_params.c +++ b/src/drivers/rpm_capture/rpm_capture_params.c @@ -32,9 +32,9 @@ ****************************************************************************/ /** - * RPM Capture Enable + * RPM capture enable * - * Enables the RPM capture module on FMU channel 5. + * Enables the RPM capture module to estimate RPM from pulses detected on a PWM pin configured as "RPM Input". * * @boolean * @group System From 24da87db1218fe0e3270d8025e2851923139b0b9 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 20 Feb 2025 09:28:27 +0100 Subject: [PATCH 45/91] FW Position Control: keep flaps in landing config during abort if below AIRSPD_MIN (#23877) Signed-off-by: Silvan Fuhrer --- src/modules/fw_pos_control/FixedwingPositionControl.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 5a55be1cc8..e939a56f54 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -1316,7 +1316,15 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons } else { // continue straight until vehicle has sufficient altitude + // keep flaps in landing configuration if the airspeed is below the min airspeed (keep deployed if airspeed not valid) roll_body = 0.0f; + + if (!_airspeed_valid || _airspeed_eas < _performance_model.getMinimumCalibratedAirspeed()) { + _flaps_setpoint = _param_fw_flaps_lnd_scl.get(); + + } else { + _flaps_setpoint = 0.f; + } } is_low_height = true; // In low-height flight, TECS will control altitude tighter From 4ea7de449a5a92209b3243fa3b9ec18cbac3d5c7 Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Fri, 21 Feb 2025 08:34:08 +1100 Subject: [PATCH 46/91] InternalCombustionEngineControl - doc corrections (#24359) * InternalCombustionEngineControl - doc corrections * Update InternalCombustionEngineControl.cpp --- .../InternalCombustionEngineControl.cpp | 45 ++++++++++++++----- 1 file changed, 33 insertions(+), 12 deletions(-) diff --git a/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.cpp b/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.cpp index f4ab6a05aa..1e8a1b1a35 100644 --- a/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.cpp +++ b/src/modules/internal_combustion_engine_control/InternalCombustionEngineControl.cpp @@ -355,31 +355,52 @@ int InternalCombustionEngineControl::print_usage(const char *reason) PRINT_MODULE_DESCRIPTION( R"DESCR_STR( ### Description + The module controls internal combustion engine (ICE) features including: -ignition (on/off),throttle and choke level, starter engine delay, and user request. -The module publishes [InternalCombustionEngineControl.msg](../msg_docs/InternalCombustionEngineControl.md). -The architecture is as shown below.: -![Architecture](../../assets/diagrams/ice_control_diagram.png) +ignition (on/off), throttle and choke level, starter engine delay, and user request. ### Enabling + This feature is not enabled by default needs to be configured in the build target for your board together with the rpm capture driver: + +``` CONFIG_MODULES_INTERNAL_COMBUSTION_ENGINE_CONTROL=y CONFIG_DRIVERS_RPM_CAPTURE=y +``` Additionally, to enable the module: -- set [ICE_EN](https://docs.px4.io/main/en/advanced_config/parameter_reference.html#ICE_EN) -to true and adjust the other module parameters ICE_ according to your needs. -- set [RPM_CAP_ENABLE](https://docs.px4.io/main/en/advanced_config/parameter_reference.html#RPM_CAP_ENABLE) to true. + +- Set [ICE_EN](../advanced_config/parameter_reference.md#ICE_EN) +to true and adjust the other `ICE_` module parameters according to your needs. +- Set [RPM_CAP_ENABLE](../advanced_config/parameter_reference.md#RPM_CAP_ENABLE) to true. + +The module outputs control signals for ignition, throttle, and choke, +and takes inputs from an RPM sensor. +These must be mapped to AUX outputs/inputs in the [Actuator configuration](../config/actuators.md), +similar to the setup shown below. + +![Actuator setup for ICE](../../assets/hardware/ice/ice_actuator_setup.png) ### Implementation + The ICE is implemented with a (4) state machine: -![Architecture](../../assets/diagrams/ice_control_state_machine.png) + +![Architecture](../../assets/hardware/ice/ice_control_state_machine.png) + The state machine: -- checks if [Rpm.msg](../msg_docs/Rpm.md) is updated to know if the engine is running -- allows for user inputs from -- AUX{N} -- Arming state in [VehicleStatus.msg(../msg_docs/VehicleStatus.md) + +- Checks if [Rpm.msg](../msg_docs/Rpm.md) is updated to know if the engine is running +- Allows for user inputs from: + - AUX{N} + - Arming state in [VehicleStatus.msg](../msg_docs/VehicleStatus.md) + +The module publishes [InternalCombustionEngineControl.msg](../msg_docs/InternalCombustionEngineControl.md). + +The architecture is as shown below: + +![Architecture](../../assets/hardware/ice/ice_control_diagram.png) + )DESCR_STR"); From 11704985f366559c323127ecae837321ebafac0e Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Fri, 21 Feb 2025 14:18:56 -0900 Subject: [PATCH 47/91] ci: add write permissions for flash_analysis workflow (#24393) --- .github/workflows/flash_analysis.yml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.github/workflows/flash_analysis.yml b/.github/workflows/flash_analysis.yml index 8ab97fdda8..6e965ab2c2 100644 --- a/.github/workflows/flash_analysis.yml +++ b/.github/workflows/flash_analysis.yml @@ -1,5 +1,9 @@ name: FLASH usage analysis +permissions: + pull-requests: write + issues: write + on: push: branches: From 73b51242c80de7220b189e771eafc1283499870d Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Sun, 16 Feb 2025 13:25:08 -0900 Subject: [PATCH 48/91] nuttx: h7: eth: add checks for PHYID in stm32_phyinit arch: stm32h7: Add support for dual bank flash memory --- platforms/nuttx/NuttX/nuttx | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/platforms/nuttx/NuttX/nuttx b/platforms/nuttx/NuttX/nuttx index 9a213327fb..205b3100f8 160000 --- a/platforms/nuttx/NuttX/nuttx +++ b/platforms/nuttx/NuttX/nuttx @@ -1 +1 @@ -Subproject commit 9a213327fbb2dab252185d468fb29348e1833d8c +Subproject commit 205b3100f8f63944a45faa5cfb5d3f86e904ee59 From 35d96d57f9e13c672fe62b3231128ae03eb26628 Mon Sep 17 00:00:00 2001 From: Ted <86834177+TedObrien@users.noreply.github.com> Date: Mon, 24 Feb 2025 12:17:11 +0000 Subject: [PATCH 49/91] control_allocator: Added linearization feature for 4 servo swash plates to prevent binding (#23961) * control_allocator: Added linearization feature for heli swashplates to help prevent servo binding * Apply suggestions from code review Co-authored-by: Mathieu Bresciani * update description of CA_LIN_SERVO parameter * update variable name * add missing semi-colon * fix variable referenced before assignment * add missing indentation Co-authored-by: Mathieu Bresciani * removed param unnecessary param * removed whitespace * remove CA_LIN_SERVO and enable feature if CA_MAX_SERVO_THROW > 0 plus Update param description. * remove CA_MAX_SVO_THROW from actuators tab to avoid confusion during standard swashplate setup. * added comment and fixed spelling mistake * fix spelling mistake * fix formatting * reduce CA_MAX_SVO_THROW short description length to stop test failure * ActuatorEffectivenessHelicopter: clarfification suggestions for servo linearization feature * remove NAN check. --------- Co-authored-by: Mathieu Bresciani Co-authored-by: Matthias Grob --- .../ActuatorEffectivenessHelicopter.cpp | 32 +++++++++++++++++++ .../ActuatorEffectivenessHelicopter.hpp | 5 +++ src/modules/control_allocator/module.yaml | 14 ++++++++ 3 files changed, 51 insertions(+) diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp index beacc7007b..967706a6d8 100644 --- a/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp @@ -65,6 +65,7 @@ ActuatorEffectivenessHelicopter::ActuatorEffectivenessHelicopter(ModuleParams *p _param_handles.yaw_throttle_scale = param_find("CA_HELI_YAW_TH_S"); _param_handles.yaw_ccw = param_find("CA_HELI_YAW_CCW"); _param_handles.spoolup_time = param_find("COM_SPOOLUP_TIME"); + _param_handles.max_servo_throw = param_find("CA_MAX_SVO_THROW"); updateParams(); } @@ -101,6 +102,21 @@ void ActuatorEffectivenessHelicopter::updateParams() int32_t yaw_ccw = 0; param_get(_param_handles.yaw_ccw, &yaw_ccw); _geometry.yaw_sign = (yaw_ccw == 1) ? -1.f : 1.f; + float max_servo_throw_deg = 0.f; + param_get(_param_handles.max_servo_throw, &max_servo_throw_deg); + + if (max_servo_throw_deg > 0.f) { + // linearization feature enabled + _geometry.linearize_servos = 1; + const float max_servo_throw = math::radians(max_servo_throw_deg); + _geometry.max_servo_height = sinf(max_servo_throw); + _geometry.inverse_max_servo_throw = 1.f / max_servo_throw; + + } else { + // handle any undefined behaviour if disabled + _geometry.linearize_servos = 0; + _geometry.max_servo_height = _geometry.inverse_max_servo_throw = 0.f; + } } bool ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &configuration, @@ -168,6 +184,11 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector Date: Mon, 24 Feb 2025 16:02:46 +0100 Subject: [PATCH 50/91] Performance & testing targets * Added minimal configs for performance testing * Rename recovery to performance * added mfg_cfg * fix params & don't inherit from default * rename performance -> performance-test --------- Co-authored-by: Igor-Misic --- ROMFS/performance-test/CMakeLists.txt | 34 +++ ROMFS/performance-test/init.d/CMakeLists.txt | 36 +++ ROMFS/performance-test/init.d/rcS | 68 ++++++ Tools/ci/generate_board_targets_json.py | 2 +- Tools/kconfig/allyesconfig.py | 1 + boards/px4/fmu-v5x/performance-test.px4board | 31 +++ boards/px4/fmu-v6x/performance-test.px4board | 31 +++ cmake/kconfig.cmake | 2 +- src/systemcmds/mft_cfg/CMakeLists.txt | 39 +++ src/systemcmds/mft_cfg/Kconfig | 5 + src/systemcmds/mft_cfg/mft_cfg.cpp | 239 +++++++++++++++++++ 11 files changed, 486 insertions(+), 2 deletions(-) create mode 100644 ROMFS/performance-test/CMakeLists.txt create mode 100644 ROMFS/performance-test/init.d/CMakeLists.txt create mode 100644 ROMFS/performance-test/init.d/rcS create mode 100644 boards/px4/fmu-v5x/performance-test.px4board create mode 100644 boards/px4/fmu-v6x/performance-test.px4board create mode 100644 src/systemcmds/mft_cfg/CMakeLists.txt create mode 100644 src/systemcmds/mft_cfg/Kconfig create mode 100644 src/systemcmds/mft_cfg/mft_cfg.cpp diff --git a/ROMFS/performance-test/CMakeLists.txt b/ROMFS/performance-test/CMakeLists.txt new file mode 100644 index 0000000000..56c6235ee2 --- /dev/null +++ b/ROMFS/performance-test/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +add_subdirectory(init.d) diff --git a/ROMFS/performance-test/init.d/CMakeLists.txt b/ROMFS/performance-test/init.d/CMakeLists.txt new file mode 100644 index 0000000000..8a08b20ab2 --- /dev/null +++ b/ROMFS/performance-test/init.d/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_romfs_files( + rcS +) diff --git a/ROMFS/performance-test/init.d/rcS b/ROMFS/performance-test/init.d/rcS new file mode 100644 index 0000000000..d79fe58c76 --- /dev/null +++ b/ROMFS/performance-test/init.d/rcS @@ -0,0 +1,68 @@ +#!/bin/sh +# Un comment and use set +e to ignore and set -e to enable 'exit on error control' +set +e +# Un comment the line below to help debug scripts by printing a trace of the script commands +#set -x +# PX4FMU startup script. +# +# NOTE: environment variable references: +# If the dollar sign ('$') is followed by a left bracket ('{') then the +# variable name is terminated with the right bracket character ('}'). +# Otherwise, the variable name goes to the end of the argument. +# +# +# NOTE: COMMENT LINES ARE REMOVED BEFORE STORED IN ROMFS. +# +#------------------------------------------------------------------------------ +set R / + +# +# Print full system version. +# +ver all + +# +# Set the parameter file the board supports params on +# MTD device. +# +if mft query -q -k MTD -s MTD_PARAMETERS -v /fs/mtd_params +then + set PARAM_FILE /fs/mtd_params +fi + +# +# Load parameters. +# +# if the board has a storage for (factory) calibration data +if mft query -q -k MTD -s MTD_CALDATA -v /fs/mtd_caldata +then + param load /fs/mtd_caldata +fi + +# +# Load parameters. +# +param select $PARAM_FILE +if ! param load +then + param reset_all +fi + +# +# Try to mount the microSD card. +# +mount -t vfat /dev/mmcsd0 /fs/microsd +if [ $? = 0 ] +then + echo "SD card mounted at /fs/microsd" +else + echo "No SD card found" +fi + +unset R + +echo "" +echo "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" +echo "!!!!!! This is the PERFORMANCE TESTING firmware! WARNs and ERRORs are expected! !!!!!" +echo "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" +echo "" diff --git a/Tools/ci/generate_board_targets_json.py b/Tools/ci/generate_board_targets_json.py index 218862cecd..5749a988d1 100755 --- a/Tools/ci/generate_board_targets_json.py +++ b/Tools/ci/generate_board_targets_json.py @@ -69,7 +69,7 @@ def process_target(px4board_file, target_name): group = None if px4board_file.endswith("default.px4board") or \ - px4board_file.endswith("recovery.px4board") or \ + px4board_file.endswith("performance-test.px4board") or \ px4board_file.endswith("bootloader.px4board"): kconf.load_config(px4board_file, replace=True) else: # Merge config with default.px4board diff --git a/Tools/kconfig/allyesconfig.py b/Tools/kconfig/allyesconfig.py index 2c0f58d75a..e432c52ec0 100644 --- a/Tools/kconfig/allyesconfig.py +++ b/Tools/kconfig/allyesconfig.py @@ -74,6 +74,7 @@ exception_list_sitl = [ 'SYSTEMCMDS_I2CDETECT', # Not supported in SITL 'SYSTEMCMDS_DMESG', # Not supported in SITL 'SYSTEMCMDS_USB_CONNECTED', # Not supported in SITL + 'SYSTEMCMDS_MFT_CFG', # Not supported in SITL 'MODULES_SPACECRAFT', # Clashes with Control Allocation (mom's spaghetti code) ] diff --git a/boards/px4/fmu-v5x/performance-test.px4board b/boards/px4/fmu-v5x/performance-test.px4board new file mode 100644 index 0000000000..5e69dd7ae3 --- /dev/null +++ b/boards/px4/fmu-v5x/performance-test.px4board @@ -0,0 +1,31 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ETHERNET=y +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS7" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1" +CONFIG_BOARD_SERIAL_EXT2="/dev/ttyS3" +CONFIG_BOARD_ROMFSROOT="performance-test" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_MFT_CFG=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SD_STRESS=y +CONFIG_SYSTEMCMDS_SERIAL_TEST=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/px4/fmu-v6x/performance-test.px4board b/boards/px4/fmu-v6x/performance-test.px4board new file mode 100644 index 0000000000..5e69dd7ae3 --- /dev/null +++ b/boards/px4/fmu-v6x/performance-test.px4board @@ -0,0 +1,31 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ETHERNET=y +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS7" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1" +CONFIG_BOARD_SERIAL_EXT2="/dev/ttyS3" +CONFIG_BOARD_ROMFSROOT="performance-test" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_MFT_CFG=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SD_STRESS=y +CONFIG_SYSTEMCMDS_SERIAL_TEST=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/cmake/kconfig.cmake b/cmake/kconfig.cmake index 7eaaa00d67..a5c79ad131 100644 --- a/cmake/kconfig.cmake +++ b/cmake/kconfig.cmake @@ -35,7 +35,7 @@ if(EXISTS ${BOARD_DEFCONFIG}) # Depend on BOARD_DEFCONFIG so that we reconfigure on config change set_property(DIRECTORY APPEND PROPERTY CMAKE_CONFIGURE_DEPENDS ${BOARD_DEFCONFIG}) - if(${LABEL} MATCHES "default" OR ${LABEL} MATCHES "recovery" OR ${LABEL} MATCHES "bootloader" OR ${LABEL} MATCHES "canbootloader") + if(${LABEL} MATCHES "default" OR ${LABEL} MATCHES "performance-test" OR ${LABEL} MATCHES "bootloader" OR ${LABEL} MATCHES "canbootloader") # Generate boardconfig from saved defconfig execute_process( COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} diff --git a/src/systemcmds/mft_cfg/CMakeLists.txt b/src/systemcmds/mft_cfg/CMakeLists.txt new file mode 100644 index 0000000000..ca1a3b5544 --- /dev/null +++ b/src/systemcmds/mft_cfg/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE systemcmds__mft_cfg + MAIN mft_cfg + COMPILE_FLAGS + SRCS + mft_cfg.cpp + ) diff --git a/src/systemcmds/mft_cfg/Kconfig b/src/systemcmds/mft_cfg/Kconfig new file mode 100644 index 0000000000..85091c6ff3 --- /dev/null +++ b/src/systemcmds/mft_cfg/Kconfig @@ -0,0 +1,5 @@ +menuconfig SYSTEMCMDS_MFT_CFG + bool "mft_cfg" + default n + ---help--- + Enable support for mft_cfg diff --git a/src/systemcmds/mft_cfg/mft_cfg.cpp b/src/systemcmds/mft_cfg/mft_cfg.cpp new file mode 100644 index 0000000000..c87c4e6bdc --- /dev/null +++ b/src/systemcmds/mft_cfg/mft_cfg.cpp @@ -0,0 +1,239 @@ +/**************************************************************************** +* +* Copyright (c) 2025 PX4 Development Team. All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in +* the documentation and/or other materials provided with the +* distribution. +* 3. Neither the name PX4 nor the names of its contributors may be +* used to endorse or promote products derived from this software +* without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +****************************************************************************/ + +/** +* @file mft_cfg.c +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +static void usage(const char *reason) +{ + if (reason != nullptr) { + printf("%s\n\n", reason); + } + + PRINT_MODULE_DESCRIPTION("Tool to set and get manifest configuration"); + + PRINT_MODULE_USAGE_NAME("mft_cfg", "command"); + PRINT_MODULE_USAGE_COMMAND_DESCR("get", "get manifest configuration"); + PRINT_MODULE_USAGE_COMMAND_DESCR("set", "set manifest configuration"); + PRINT_MODULE_USAGE_COMMAND_DESCR("reset", "reset manifest configuration"); + PRINT_MODULE_USAGE_ARG("hwver|hwrev", "Select type: MTD_MTF_VER|MTD_MTF_REV", false); + PRINT_MODULE_USAGE_PARAM_INT('i', 0x10, 0x10, 0xFFFF, + "argument to set extended hardware id (id == version for , id == revision for )", false); + +} + + +static int print_extended_id(const char *type) +{ + mtd_mft_v0_t mtd_mft; + mtd_mft.version.id = MTD_MFT_v0; + mtd_mft.hw_extended_id = -1; + + const char *path = nullptr; + int ret_val = px4_mtd_query(type, NULL, &path); + + if (ret_val != PX4_OK) { + PX4_ERR("Can't get mtd query (%s, %i)", type, ret_val); + + } else { + + ret_val = board_get_eeprom_hw_info(path, (mtd_mft_t *)&mtd_mft); + + if (ret_val == PX4_OK) { + PX4_INFO("%s, hw_extended_id = %#x", type, mtd_mft.hw_extended_id); + + } else { + + if (ret_val == -EPROTO) { + PX4_ERR("Manifest data may not exist for %s", path); + + } else { + PX4_ERR("Can't read hw_extended_id from EEPROM (%s, %i)", type, ret_val); + } + } + } + + return ret_val; +} + +extern "C" __EXPORT int mft_cfg_main(int argc, char *argv[]) +{ + if ((argc == 2) && (!strcmp(argv[1], "get"))) { + + char type_ver[] = "MTD_MFT_VER"; + char type_rev[] = "MTD_MFT_REV"; + + print_extended_id(type_ver); + print_extended_id(type_rev); + + return 0; + + } else if (argc >= 3) { + + int ret_val = -1; + const char *path = nullptr; + bool arg_exist = false; + + for (int i = 2; i < argc; ++i) { + if (strcmp("hwver", argv[i]) == 0) { + ret_val = px4_mtd_query("MTD_MFT_VER", NULL, &path); + arg_exist = true; + break; + } + + if (strcmp("hwrev", argv[i]) == 0) { + ret_val = px4_mtd_query("MTD_MFT_REV", NULL, &path); + arg_exist = true; + break; + } + } + + if (!arg_exist) { + PX4_ERR("Missing or arguments'"); + return 1; + } + + if (ret_val != PX4_OK) { + PX4_ERR("Can't get mtd query (%i)", ret_val); + return 1; + } + + mtd_mft_v0_t mtd_mft; + mtd_mft.version.id = MTD_MFT_v0; + mtd_mft.hw_extended_id = -1; + + if (!strcmp(argv[1], "set")) { + if (argc == 5) { + + const char *myoptarg = NULL; + int ch = 0; + int myoptind = 1; + int hw_extended_id = -1; + + while ((ch = px4_getopt(argc, argv, "i:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'i': + hw_extended_id = strtol(myoptarg, NULL, 0); + break; + + default: + PX4_ERR("To set id use '-i x'"); + break; + } + } + + if (hw_extended_id != -1) { + + mtd_mft.hw_extended_id = (uint16_t)hw_extended_id; + + ret_val = board_set_eeprom_hw_info(path, (mtd_mft_t *)&mtd_mft); + + if (ret_val != PX4_OK) { + PX4_ERR("Can't write to EEPROM (%i)", ret_val); + + } else { + board_get_eeprom_hw_info(path, (mtd_mft_t *)&mtd_mft); + PX4_INFO("New hw_extended_id = %#x", mtd_mft.hw_extended_id); + } + } + + } else { + PX4_ERR("Not enough arguments, try 'mft_cfg set hwver -i x'"); + return 1; + } + + return 0; + } + + if (!strcmp(argv[1], "get")) { + + ret_val = board_get_eeprom_hw_info(path, (mtd_mft_t *)&mtd_mft); + + if (ret_val == PX4_OK) { + PX4_INFO("hw_extended_id = %#x", mtd_mft.hw_extended_id); + + } else { + + if (ret_val == -EPROTO) { + PX4_ERR("Manifest data may not exist for %s", path); + + } else { + PX4_ERR("Can't read from EEPROM (%s, %i)", path, ret_val); + } + + return 1; + } + + return 0; + } + + if (!strcmp(argv[1], "reset")) { + + uint8_t buffer[64]; + memset(buffer, 0xFF, sizeof(buffer)); + + int fd = open(path, O_WRONLY); + + if (fd == -1) { + PX4_ERR("Failed to open partition %s", path); + return 1; + } + + while (write(fd, buffer, sizeof(buffer)) == sizeof(buffer)) {} + + PX4_INFO("Reset for %s completed. To remove manifest data from RAM, a reboot is required.", path); + close(fd); + + return 0; + } + + } else { + usage("Error, not enough parameters."); + return 1; + } + + return 0; +} From 689db41c57b5f8f4b4ab5604e1f3f1e9f6794974 Mon Sep 17 00:00:00 2001 From: Marco Hauswirth <58551738+haumarco@users.noreply.github.com> Date: Mon, 24 Feb 2025 16:33:54 +0100 Subject: [PATCH 51/91] ci: flash test adjust permissions for downstream forks (#24404) adds "contents: read" to permissions to enable clones on private forks of PX4 again. (#24404) --- .github/workflows/flash_analysis.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/flash_analysis.yml b/.github/workflows/flash_analysis.yml index 6e965ab2c2..5f03a589bd 100644 --- a/.github/workflows/flash_analysis.yml +++ b/.github/workflows/flash_analysis.yml @@ -1,6 +1,7 @@ name: FLASH usage analysis permissions: + contents: read pull-requests: write issues: write From 393d4c13db88ec3ba19efecfed4b6bb1e0017577 Mon Sep 17 00:00:00 2001 From: Ramon Roche Date: Mon, 24 Feb 2025 13:02:28 -0800 Subject: [PATCH 52/91] ci: disable flash workflow comments (#24409) Signed-off-by: Ramon Roche --- .github/workflows/flash_analysis.yml | 104 ++++++++++++++------------- 1 file changed, 54 insertions(+), 50 deletions(-) diff --git a/.github/workflows/flash_analysis.yml b/.github/workflows/flash_analysis.yml index 5f03a589bd..da37dfa32d 100644 --- a/.github/workflows/flash_analysis.yml +++ b/.github/workflows/flash_analysis.yml @@ -88,59 +88,63 @@ jobs: echo '${{ steps.bloaty-step.outputs.bloaty-summary-map }}' >> $GITHUB_OUTPUT echo "$EOF" >> $GITHUB_OUTPUT - post_pr_comment: - name: Publish Results - runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false] - needs: [analyze_flash] - env: - V5X-SUMMARY-MAP-ABS: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v5x-bloaty-summary-map).vm-absolute) }} - V5X-SUMMARY-MAP-PERC: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v5x-bloaty-summary-map).vm-percentage) }} - V6X-SUMMARY-MAP-ABS: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v6x-bloaty-summary-map).vm-absolute) }} - V6X-SUMMARY-MAP-PERC: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v6x-bloaty-summary-map).vm-percentage) }} - if: ${{ github.event.pull_request }} - steps: - - name: Find Comment - uses: peter-evans/find-comment@v3 - id: fc - with: - issue-number: ${{ github.event.pull_request.number }} - comment-author: 'github-actions[bot]' - body-includes: FLASH Analysis + # TODO: + # This part of the workflow is causing errors, we should find a way to fix this and enable this test again + # Track this issue https://github.com/PX4/PX4-Autopilot/issues/24408 + # + #post_pr_comment: + #name: Publish Results + #runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false] + #needs: [analyze_flash] + #env: + #V5X-SUMMARY-MAP-ABS: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v5x-bloaty-summary-map).vm-absolute) }} + #V5X-SUMMARY-MAP-PERC: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v5x-bloaty-summary-map).vm-percentage) }} + #V6X-SUMMARY-MAP-ABS: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v6x-bloaty-summary-map).vm-absolute) }} + #V6X-SUMMARY-MAP-PERC: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v6x-bloaty-summary-map).vm-percentage) }} + #if: ${{ github.event.pull_request }} + #steps: + #- name: Find Comment + #uses: peter-evans/find-comment@v3 + #id: fc + #with: + #issue-number: ${{ github.event.pull_request.number }} + #comment-author: 'github-actions[bot]' + #body-includes: FLASH Analysis - - name: Set Build Time - id: bt - run: | - echo "timestamp=$(date +'%Y-%m-%dT%H:%M:%S')" >> $GITHUB_OUTPUT + #- name: Set Build Time + #id: bt + #run: | + #echo "timestamp=$(date +'%Y-%m-%dT%H:%M:%S')" >> $GITHUB_OUTPUT - - name: Create or update comment - # This can't be moved to the job-level conditions, as GH actions don't allow a job-level if condition to access the env. - if: | - steps.fc.outputs.comment-id != '' || - env.V5X-SUMMARY-MAP-ABS >= fromJSON(env.MIN_FLASH_POS_DIFF_FOR_COMMENT) || - env.V5X-SUMMARY-MAP-ABS <= fromJSON(env.MIN_FLASH_NEG_DIFF_FOR_COMMENT) || - env.V6X-SUMMARY-MAP-ABS >= fromJSON(env.MIN_FLASH_POS_DIFF_FOR_COMMENT) || - env.V6X-SUMMARY-MAP-ABS <= fromJSON(env.MIN_FLASH_NEG_DIFF_FOR_COMMENT) - uses: peter-evans/create-or-update-comment@v4 - with: - comment-id: ${{ steps.fc.outputs.comment-id }} - issue-number: ${{ github.event.pull_request.number }} - body: | - ## 🔎 FLASH Analysis -
- px4_fmu-v5x [Total VM Diff: ${{ env.V5X-SUMMARY-MAP-ABS }} byte (${{ env.V5X-SUMMARY-MAP-PERC}} %)] + #- name: Create or update comment + ## This can't be moved to the job-level conditions, as GH actions don't allow a job-level if condition to access the env. + #if: | + #steps.fc.outputs.comment-id != '' || + #env.V5X-SUMMARY-MAP-ABS >= fromJSON(env.MIN_FLASH_POS_DIFF_FOR_COMMENT) || + #env.V5X-SUMMARY-MAP-ABS <= fromJSON(env.MIN_FLASH_NEG_DIFF_FOR_COMMENT) || + #env.V6X-SUMMARY-MAP-ABS >= fromJSON(env.MIN_FLASH_POS_DIFF_FOR_COMMENT) || + #env.V6X-SUMMARY-MAP-ABS <= fromJSON(env.MIN_FLASH_NEG_DIFF_FOR_COMMENT) + #uses: peter-evans/create-or-update-comment@v4 + #with: + #comment-id: ${{ steps.fc.outputs.comment-id }} + #issue-number: ${{ github.event.pull_request.number }} + #body: | + ### 🔎 FLASH Analysis + #
+ #px4_fmu-v5x [Total VM Diff: ${{ env.V5X-SUMMARY-MAP-ABS }} byte (${{ env.V5X-SUMMARY-MAP-PERC}} %)] - ``` - ${{ needs.analyze_flash.outputs.px4_fmu-v5x-bloaty-output }} - ``` -
+ #``` + #${{ needs.analyze_flash.outputs.px4_fmu-v5x-bloaty-output }} + #``` + #
-
- px4_fmu-v6x [Total VM Diff: ${{ env.V6X-SUMMARY-MAP-ABS }} byte (${{ env.V6X-SUMMARY-MAP-PERC }} %)] + #
+ #px4_fmu-v6x [Total VM Diff: ${{ env.V6X-SUMMARY-MAP-ABS }} byte (${{ env.V6X-SUMMARY-MAP-PERC }} %)] - ``` - ${{ needs.analyze_flash.outputs.px4_fmu-v6x-bloaty-output }} - ``` -
+ #``` + #${{ needs.analyze_flash.outputs.px4_fmu-v6x-bloaty-output }} + #``` + #
- **Updated: _${{ steps.bt.outputs.timestamp }}_** - edit-mode: replace + #**Updated: _${{ steps.bt.outputs.timestamp }}_** + #edit-mode: replace From c09c63171c90ccb5b4fcef4cfbebac5d33c0eaab Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 24 Feb 2025 09:04:42 +0100 Subject: [PATCH 53/91] MC auto: fix land nudging Revert removal of isTargetModified as this is required when the target is changed by "RC help" (nudging) during landing. --- .../tasks/Auto/FlightTaskAuto.cpp | 14 ++++++++++++++ .../tasks/Auto/FlightTaskAuto.hpp | 1 + 2 files changed, 15 insertions(+) diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 419f25827a..556ac6c72e 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -159,6 +159,11 @@ bool FlightTaskAuto::update() _checkEmergencyBraking(); Vector3f waypoints[] = {_prev_wp, _position_setpoint, _next_wp}; + if (isTargetModified()) { + // In case the target has been modified, we take this as the next waypoints + waypoints[2] = _position_setpoint; + } + const bool should_wait_for_yaw_align = _param_mpc_yaw_mode.get() == int32_t(yaw_mode::towards_waypoint_yaw_first) && !_yaw_sp_aligned; const bool force_zero_velocity_setpoint = should_wait_for_yaw_align || _is_emergency_braking_active; @@ -759,6 +764,15 @@ bool FlightTaskAuto::_generateHeadingAlongTraj() return res; } +bool FlightTaskAuto::isTargetModified() const +{ + const bool xy_modified = (_target - _position_setpoint).xy().longerThan(FLT_EPSILON); + const bool z_valid = PX4_ISFINITE(_position_setpoint(2)); + const bool z_modified = z_valid && std::fabs((_target - _position_setpoint)(2)) > FLT_EPSILON; + + return xy_modified || z_modified; +} + void FlightTaskAuto::_updateTrajConstraints() { // update params of the position smoothing diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp index 5b8d3e9b5b..6c49dd8726 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp @@ -108,6 +108,7 @@ protected: void _checkEmergencyBraking(); bool _generateHeadingAlongTraj(); /**< Generates heading along trajectory. */ + bool isTargetModified() const; void _updateTrajConstraints(); void rcHelpModifyYaw(float &yaw_sp); From d602b569b1b1ee70caaef2002970c45361cfc916 Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Tue, 25 Feb 2025 04:04:52 -0900 Subject: [PATCH 54/91] msg: fix comments in SensorOpticalFlow and VehicleOpticalFlow (#24337) --- msg/SensorOpticalFlow.msg | 2 +- msg/VehicleOpticalFlow.msg | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/msg/SensorOpticalFlow.msg b/msg/SensorOpticalFlow.msg index ce7e8bf08c..ed6a3cfefb 100644 --- a/msg/SensorOpticalFlow.msg +++ b/msg/SensorOpticalFlow.msg @@ -3,7 +3,7 @@ uint64 timestamp_sample uint32 device_id # unique device ID for the sensor that does not change between power cycles -float32[2] pixel_flow # (radians) optical flow in radians where a positive value is produced by a RH rotation about the body axis +float32[2] pixel_flow # (radians) optical flow in radians where a positive value is produced by a RH rotation of the sensor about the body axis float32[3] delta_angle # (radians) accumulated gyro radians where a positive value is produced by a RH rotation about the body axis. Set to NaN if flow sensor does not have 3-axis gyro data. bool delta_angle_available diff --git a/msg/VehicleOpticalFlow.msg b/msg/VehicleOpticalFlow.msg index 13bdb57bbf..23472b4c97 100644 --- a/msg/VehicleOpticalFlow.msg +++ b/msg/VehicleOpticalFlow.msg @@ -7,7 +7,7 @@ uint32 device_id # unique device ID for the sensor that does not c float32[2] pixel_flow # (radians) accumulated optical flow in radians where a positive value is produced by a RH rotation about the body axis -float32[3] delta_angle # (radians) accumulated gyro radians where a positive value is produced by a RH rotation about the body axis. (NAN if unavailable) +float32[3] delta_angle # (radians) accumulated gyro radians where a positive value is produced by a RH rotation of the sensor about the body axis. (NAN if unavailable) float32 distance_m # (meters) Distance to the center of the flow field (NAN if unavailable) From 7c63468e8bbff1da617ff1a746ade6281de24709 Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Mon, 17 Feb 2025 11:56:05 +0100 Subject: [PATCH 55/91] mecanum: refactor code architecture --- .../airframes/4015_gz_r1_rover_mecanum | 48 +- msg/CMakeLists.txt | 5 +- msg/RoverMecanumGuidanceStatus.msg | 7 - msg/RoverMecanumSetpoint.msg | 11 - msg/RoverMecanumStatus.msg | 17 - src/modules/logger/logged_topics.cpp | 5 +- src/modules/rover_mecanum/CMakeLists.txt | 14 +- src/modules/rover_mecanum/Kconfig | 3 +- .../CMakeLists.txt | 9 +- .../MecanumAttControl/MecanumAttControl.cpp | 215 +++++++++ .../MecanumAttControl/MecanumAttControl.hpp | 142 ++++++ .../MecanumPosVelControl/CMakeLists.txt | 40 ++ .../MecanumPosVelControl.cpp | 426 ++++++++++++++++++ .../MecanumPosVelControl.hpp | 233 ++++++++++ .../CMakeLists.txt | 10 +- .../MecanumRateControl/MecanumRateControl.cpp | 190 ++++++++ .../MecanumRateControl/MecanumRateControl.hpp | 144 ++++++ src/modules/rover_mecanum/RoverMecanum.cpp | 344 ++++---------- src/modules/rover_mecanum/RoverMecanum.hpp | 126 +++--- .../RoverMecanumControl.cpp | 323 ------------- .../RoverMecanumControl.hpp | 177 -------- .../RoverMecanumGuidance.cpp | 206 --------- .../RoverMecanumGuidance.hpp | 139 ------ src/modules/rover_mecanum/module.yaml | 200 ++------ 24 files changed, 1634 insertions(+), 1400 deletions(-) delete mode 100644 msg/RoverMecanumGuidanceStatus.msg delete mode 100644 msg/RoverMecanumSetpoint.msg delete mode 100644 msg/RoverMecanumStatus.msg rename src/modules/rover_mecanum/{RoverMecanumGuidance => MecanumAttControl}/CMakeLists.txt (86%) create mode 100644 src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp create mode 100644 src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp create mode 100644 src/modules/rover_mecanum/MecanumPosVelControl/CMakeLists.txt create mode 100644 src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.cpp create mode 100644 src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.hpp rename src/modules/rover_mecanum/{RoverMecanumControl => MecanumRateControl}/CMakeLists.txt (86%) create mode 100644 src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp create mode 100644 src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp delete mode 100644 src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp delete mode 100644 src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp delete mode 100644 src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp delete mode 100644 src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum b/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum index e252b3cfa0..724e7613df 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum @@ -11,28 +11,38 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=r1_rover_mecanum} param set-default SIM_GZ_EN 1 # Gazebo bridge -# Rover parameters -param set-default RM_WHEEL_TRACK 0.3 -param set-default RM_YAW_RATE_I 0.1 -param set-default RM_YAW_RATE_P 0.1 -param set-default RM_MAX_ACCEL 3 -param set-default RM_MAX_DECEL 5 -param set-default RM_MAX_JERK 5 -param set-default RM_MAX_SPEED 2 -param set-default RM_MAX_THR_SPD 2.2 -param set-default RM_MAX_THR_YAW_R 1.2 -param set-default RM_YAW_P 5 -param set-default RM_YAW_I 0.1 -param set-default RM_MAX_YAW_RATE 120 -param set-default RM_MAX_YAW_ACCEL 240 -param set-default RM_MISS_VEL_GAIN 1 -param set-default RM_SPEED_I 0.01 -param set-default RM_SPEED_P 0.1 +param set-default NAV_ACC_RAD 0.5 -# Pure pursuit parameters +# Mecanum Parameters +param set-default RM_WHEEL_TRACK 0.3 +param set-default RM_MAX_THR_YAW_R 1.2 +param set-default RM_MISS_SPD_GAIN 1 + +# Rover Control Parameters +param set-default RO_ACCEL_LIM 3 +param set-default RO_DECEL_LIM 5 +param set-default RO_JERK_LIM 30 +param set-default RO_MAX_THR_SPEED 2.1 + +# Rover Rate Control Parameters +param set-default RO_YAW_RATE_I 0.1 +param set-default RO_YAW_RATE_P 0.1 +param set-default RO_YAW_RATE_LIM 120 +param set-default RO_YAW_ACCEL_LIM 240 +param set-default RO_YAW_DECEL_LIM 1000 + +# Rover Attitude Control Parameters +param set-default RO_YAW_P 5 + +# Rover Position Control Parameters +param set-default RO_SPEED_LIM 2 +param set-default RO_SPEED_I 0.5 +param set-default RO_SPEED_P 1 + +# Pure Pursuit parameters +param set-default PP_LOOKAHD_GAIN 0.5 param set-default PP_LOOKAHD_MAX 10 param set-default PP_LOOKAHD_MIN 1 -param set-default PP_LOOKAHD_GAIN 0.5 # Simulated sensors param set-default SENS_EN_GPSSIM 1 diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index a88567b286..8ef4407504 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -173,14 +173,11 @@ set(msg_files RcParameterMap.msg RoverAttitudeSetpoint.msg RoverAttitudeStatus.msg - RoverVelocityStatus.msg RoverRateSetpoint.msg RoverRateStatus.msg RoverSteeringSetpoint.msg RoverThrottleSetpoint.msg - RoverMecanumGuidanceStatus.msg - RoverMecanumSetpoint.msg - RoverMecanumStatus.msg + RoverVelocityStatus.msg Rpm.msg RtlStatus.msg RtlTimeEstimate.msg diff --git a/msg/RoverMecanumGuidanceStatus.msg b/msg/RoverMecanumGuidanceStatus.msg deleted file mode 100644 index fba920033b..0000000000 --- a/msg/RoverMecanumGuidanceStatus.msg +++ /dev/null @@ -1,7 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller -float32 heading_error # [rad] Heading error of the pure pursuit controller -float32 desired_speed # [m/s] Desired velocity magnitude (speed) - -# TOPICS rover_mecanum_guidance_status diff --git a/msg/RoverMecanumSetpoint.msg b/msg/RoverMecanumSetpoint.msg deleted file mode 100644 index 0cc9415be1..0000000000 --- a/msg/RoverMecanumSetpoint.msg +++ /dev/null @@ -1,11 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 forward_speed_setpoint # [m/s] Desired forward speed -float32 forward_speed_setpoint_normalized # [-1, 1] Desired normalized forward speed -float32 lateral_speed_setpoint # [m/s] Desired lateral speed -float32 lateral_speed_setpoint_normalized # [-1, 1] Desired normalized lateral speed -float32 yaw_rate_setpoint # [rad/s] Desired yaw rate -float32 speed_diff_setpoint_normalized # [-1, 1] Normalized speed difference between the left and right wheels -float32 yaw_setpoint # [rad] Desired yaw (heading) - -# TOPICS rover_mecanum_setpoint diff --git a/msg/RoverMecanumStatus.msg b/msg/RoverMecanumStatus.msg deleted file mode 100644 index 7547fd5be6..0000000000 --- a/msg/RoverMecanumStatus.msg +++ /dev/null @@ -1,17 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 measured_forward_speed # [m/s] Measured speed in body x direction. Positiv: forwards, Negativ: backwards -float32 adjusted_forward_speed_setpoint # [m/s] Speed setpoint after applying slew rate -float32 measured_lateral_speed # [m/s] Measured speed in body y direction. Positiv: right, Negativ: left -float32 adjusted_lateral_speed_setpoint # [m/s] Speed setpoint after applying slew rate -float32 measured_yaw_rate # [rad/s] Measured yaw rate -float32 clyaw_yaw_rate_setpoint # [rad/s] Yaw rate setpoint output by the closed loop yaw controller -float32 adjusted_yaw_rate_setpoint # [rad/s] Yaw rate setpoint from the closed loop yaw controller -float32 measured_yaw # [rad] Measured yaw -float32 adjusted_yaw_setpoint # [rad] Yaw setpoint after applying slew rate -float32 pid_yaw_rate_integral # Integral of the PID for the closed loop yaw rate controller -float32 pid_yaw_integral # Integral of the PID for the closed loop yaw controller -float32 pid_forward_throttle_integral # Integral of the PID for the closed loop forward speed controller -float32 pid_lateral_throttle_integral # Integral of the PID for the closed loop lateral speed controller - -# TOPICS rover_mecanum_status diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index b98d185aff..84d8e3eadd 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -105,14 +105,11 @@ void LoggedTopics::add_default_topics() add_topic("radio_status"); add_optional_topic("rover_attitude_setpoint", 100); add_optional_topic("rover_attitude_status", 100); - add_optional_topic("rover_velocity_status", 100); add_optional_topic("rover_rate_setpoint", 100); add_optional_topic("rover_rate_status", 100); add_optional_topic("rover_steering_setpoint", 100); add_optional_topic("rover_throttle_setpoint", 100); - add_optional_topic("rover_mecanum_guidance_status", 100); - add_optional_topic("rover_mecanum_setpoint", 100); - add_optional_topic("rover_mecanum_status", 100); + add_optional_topic("rover_velocity_status", 100); add_topic("rtl_time_estimate", 1000); add_topic("rtl_status", 2000); add_optional_topic("sensor_airflow", 100); diff --git a/src/modules/rover_mecanum/CMakeLists.txt b/src/modules/rover_mecanum/CMakeLists.txt index 8eea90fb2b..cf5e5b9187 100644 --- a/src/modules/rover_mecanum/CMakeLists.txt +++ b/src/modules/rover_mecanum/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2024 PX4 Development Team. All rights reserved. +# Copyright (c) 2025 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -31,8 +31,9 @@ # ############################################################################ -add_subdirectory(RoverMecanumGuidance) -add_subdirectory(RoverMecanumControl) +add_subdirectory(MecanumRateControl) +add_subdirectory(MecanumAttControl) +add_subdirectory(MecanumPosVelControl) px4_add_module( MODULE modules__rover_mecanum @@ -41,10 +42,11 @@ px4_add_module( RoverMecanum.cpp RoverMecanum.hpp DEPENDS - RoverMecanumGuidance - RoverMecanumControl + MecanumRateControl + MecanumAttControl + MecanumPosVelControl px4_work_queue - pure_pursuit + rover_control MODULE_CONFIG module.yaml ) diff --git a/src/modules/rover_mecanum/Kconfig b/src/modules/rover_mecanum/Kconfig index 46a24b3fc8..5c1778c67c 100644 --- a/src/modules/rover_mecanum/Kconfig +++ b/src/modules/rover_mecanum/Kconfig @@ -1,6 +1,5 @@ menuconfig MODULES_ROVER_MECANUM bool "rover_mecanum" default n - depends on MODULES_CONTROL_ALLOCATOR ---help--- - Enable support for control of mecanum rovers + Enable support for mecanum rovers diff --git a/src/modules/rover_mecanum/RoverMecanumGuidance/CMakeLists.txt b/src/modules/rover_mecanum/MecanumAttControl/CMakeLists.txt similarity index 86% rename from src/modules/rover_mecanum/RoverMecanumGuidance/CMakeLists.txt rename to src/modules/rover_mecanum/MecanumAttControl/CMakeLists.txt index a15f489245..34bad46d91 100644 --- a/src/modules/rover_mecanum/RoverMecanumGuidance/CMakeLists.txt +++ b/src/modules/rover_mecanum/MecanumAttControl/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2024 PX4 Development Team. All rights reserved. +# Copyright (c) 2025 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -31,8 +31,9 @@ # ############################################################################ -px4_add_library(RoverMecanumGuidance - RoverMecanumGuidance.cpp +px4_add_library(MecanumAttControl + MecanumAttControl.cpp ) -target_include_directories(RoverMecanumGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(MecanumAttControl PUBLIC PID) +target_include_directories(MecanumAttControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp new file mode 100644 index 0000000000..0582c5f299 --- /dev/null +++ b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp @@ -0,0 +1,215 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "MecanumAttControl.hpp" + +using namespace time_literals; + +MecanumAttControl::MecanumAttControl(ModuleParams *parent) : ModuleParams(parent) +{ + _rover_rate_setpoint_pub.advertise(); + _rover_throttle_setpoint_pub.advertise(); + _rover_attitude_setpoint_pub.advertise(); + _rover_attitude_status_pub.advertise(); + updateParams(); +} + +void MecanumAttControl::updateParams() +{ + ModuleParams::updateParams(); + + if (_param_ro_yaw_rate_limit.get() > FLT_EPSILON) { + _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + } + + _pid_yaw.setGains(_param_ro_yaw_p.get(), 0.f, 0.f); + _pid_yaw.setIntegralLimit(_max_yaw_rate); + _pid_yaw.setOutputLimit(_max_yaw_rate); + _adjusted_yaw_setpoint.setSlewRate(_max_yaw_rate); +} + +void MecanumAttControl::updateAttControl() +{ + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); + } + + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); + matrix::Quatf vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); + _vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi(); + } + + if (_vehicle_control_mode.flag_control_attitude_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) { + + if (_vehicle_control_mode.flag_control_manual_enabled || _vehicle_control_mode.flag_control_offboard_enabled) { + generateAttitudeSetpoint(); + } + + generateRateSetpoint(); + + } else { // Reset pid and slew rate when attitude control is not active + _pid_yaw.resetIntegral(); + _adjusted_yaw_setpoint.setForcedValue(0.f); + } + + // Publish attitude controller status (logging only) + rover_attitude_status_s rover_attitude_status; + rover_attitude_status.timestamp = _timestamp; + rover_attitude_status.measured_yaw = _vehicle_yaw; + rover_attitude_status.adjusted_yaw_setpoint = _adjusted_yaw_setpoint.getState(); + _rover_attitude_status_pub.publish(rover_attitude_status); + +} + +void MecanumAttControl::generateAttitudeSetpoint() +{ + const bool stab_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled + && !_vehicle_control_mode.flag_control_position_enabled && _vehicle_control_mode.flag_control_attitude_enabled; + + if (stab_mode_enabled && _manual_control_setpoint_sub.updated()) { // Stab Mode + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + rover_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + + const float yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.yaw, + _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + + if (fabsf(yaw_rate_setpoint) > FLT_EPSILON) { // Closed loop yaw rate control + _stab_yaw_setpoint = NAN; + _adjusted_yaw_setpoint.setForcedValue(0.f); + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = yaw_rate_setpoint; + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + + } else if (fabsf(rover_throttle_setpoint.throttle_body_x) > FLT_EPSILON + || fabsf(rover_throttle_setpoint.throttle_body_y) > + FLT_EPSILON) { // Closed loop yaw control if the yaw rate input is zero (keep current yaw) + if (!PX4_ISFINITE(_stab_yaw_setpoint)) { + _stab_yaw_setpoint = _vehicle_yaw; + } + + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = _stab_yaw_setpoint; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + + } else { // Reset yaw control and yaw rate setpoint + _stab_yaw_setpoint = NAN; + _adjusted_yaw_setpoint.setForcedValue(0.f); + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = 0.f; + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + } + + + } + + } else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard attitude control + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + + if (_offboard_control_mode_sub.updated()) { + _offboard_control_mode_sub.copy(&_offboard_control_mode); + } + + const bool offboard_att_control = _offboard_control_mode.attitude; + + if (offboard_att_control && PX4_ISFINITE(trajectory_setpoint.yaw)) { + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = trajectory_setpoint.yaw; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + } + } +} + +void MecanumAttControl::generateRateSetpoint() +{ + if (_rover_attitude_setpoint_sub.updated()) { + _rover_attitude_setpoint_sub.copy(&_rover_attitude_setpoint); + } + + if (_rover_rate_setpoint_sub.updated()) { + _rover_rate_setpoint_sub.copy(&_rover_rate_setpoint); + } + + // Check if a new rate setpoint was already published from somewhere else + if (_rover_rate_setpoint.timestamp > _last_rate_setpoint_update + && _rover_rate_setpoint.timestamp > _rover_attitude_setpoint.timestamp) { + return; + } + + const float yaw_rate_setpoint = RoverControl::attitudeControl(_adjusted_yaw_setpoint, _pid_yaw, _max_yaw_rate, + _vehicle_yaw, _rover_attitude_setpoint.yaw_setpoint, _dt); + + _last_rate_setpoint_update = _timestamp; + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = math::constrain(yaw_rate_setpoint, -_max_yaw_rate, _max_yaw_rate); + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); +} + +bool MecanumAttControl::runSanityChecks() +{ + bool ret = true; + + if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_ro_yaw_p.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("mecanum_att_control_conf_invalid_yaw_p"), events::Log::Error, + "Invalid configuration of necessary parameter RO_YAW_P", _param_ro_yaw_p.get()); + } + } + + _prev_param_check_passed = ret; + return ret; +} diff --git a/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp new file mode 100644 index 0000000000..c6e9a3353e --- /dev/null +++ b/src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp @@ -0,0 +1,142 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +// PX4 includes +#include +#include + +// Libraries +#include +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * @brief Class for mecanum attitude control. + */ +class MecanumAttControl : public ModuleParams +{ +public: + /** + * @brief Constructor for MecanumAttControl. + * @param parent The parent ModuleParams object. + */ + MecanumAttControl(ModuleParams *parent); + ~MecanumAttControl() = default; + + /** + * @brief Update attitude controller. + */ + void updateAttControl(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + /** + * @brief Generate and publish roverAttitudeSetpoint from manualControlSetpoint (Stab Mode) + * or trajectorySetpoint (Offboard attitude control). + */ + void generateAttitudeSetpoint(); + + /** + * @brief Generate and publish roverRateSetpoint from roverAttitudeSetpoint. + */ + void generateRateSetpoint(); + + /** + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. + */ + bool runSanityChecks(); + + // uORB subscriptions + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + uORB::Subscription _rover_attitude_setpoint_sub{ORB_ID(rover_attitude_setpoint)}; + uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)}; + vehicle_control_mode_s _vehicle_control_mode{}; + rover_attitude_setpoint_s _rover_attitude_setpoint{}; + rover_rate_setpoint_s _rover_rate_setpoint{}; + offboard_control_mode_s _offboard_control_mode{}; + + // uORB publications + uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; + uORB::Publication _rover_attitude_status_pub{ORB_ID(rover_attitude_status)}; + + // Variables + hrt_abstime _timestamp{0}; + hrt_abstime _last_rate_setpoint_update{0}; + float _vehicle_yaw{0.f}; + float _dt{0.f}; + float _max_yaw_rate{0.f}; + float _stab_yaw_setpoint{0.f}; // Yaw setpoint for stab mode, NAN if yaw rate is manually controlled [rad] + bool _prev_param_check_passed{true}; + + // Controllers + PID _pid_yaw; + SlewRateYaw _adjusted_yaw_setpoint; + + // Parameters + DEFINE_PARAMETERS( + (ParamFloat) _param_ro_yaw_rate_limit, + (ParamFloat) _param_ro_yaw_p, + (ParamFloat) _param_ro_yaw_stick_dz + ) +}; diff --git a/src/modules/rover_mecanum/MecanumPosVelControl/CMakeLists.txt b/src/modules/rover_mecanum/MecanumPosVelControl/CMakeLists.txt new file mode 100644 index 0000000000..692e4e661b --- /dev/null +++ b/src/modules/rover_mecanum/MecanumPosVelControl/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(MecanumPosVelControl + MecanumPosVelControl.cpp +) + +target_link_libraries(MecanumPosVelControl PUBLIC PID) +target_link_libraries(MecanumPosVelControl PUBLIC pure_pursuit) +target_include_directories(MecanumPosVelControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.cpp b/src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.cpp new file mode 100644 index 0000000000..a5ee63496e --- /dev/null +++ b/src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.cpp @@ -0,0 +1,426 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "MecanumPosVelControl.hpp" + +using namespace time_literals; + +MecanumPosVelControl::MecanumPosVelControl(ModuleParams *parent) : ModuleParams(parent) +{ + _rover_rate_setpoint_pub.advertise(); + _rover_throttle_setpoint_pub.advertise(); + _rover_attitude_setpoint_pub.advertise(); + _rover_velocity_status_pub.advertise(); + updateParams(); +} + +void MecanumPosVelControl::updateParams() +{ + ModuleParams::updateParams(); + _pid_speed_x.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f); + _pid_speed_x.setIntegralLimit(1.f); + _pid_speed_x.setOutputLimit(1.f); + _pid_speed_y.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f); + _pid_speed_y.setIntegralLimit(1.f); + _pid_speed_y.setOutputLimit(1.f); + _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + + if (_param_ro_accel_limit.get() > FLT_EPSILON) { + _speed_x_setpoint.setSlewRate(_param_ro_accel_limit.get()); + _speed_y_setpoint.setSlewRate(_param_ro_accel_limit.get()); + } +} + +void MecanumPosVelControl::updatePosControl() +{ + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + updateSubscriptions(); + + if ((_vehicle_control_mode.flag_control_position_enabled || _vehicle_control_mode.flag_control_velocity_enabled) + && _vehicle_control_mode.flag_armed && runSanityChecks()) { + generateAttitudeSetpoint(); + + if (_param_ro_max_thr_speed.get() > FLT_EPSILON) { // Adjust speed setpoints if infeasible + if (_rover_steering_setpoint_sub.updated()) { + _rover_steering_setpoint_sub.copy(&_rover_steering_setpoint); + } + + float speed_body_x_setpoint_normalized = math::interpolate(_speed_body_x_setpoint, + -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f); + + float speed_body_y_setpoint_normalized = math::interpolate(_speed_body_y_setpoint, + -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f); + + const float total_speed = fabsf(speed_body_x_setpoint_normalized) + fabsf(speed_body_y_setpoint_normalized) + fabsf( + _rover_steering_setpoint.normalized_speed_diff); + + if (total_speed > 1.f) { + const float theta = atan2f(fabsf(speed_body_y_setpoint_normalized), fabsf(speed_body_x_setpoint_normalized)); + const float magnitude = (1.f - fabsf(_rover_steering_setpoint.normalized_speed_diff)) / (sinf(theta) + cosf(theta)); + const float normalization = 1.f / (sqrtf(powf(speed_body_x_setpoint_normalized, + 2.f) + powf(speed_body_y_setpoint_normalized, 2.f))); + speed_body_x_setpoint_normalized *= magnitude * normalization; + speed_body_y_setpoint_normalized *= magnitude * normalization; + _speed_body_x_setpoint = math::interpolate(speed_body_x_setpoint_normalized, -1.f, 1.f, + -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); + _speed_body_y_setpoint = math::interpolate(speed_body_y_setpoint_normalized, -1.f, 1.f, + -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()); + } + } + + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + _speed_body_x_setpoint = fabsf(_speed_body_x_setpoint) > _param_ro_speed_th.get() ? _speed_body_x_setpoint : 0.f; + _speed_body_y_setpoint = fabsf(_speed_body_y_setpoint) > _param_ro_speed_th.get() ? _speed_body_y_setpoint : 0.f; + rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_speed_x_setpoint, _pid_speed_x, + _speed_body_x_setpoint, _vehicle_speed_body_x, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), + _param_ro_max_thr_speed.get(), _dt); + rover_throttle_setpoint.throttle_body_y = RoverControl::speedControl(_speed_y_setpoint, _pid_speed_y, + _speed_body_y_setpoint, _vehicle_speed_body_y, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(), + _param_ro_max_thr_speed.get(), _dt); + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + + } else { // Reset controller and slew rate when position control is not active + _pid_speed_x.resetIntegral(); + _speed_x_setpoint.setForcedValue(0.f); + _pid_speed_y.resetIntegral(); + _speed_y_setpoint.setForcedValue(0.f); + } + + // Publish position controller status (logging only) + rover_velocity_status_s rover_velocity_status; + rover_velocity_status.timestamp = _timestamp; + rover_velocity_status.measured_speed_body_x = _vehicle_speed_body_x; + rover_velocity_status.speed_body_x_setpoint = _speed_body_x_setpoint; + rover_velocity_status.adjusted_speed_body_x_setpoint = _speed_x_setpoint.getState(); + rover_velocity_status.measured_speed_body_y = _vehicle_speed_body_y; + rover_velocity_status.speed_body_y_setpoint = _speed_body_y_setpoint; + rover_velocity_status.adjusted_speed_body_y_setpoint = _speed_y_setpoint.getState(); + rover_velocity_status.pid_throttle_body_x_integral = _pid_speed_x.getIntegral(); + rover_velocity_status.pid_throttle_body_y_integral = _pid_speed_y.getIntegral(); + _rover_velocity_status_pub.publish(rover_velocity_status); +} + +void MecanumPosVelControl::updateSubscriptions() +{ + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); + } + + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); + _vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); + _vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi(); + } + + if (_vehicle_local_position_sub.updated()) { + vehicle_local_position_s vehicle_local_position{}; + _vehicle_local_position_sub.copy(&vehicle_local_position); + + if (!_global_ned_proj_ref.isInitialized() + || (_global_ned_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.ref_timestamp)) { + _global_ned_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon, + vehicle_local_position.ref_timestamp); + } + + _curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y); + Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); + Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); + _vehicle_speed_body_x = fabsf(velocity_in_body_frame(0)) > _param_ro_speed_th.get() ? velocity_in_body_frame(0) : 0.f; + _vehicle_speed_body_y = fabsf(velocity_in_body_frame(1)) > _param_ro_speed_th.get() ? velocity_in_body_frame(1) : 0.f; + } + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + _vehicle_status_sub.copy(&vehicle_status); + _nav_state = vehicle_status.nav_state; + } + +} + +void MecanumPosVelControl::generateAttitudeSetpoint() +{ + if (_vehicle_control_mode.flag_control_manual_enabled + && _vehicle_control_mode.flag_control_position_enabled) { // Position Mode + manualPositionMode(); + + } else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard Control + if (_offboard_control_mode_sub.updated()) { + _offboard_control_mode_sub.copy(&_offboard_control_mode); + } + + if (_offboard_control_mode.position) { + offboardPositionMode(); + + } else if (_offboard_control_mode.velocity) { + offboardVelocityMode(); + } + + } else if (_vehicle_control_mode.flag_control_auto_enabled) { // Auto Mode + autoPositionMode(); + } +} + +void MecanumPosVelControl::manualPositionMode() +{ + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + _speed_body_x_setpoint = math::interpolate(manual_control_setpoint.throttle, + -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get()); + _speed_body_y_setpoint = math::interpolate(manual_control_setpoint.roll, + -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get()); + const float yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.yaw, + _param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + + if (fabsf(yaw_rate_setpoint) > FLT_EPSILON) { // Closed loop yaw rate control + _pos_ctl_yaw_setpoint = NAN; + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = yaw_rate_setpoint; + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + + } else if ((fabsf(_speed_body_x_setpoint) > FLT_EPSILON + || fabsf(_speed_body_y_setpoint) > + FLT_EPSILON)) { // Course control if the steering input is zero (keep driving on a straight line) + const Vector3f velocity = Vector3f(_speed_body_x_setpoint, _speed_body_y_setpoint, 0.f); + const float velocity_magnitude_setpoint = velocity.norm(); + const Vector3f pos_ctl_course_direction_local = _vehicle_attitude_quaternion.rotateVector(velocity.normalized()); + const Vector2f pos_ctl_course_direction_temp = Vector2f(pos_ctl_course_direction_local(0), + pos_ctl_course_direction_local(1)); + + // Reset course control if course direction change is above threshold + if (fabsf(asinf(pos_ctl_course_direction_temp % _pos_ctl_course_direction)) > _param_rm_course_ctl_th.get()) { + _pos_ctl_yaw_setpoint = NAN; + } + + if (!PX4_ISFINITE(_pos_ctl_yaw_setpoint)) { + _pos_ctl_start_position_ned = _curr_pos_ned; + _pos_ctl_yaw_setpoint = _vehicle_yaw; + _pos_ctl_course_direction = pos_ctl_course_direction_temp; + } + + // Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover + const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(), + 2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment(); + const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + vector_scaling * _pos_ctl_course_direction; + const float bearing_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _pos_ctl_start_position_ned, + _curr_pos_ned, velocity_magnitude_setpoint); + const float bearing_setpoint_body_frame = matrix::wrap_pi(bearing_setpoint - _vehicle_yaw); + _speed_body_x_setpoint = velocity_magnitude_setpoint * cosf(bearing_setpoint_body_frame); + _speed_body_y_setpoint = velocity_magnitude_setpoint * sinf(bearing_setpoint_body_frame); + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = _pos_ctl_yaw_setpoint; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); + + } else { // Reset course control and yaw rate setpoint + _pos_ctl_yaw_setpoint = NAN; + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = 0.f; + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + } + } +} + +void MecanumPosVelControl::offboardPositionMode() +{ + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + + // Translate trajectory setpoint to rover setpoints + const Vector2f target_waypoint_ned(trajectory_setpoint.position[0], trajectory_setpoint.position[1]); + const float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm(); + + if (target_waypoint_ned.isAllFinite() && distance_to_target > _param_nav_acc_rad.get()) { + const float velocity_magnitude_setpoint = math::min(math::trajectory::computeMaxSpeedFromDistance( + _param_ro_jerk_limit.get(), + _param_ro_decel_limit.get(), distance_to_target, 0.f), _param_ro_speed_limit.get()); + const float bearing_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _curr_pos_ned, + _curr_pos_ned, fabsf(_speed_body_x_setpoint)); + const float bearing_setpoint_body_frame = matrix::wrap_pi(bearing_setpoint - _vehicle_yaw); + _speed_body_x_setpoint = velocity_magnitude_setpoint * cosf(bearing_setpoint_body_frame); + _speed_body_y_setpoint = velocity_magnitude_setpoint * sinf(bearing_setpoint_body_frame); + + } else { + _speed_body_x_setpoint = 0.f; + _speed_body_y_setpoint = 0.f; + } +} + +void MecanumPosVelControl::offboardVelocityMode() +{ + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + + const Vector3f velocity_in_local_frame(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1], + trajectory_setpoint.velocity[2]); + const Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); + + if (velocity_in_body_frame.isAllFinite()) { + _speed_body_x_setpoint = velocity_in_body_frame(0); + _speed_body_y_setpoint = velocity_in_body_frame(1); + + } else { + _speed_body_x_setpoint = 0.f; + _speed_body_y_setpoint = 0.f; + } +} + +void MecanumPosVelControl::autoPositionMode() +{ + updateAutoSubscriptions(); + + const float distance_to_curr_wp = sqrt(powf(_curr_pos_ned(0) - _curr_wp_ned(0), + 2) + powf(_curr_pos_ned(1) - _curr_wp_ned(1), 2)); + + if (_nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { // Check RTL arrival + _mission_finished = distance_to_curr_wp < _param_nav_acc_rad.get(); + } + + const float velocity_magnitude = calcVelocityMagnitude(_auto_speed, distance_to_curr_wp, _param_ro_decel_limit.get(), + _param_ro_jerk_limit.get(), _waypoint_transition_angle, _param_ro_speed_limit.get(), _param_rm_miss_spd_gain.get(), + _nav_state); + const float bearing_setpoint = _posctl_pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, + velocity_magnitude); + const float bearing_setpoint_body_frame = matrix::wrap_pi(bearing_setpoint - _vehicle_yaw); + Vector2f desired_velocity(0.f, 0.f); + _speed_body_x_setpoint = _mission_finished ? 0.f : velocity_magnitude * cosf(bearing_setpoint_body_frame); + _speed_body_y_setpoint = _mission_finished ? 0.f : velocity_magnitude * sinf(bearing_setpoint_body_frame); + rover_attitude_setpoint_s rover_attitude_setpoint{}; + rover_attitude_setpoint.timestamp = _timestamp; + rover_attitude_setpoint.yaw_setpoint = _auto_yaw; + _rover_attitude_setpoint_pub.publish(rover_attitude_setpoint); +} + +void MecanumPosVelControl::updateAutoSubscriptions() +{ + if (_home_position_sub.updated()) { + home_position_s home_position{}; + _home_position_sub.copy(&home_position); + _home_position = Vector2d(home_position.lat, home_position.lon); + } + + if (_position_setpoint_triplet_sub.updated()) { + position_setpoint_triplet_s position_setpoint_triplet{}; + _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); + + RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet, + _curr_pos_ned, _home_position, _global_ned_proj_ref); + + _waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned); + + // Waypoint cruising speed + _auto_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain( + position_setpoint_triplet.current.cruising_speed, 0.f, _param_ro_speed_limit.get()) : _param_ro_speed_limit.get(); + + // Waypoint yaw setpoint + if (PX4_ISFINITE(position_setpoint_triplet.current.yaw)) { + _auto_yaw = position_setpoint_triplet.current.yaw; + + } else { + _auto_yaw = _vehicle_yaw; + } + } + + if (_mission_result_sub.updated()) { + mission_result_s mission_result{}; + _mission_result_sub.copy(&mission_result); + _mission_finished = mission_result.finished; + } +} + +float MecanumPosVelControl::calcVelocityMagnitude(const float auto_speed, const float distance_to_curr_wp, + const float max_decel, const float max_jerk, const float waypoint_transition_angle, const float max_speed, + const float miss_spd_gain, const int nav_state) +{ + float velocity_magnitude{auto_speed}; + + if (max_jerk > FLT_EPSILON && max_decel > FLT_EPSILON + && miss_spd_gain > FLT_EPSILON) { + float max_velocity_magnitude = velocity_magnitude; + + if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { + max_velocity_magnitude = math::trajectory::computeMaxSpeedFromDistance(max_jerk, + max_decel, distance_to_curr_wp, 0.f); + + } else if (PX4_ISFINITE(waypoint_transition_angle)) { + const float speed_reduction = math::constrain(miss_spd_gain * math::interpolate(M_PI_F - waypoint_transition_angle, 0.f, + M_PI_F, 0.f, 1.f), 0.f, 1.f); + max_velocity_magnitude = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_decel, distance_to_curr_wp, + max_speed * (1.f - speed_reduction)); + } + + velocity_magnitude = math::constrain(max_velocity_magnitude, -auto_speed, auto_speed); + } + + return velocity_magnitude; +} + +bool MecanumPosVelControl::runSanityChecks() +{ + bool ret = true; + + if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { + ret = false; + } + + if (_param_ro_speed_limit.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("mecanum_posVel_control_conf_invalid_speed_lim"), events::Log::Error, + "Invalid configuration of necessary parameter RO_SPEED_LIM", _param_ro_speed_limit.get()); + } + + } + + if (_param_ro_max_thr_speed.get() < FLT_EPSILON && _param_ro_speed_p.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("mecanum_posVel_control_conf_invalid_speed_control"), events::Log::Error, + "Invalid configuration for speed control: Neither feed forward (RO_MAX_THR_SPD) nor feedback (RO_SPEED_P) is setup", + _param_ro_max_thr_speed.get(), + _param_ro_speed_p.get()); + } + } + + _prev_param_check_passed = ret; + return ret; +} diff --git a/src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.hpp b/src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.hpp new file mode 100644 index 0000000000..4659588fdf --- /dev/null +++ b/src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.hpp @@ -0,0 +1,233 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +// PX4 includes +#include +#include + +// Libraries +#include +#include +#include +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace matrix; + +/** + * @brief Class for mecanum position/velocity control. + */ +class MecanumPosVelControl : public ModuleParams +{ +public: + /** + * @brief Constructor for MecanumPosVelControl. + * @param parent The parent ModuleParams object. + */ + MecanumPosVelControl(ModuleParams *parent); + ~MecanumPosVelControl() = default; + + /** + * @brief Update position controller. + */ + void updatePosControl(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + /** + * @brief Update uORB subscriptions used in position controller. + */ + void updateSubscriptions(); + + /** + * @brief Generate and publish roverAttitudeSetpoint from manualControlSetpoint (Position Mode) or + * trajcetorySetpoint (Offboard position or velocity control) or + * positionSetpointTriplet (Auto Mode). + */ + void generateAttitudeSetpoint(); + + /** + * @brief Generate and publish roverThrottleSetpoint and roverAttitudeSetpoint or roverRateSetpoint from manualControlSetpoint. + */ + void manualPositionMode(); + + /** + * @brief Generate and publish roverAttitudeSetpoint from position of trajectorySetpoint. + */ + void offboardPositionMode(); + + /** + * @brief Generate and publish roverAttitudeSetpoint from velocity of trajectorySetpoint. + */ + void offboardVelocityMode(); + + /** + * @brief Generate and publish roverThrottleSetpoint and roverAttitudeSetpoint from positionSetpointTriplet. + */ + void autoPositionMode(); + + /** + * @brief Update uORB subscriptions used for auto modes. + */ + void updateAutoSubscriptions(); + + /** + * @brief Calculate the velocity magnitude setpoint. During waypoint transition the speed is restricted to + * Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN). + * On straight lines it is based on a speed trajectory such that the rover will arrive at the next waypoint transition + * with the desired waypoiny transition speed under consideration of the maximum deceleration and jerk. + * @param auto_speed Default auto speed [m/s]. + * @param distance_to_curr_wp Distance to the current waypoint [m]. + * @param max_decel Maximum allowed deceleration [m/s^2]. + * @param max_jerk Maximum allowed jerk [m/s^3]. + * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + * @param max_speed Maximum velocity magnitude setpoint [m/s] + * @param miss_spd_gain Tuning parameter for the speed reduction during waypoint transition. + * @param nav_state Vehicle navigation state + * @return Velocity magnitude setpoint [m/s]. + */ + float calcVelocityMagnitude(float auto_speed, float distance_to_curr_wp, float max_decel, float max_jerk, + float waypoint_transition_angle, float max_speed, float miss_spd_gain, int nav_state); + + /** + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. + */ + bool runSanityChecks(); + + // uORB subscriptions + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _mission_result_sub{ORB_ID(mission_result)}; + uORB::Subscription _home_position_sub{ORB_ID(home_position)}; + uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)}; + vehicle_control_mode_s _vehicle_control_mode{}; + offboard_control_mode_s _offboard_control_mode{}; + rover_steering_setpoint_s _rover_steering_setpoint{}; + + // uORB publications + uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)}; + uORB::Publication _rover_velocity_status_pub{ORB_ID(rover_velocity_status)}; + uORB::Publication _position_controller_status_pub{ORB_ID(position_controller_status)}; + + // Variables + hrt_abstime _timestamp{0}; + Quatf _vehicle_attitude_quaternion{}; + Vector2d _home_position{}; + Vector2f _curr_pos_ned{}; + Vector2f _pos_ctl_course_direction{}; + Vector2f _pos_ctl_start_position_ned{}; + Vector2f _curr_wp_ned{}; + Vector2f _prev_wp_ned{}; + Vector2f _next_wp_ned{}; + float _vehicle_speed_body_x{0.f}; + float _vehicle_speed_body_y{0.f}; + float _vehicle_yaw{0.f}; + float _max_yaw_rate{0.f}; + float _speed_body_x_setpoint{0.f}; + float _speed_body_y_setpoint{0.f}; + float _pos_ctl_yaw_setpoint{0.f}; // Yaw setpoint for manual position mode, NAN if yaw rate is manually controlled [rad] + float _dt{0.f}; + float _auto_speed{0.f}; + float _auto_yaw{0.f}; + float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad] + int _nav_state{0}; + bool _mission_finished{false}; + bool _prev_param_check_passed{true}; + + // Controllers + PID _pid_speed_x; + PID _pid_speed_y; + SlewRate _speed_x_setpoint; + SlewRate _speed_y_setpoint; + + // Class Instances + PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library + MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates + + DEFINE_PARAMETERS( + (ParamFloat) _param_rm_miss_spd_gain, + (ParamFloat) _param_rm_course_ctl_th, + (ParamFloat) _param_ro_max_thr_speed, + (ParamFloat) _param_ro_speed_p, + (ParamFloat) _param_ro_speed_i, + (ParamFloat) _param_ro_yaw_stick_dz, + (ParamFloat) _param_ro_accel_limit, + (ParamFloat) _param_ro_decel_limit, + (ParamFloat) _param_ro_jerk_limit, + (ParamFloat) _param_ro_speed_limit, + (ParamFloat) _param_ro_speed_th, + (ParamFloat) _param_pp_lookahd_max, + (ParamFloat) _param_ro_yaw_rate_limit, + (ParamFloat) _param_nav_acc_rad + + ) +}; diff --git a/src/modules/rover_mecanum/RoverMecanumControl/CMakeLists.txt b/src/modules/rover_mecanum/MecanumRateControl/CMakeLists.txt similarity index 86% rename from src/modules/rover_mecanum/RoverMecanumControl/CMakeLists.txt rename to src/modules/rover_mecanum/MecanumRateControl/CMakeLists.txt index 61b64980f1..5a876fab55 100644 --- a/src/modules/rover_mecanum/RoverMecanumControl/CMakeLists.txt +++ b/src/modules/rover_mecanum/MecanumRateControl/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2024 PX4 Development Team. All rights reserved. +# Copyright (c) 2025 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -31,9 +31,9 @@ # ############################################################################ -px4_add_library(RoverMecanumControl - RoverMecanumControl.cpp +px4_add_library(MecanumRateControl + MecanumRateControl.cpp ) -target_link_libraries(RoverMecanumControl PUBLIC PID) -target_include_directories(RoverMecanumControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(MecanumRateControl PUBLIC PID) +target_include_directories(MecanumRateControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp b/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp new file mode 100644 index 0000000000..49f1ae5a8c --- /dev/null +++ b/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.cpp @@ -0,0 +1,190 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "MecanumRateControl.hpp" + +using namespace time_literals; + +MecanumRateControl::MecanumRateControl(ModuleParams *parent) : ModuleParams(parent) +{ + _rover_rate_setpoint_pub.advertise(); + _rover_throttle_setpoint_pub.advertise(); + _rover_steering_setpoint_pub.advertise(); + _rover_rate_status_pub.advertise(); + updateParams(); +} + +void MecanumRateControl::updateParams() +{ + ModuleParams::updateParams(); + _max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F; + _max_yaw_accel = _param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F; + _max_yaw_decel = _param_ro_yaw_decel_limit.get() * M_DEG_TO_RAD_F; + _pid_yaw_rate.setGains(_param_ro_yaw_rate_p.get(), _param_ro_yaw_rate_i.get(), 0.f); + _pid_yaw_rate.setIntegralLimit(1.f); + _pid_yaw_rate.setOutputLimit(1.f); + _adjusted_yaw_rate_setpoint.setSlewRate(_max_yaw_accel); +} + +void MecanumRateControl::updateRateControl() +{ + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); + } + + if (_vehicle_angular_velocity_sub.updated()) { + vehicle_angular_velocity_s vehicle_angular_velocity{}; + _vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity); + _vehicle_yaw_rate = fabsf(vehicle_angular_velocity.xyz[2]) > _param_ro_yaw_rate_th.get() * M_DEG_TO_RAD_F ? + vehicle_angular_velocity.xyz[2] : 0.f; + } + + if (_vehicle_control_mode.flag_control_rates_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) { + if (_vehicle_control_mode.flag_control_manual_enabled || _vehicle_control_mode.flag_control_offboard_enabled) { + generateRateSetpoint(); + } + + generateSteeringSetpoint(); + + } else { // Reset controller and slew rate when rate control is not active + _pid_yaw_rate.resetIntegral(); + _adjusted_yaw_rate_setpoint.setForcedValue(0.f); + } + + // Publish rate controller status (logging only) + rover_rate_status_s rover_rate_status; + rover_rate_status.timestamp = _timestamp; + rover_rate_status.measured_yaw_rate = _vehicle_yaw_rate; + rover_rate_status.adjusted_yaw_rate_setpoint = _adjusted_yaw_rate_setpoint.getState(); + rover_rate_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral(); + _rover_rate_status_pub.publish(rover_rate_status); + +} + +void MecanumRateControl::generateRateSetpoint() +{ + const bool acro_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled + && !_vehicle_control_mode.flag_control_position_enabled && !_vehicle_control_mode.flag_control_attitude_enabled; + + if (acro_mode_enabled && _manual_control_setpoint_sub.updated()) { // Acro Mode + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + rover_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = math::interpolate (manual_control_setpoint.yaw, -1.f, 1.f, + -_max_yaw_rate, _max_yaw_rate); + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + } + + } else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard rate control + trajectory_setpoint_s trajectory_setpoint{}; + _trajectory_setpoint_sub.copy(&trajectory_setpoint); + + if (_offboard_control_mode_sub.updated()) { + _offboard_control_mode_sub.copy(&_offboard_control_mode); + } + + const bool offboard_rate_control = _offboard_control_mode.body_rate && !_offboard_control_mode.attitude; + + if (offboard_rate_control && PX4_ISFINITE(trajectory_setpoint.yawspeed)) { + rover_rate_setpoint_s rover_rate_setpoint{}; + rover_rate_setpoint.timestamp = _timestamp; + rover_rate_setpoint.yaw_rate_setpoint = trajectory_setpoint.yawspeed; + _rover_rate_setpoint_pub.publish(rover_rate_setpoint); + } + } +} + +void MecanumRateControl::generateSteeringSetpoint() +{ + if (_rover_rate_setpoint_sub.updated()) { + _rover_rate_setpoint_sub.copy(&_rover_rate_setpoint); + + } + + float speed_diff_normalized{0.f}; + + if (PX4_ISFINITE(_rover_rate_setpoint.yaw_rate_setpoint) && PX4_ISFINITE(_vehicle_yaw_rate)) { + const float yaw_rate_setpoint = fabsf(_rover_rate_setpoint.yaw_rate_setpoint) > _param_ro_yaw_rate_th.get() * + M_DEG_TO_RAD_F ? + _rover_rate_setpoint.yaw_rate_setpoint : 0.f; + speed_diff_normalized = RoverControl::rateControl(_adjusted_yaw_rate_setpoint, _pid_yaw_rate, + yaw_rate_setpoint, _vehicle_yaw_rate, _param_rm_max_thr_yaw_r.get(), _max_yaw_accel, + _max_yaw_decel, _param_rm_wheel_track.get(), _dt); + } + + rover_steering_setpoint_s rover_steering_setpoint{}; + rover_steering_setpoint.timestamp = _timestamp; + rover_steering_setpoint.normalized_speed_diff = speed_diff_normalized; + _rover_steering_setpoint_pub.publish(rover_steering_setpoint); +} + +bool MecanumRateControl::runSanityChecks() +{ + bool ret = true; + + if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("mecanum_rate_control_conf_invalid_yaw_rate_lim"), events::Log::Error, + "Invalid configuration of necessary parameter RO_YAW_RATE_LIM", _param_ro_yaw_rate_limit.get()); + } + + } + + if ((_param_rm_wheel_track.get() < FLT_EPSILON || _param_rm_max_thr_yaw_r.get() < FLT_EPSILON) + && _param_ro_yaw_rate_p.get() < FLT_EPSILON) { + ret = false; + + if (_prev_param_check_passed) { + events::send(events::ID("mecanum_rate_control_conf_invalid_rate_control"), events::Log::Error, + "Invalid configuration for rate control: Neither feed forward (RM_MAX_THR_YAW_R) nor feedback (RO_YAW_RATE_P) is setup", + _param_rm_wheel_track.get(), + _param_rm_max_thr_yaw_r.get(), _param_ro_yaw_rate_p.get()); + } + } + + _prev_param_check_passed = ret; + return ret; +} diff --git a/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp b/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp new file mode 100644 index 0000000000..3e61c4cb1b --- /dev/null +++ b/src/modules/rover_mecanum/MecanumRateControl/MecanumRateControl.hpp @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +// PX4 includes +#include +#include + +// Libraries +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * @brief Class for mecanum rate control. + */ +class MecanumRateControl : public ModuleParams +{ +public: + /** + * @brief Constructor for MecanumRateControl. + * @param parent The parent ModuleParams object. + */ + MecanumRateControl(ModuleParams *parent); + ~MecanumRateControl() = default; + + /** + * @brief Update rate controller. + */ + void updateRateControl(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + + /** + * @brief Generate and publish roverRateSetpoint from manualControlSetpoint (Acro Mode) + * or trajectorySetpoint (Offboard rate control). + */ + void generateRateSetpoint(); + + /** + * @brief Generate and publish roverSteeringSetpoint from RoverRateSetpoint. + */ + void generateSteeringSetpoint(); + + /** + * @brief Check if the necessary parameters are set. + * @return True if all checks pass. + */ + bool runSanityChecks(); + + // uORB subscriptions + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; + uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)}; + uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)}; + uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + vehicle_control_mode_s _vehicle_control_mode{}; + offboard_control_mode_s _offboard_control_mode{}; + rover_rate_setpoint_s _rover_rate_setpoint{}; + + // uORB publications + uORB::Publication _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; + uORB::Publication _rover_rate_status_pub{ORB_ID(rover_rate_status)}; + + // Variables + hrt_abstime _timestamp{0}; + float _max_yaw_rate{0.f}; + float _max_yaw_accel{0.f}; + float _max_yaw_decel{0.f}; + float _vehicle_yaw_rate{0.f}; + float _dt{0.f}; // Time since last update [s]. + bool _prev_param_check_passed{true}; + + // Controllers + PID _pid_yaw_rate; + SlewRate _adjusted_yaw_rate_setpoint{0.f}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_rm_wheel_track, + (ParamFloat) _param_rm_max_thr_yaw_r, + (ParamFloat) _param_ro_yaw_rate_limit, + (ParamFloat) _param_ro_yaw_rate_th, + (ParamFloat) _param_ro_yaw_rate_p, + (ParamFloat) _param_ro_yaw_rate_i, + (ParamFloat) _param_ro_yaw_accel_limit, + (ParamFloat) _param_ro_yaw_decel_limit + ) +}; diff --git a/src/modules/rover_mecanum/RoverMecanum.cpp b/src/modules/rover_mecanum/RoverMecanum.cpp index b0729acc02..f881fcdc5d 100644 --- a/src/modules/rover_mecanum/RoverMecanum.cpp +++ b/src/modules/rover_mecanum/RoverMecanum.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. + * Copyright (c) 2025 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,15 +32,16 @@ ****************************************************************************/ #include "RoverMecanum.hpp" -using namespace matrix; + using namespace time_literals; RoverMecanum::RoverMecanum() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) { + _rover_throttle_setpoint_pub.advertise(); + _rover_steering_setpoint_pub.advertise(); updateParams(); - _rover_mecanum_setpoint_pub.advertise(); } bool RoverMecanum::init() @@ -53,266 +54,117 @@ void RoverMecanum::updateParams() { ModuleParams::updateParams(); - _max_yaw_rate = _param_rm_max_yaw_rate.get() * M_DEG_TO_RAD_F; + if (_param_ro_accel_limit.get() > FLT_EPSILON && _param_ro_max_thr_speed.get() > FLT_EPSILON) { + _throttle_body_x_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get()); + _throttle_body_y_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get()); + } } void RoverMecanum::Run() -{ - if (should_exit()) { - ScheduleClear(); - exit_and_cleanup(); - return; - } - - updateSubscriptions(); - - // Generate and publish attitude and velocity setpoints - hrt_abstime timestamp = hrt_absolute_time(); - - switch (_nav_state) { - case vehicle_status_s::NAVIGATION_STATE_MANUAL: { - manual_control_setpoint_s manual_control_setpoint{}; - rover_mecanum_setpoint_s rover_mecanum_setpoint{}; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - rover_mecanum_setpoint.timestamp = timestamp; - rover_mecanum_setpoint.forward_speed_setpoint = NAN; - rover_mecanum_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle; - rover_mecanum_setpoint.lateral_speed_setpoint = NAN; - rover_mecanum_setpoint.lateral_speed_setpoint_normalized = manual_control_setpoint.roll; - rover_mecanum_setpoint.yaw_rate_setpoint = NAN; - - if (_max_yaw_rate > FLT_EPSILON && _param_rm_max_thr_yaw_r.get() > FLT_EPSILON) { - const float scaled_yaw_rate_input = math::interpolate(manual_control_setpoint.yaw, - -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - const float speed_diff = scaled_yaw_rate_input * _param_rm_wheel_track.get() / 2.f; - rover_mecanum_setpoint.speed_diff_setpoint_normalized = math::interpolate(speed_diff, - -_param_rm_max_thr_yaw_r.get(), _param_rm_max_thr_yaw_r.get(), -1.f, 1.f); - - } else { - rover_mecanum_setpoint.speed_diff_setpoint_normalized = manual_control_setpoint.yaw; - - } - - rover_mecanum_setpoint.yaw_setpoint = NAN; - _rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint); - - } - } break; - - case vehicle_status_s::NAVIGATION_STATE_ACRO: { - manual_control_setpoint_s manual_control_setpoint{}; - rover_mecanum_setpoint_s rover_mecanum_setpoint{}; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - rover_mecanum_setpoint.timestamp = timestamp; - rover_mecanum_setpoint.forward_speed_setpoint = NAN; - rover_mecanum_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle; - rover_mecanum_setpoint.lateral_speed_setpoint = NAN; - rover_mecanum_setpoint.lateral_speed_setpoint_normalized = manual_control_setpoint.roll; - rover_mecanum_setpoint.yaw_rate_setpoint = math::interpolate(manual_control_setpoint.yaw, - -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - rover_mecanum_setpoint.speed_diff_setpoint_normalized = NAN; - rover_mecanum_setpoint.yaw_setpoint = NAN; - _rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint); - } - } break; - - case vehicle_status_s::NAVIGATION_STATE_STAB: { - manual_control_setpoint_s manual_control_setpoint{}; - rover_mecanum_setpoint_s rover_mecanum_setpoint{}; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - rover_mecanum_setpoint.timestamp = timestamp; - rover_mecanum_setpoint.forward_speed_setpoint = NAN; - rover_mecanum_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle; - rover_mecanum_setpoint.lateral_speed_setpoint = NAN; - rover_mecanum_setpoint.lateral_speed_setpoint_normalized = manual_control_setpoint.roll; - rover_mecanum_setpoint.yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.yaw, - STICK_DEADZONE), - -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - rover_mecanum_setpoint.speed_diff_setpoint_normalized = NAN; - rover_mecanum_setpoint.yaw_setpoint = NAN; - - if (fabsf(rover_mecanum_setpoint.yaw_rate_setpoint) > FLT_EPSILON - || (fabsf(rover_mecanum_setpoint.forward_speed_setpoint_normalized) < FLT_EPSILON - && fabsf(rover_mecanum_setpoint.lateral_speed_setpoint_normalized) < FLT_EPSILON)) { // Closed loop yaw rate control - _yaw_ctl = false; - - } else { // Closed loop yaw control - - if (!_yaw_ctl) { - _desired_yaw = _vehicle_yaw; - _yaw_ctl = true; - } - - rover_mecanum_setpoint.yaw_setpoint = _desired_yaw; - rover_mecanum_setpoint.yaw_rate_setpoint = NAN; - } - - _rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint); - } - } break; - - case vehicle_status_s::NAVIGATION_STATE_POSCTL: { - manual_control_setpoint_s manual_control_setpoint{}; - rover_mecanum_setpoint_s rover_mecanum_setpoint{}; - - if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { - rover_mecanum_setpoint.timestamp = timestamp; - rover_mecanum_setpoint.forward_speed_setpoint = math::interpolate(manual_control_setpoint.throttle, - -1.f, 1.f, -_param_rm_max_speed.get(), _param_rm_max_speed.get());; - rover_mecanum_setpoint.forward_speed_setpoint_normalized = NAN; - rover_mecanum_setpoint.lateral_speed_setpoint = math::interpolate(manual_control_setpoint.roll, - -1.f, 1.f, -_param_rm_max_speed.get(), _param_rm_max_speed.get()); - rover_mecanum_setpoint.lateral_speed_setpoint_normalized = NAN; - rover_mecanum_setpoint.yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.yaw, - STICK_DEADZONE), - -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - rover_mecanum_setpoint.speed_diff_setpoint_normalized = NAN; - rover_mecanum_setpoint.yaw_setpoint = NAN; - - // Reset cruise control if the manual input changes - if (_yaw_ctl && (!(fabsf(manual_control_setpoint.throttle - _prev_throttle) > STICK_DEADZONE) - || !(fabsf(manual_control_setpoint.roll - _prev_roll) > STICK_DEADZONE))) { - _yaw_ctl = false; - _prev_throttle = manual_control_setpoint.throttle; - _prev_roll = manual_control_setpoint.roll; - - } else if (!_yaw_ctl) { - _prev_throttle = manual_control_setpoint.throttle; - _prev_roll = manual_control_setpoint.roll; - } - - - if (fabsf(rover_mecanum_setpoint.yaw_rate_setpoint) > FLT_EPSILON - || (fabsf(rover_mecanum_setpoint.forward_speed_setpoint) < FLT_EPSILON - && fabsf(rover_mecanum_setpoint.lateral_speed_setpoint) < FLT_EPSILON)) { // Closed loop yaw rate control - rover_mecanum_setpoint.yaw_rate_setpoint = math::interpolate(manual_control_setpoint.yaw, - -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - rover_mecanum_setpoint.yaw_setpoint = NAN; - _yaw_ctl = false; - - } else { // Cruise control - const Vector3f velocity = Vector3f(rover_mecanum_setpoint.forward_speed_setpoint, - rover_mecanum_setpoint.lateral_speed_setpoint, 0.f); - const float desired_velocity_magnitude = velocity.norm(); - - if (!_yaw_ctl) { - _desired_yaw = _vehicle_yaw; - _yaw_ctl = true; - _pos_ctl_start_position_ned = _curr_pos_ned; - const Vector3f pos_ctl_course_direction_local = _vehicle_attitude_quaternion.rotateVector(velocity.normalized()); - _pos_ctl_course_direction = Vector2f(pos_ctl_course_direction_local(0), pos_ctl_course_direction_local(1)); - - } - - // Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover - const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(), - 2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment(); - const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + vector_scaling * _pos_ctl_course_direction; - const float desired_heading = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _pos_ctl_start_position_ned, - _curr_pos_ned, desired_velocity_magnitude); - const float heading_error = matrix::wrap_pi(desired_heading - _vehicle_yaw); - const Vector2f desired_velocity = desired_velocity_magnitude * Vector2f(cosf(heading_error), sinf(heading_error)); - rover_mecanum_setpoint.forward_speed_setpoint = desired_velocity(0); - rover_mecanum_setpoint.lateral_speed_setpoint = desired_velocity(1); - rover_mecanum_setpoint.yaw_setpoint = _desired_yaw; - rover_mecanum_setpoint.yaw_rate_setpoint = NAN; - } - - _rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint); - } - } break; - - case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: - case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: - _rover_mecanum_guidance.computeGuidance(_vehicle_yaw, _nav_state); - break; - - default: // Unimplemented nav states will stop the rover - rover_mecanum_setpoint_s rover_mecanum_setpoint{}; - rover_mecanum_setpoint.timestamp = timestamp; - rover_mecanum_setpoint.forward_speed_setpoint = NAN; - rover_mecanum_setpoint.forward_speed_setpoint_normalized = 0.f; - rover_mecanum_setpoint.lateral_speed_setpoint = NAN; - rover_mecanum_setpoint.lateral_speed_setpoint_normalized = 0.f; - rover_mecanum_setpoint.yaw_rate_setpoint = NAN; - rover_mecanum_setpoint.speed_diff_setpoint_normalized = 0.f; - rover_mecanum_setpoint.yaw_setpoint = NAN; - _rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint); - break; - } - - if (!_armed) { // Reset when disarmed - _rover_mecanum_control.resetControllers(); - _yaw_ctl = false; - } - - _rover_mecanum_control.computeMotorCommands(_vehicle_yaw, _vehicle_yaw_rate, _vehicle_forward_speed, - _vehicle_lateral_speed); - -} - -void RoverMecanum::updateSubscriptions() { if (_parameter_update_sub.updated()) { - parameter_update_s parameter_update; - _parameter_update_sub.copy(¶meter_update); updateParams(); } - if (_vehicle_status_sub.updated()) { - vehicle_status_s vehicle_status{}; - _vehicle_status_sub.copy(&vehicle_status); + const hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + _dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - if (vehicle_status.nav_state != _nav_state) { - _rover_mecanum_control.resetControllers(); - _yaw_ctl = false; + _mecanum_pos_vel_control.updatePosControl(); + _mecanum_att_control.updateAttControl(); + _mecanum_rate_control.updateRateControl(); - if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION - || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { - _rover_mecanum_guidance.setDesiredYaw(_vehicle_yaw); - } - } - - _nav_state = vehicle_status.nav_state; - _armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED; + if (_vehicle_control_mode_sub.updated()) { + _vehicle_control_mode_sub.copy(&_vehicle_control_mode); } - if (_vehicle_local_position_sub.updated()) { - vehicle_local_position_s vehicle_local_position{}; - _vehicle_local_position_sub.copy(&vehicle_local_position); - _curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y); - Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); - Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); - // Apply threshold to the velocity measurement to cut off measurement noise when standing still - _vehicle_forward_speed = fabsf(velocity_in_body_frame(0)) > SPEED_THRESHOLD ? velocity_in_body_frame(0) : 0.f; - _vehicle_lateral_speed = fabsf(velocity_in_body_frame(1)) > SPEED_THRESHOLD ? velocity_in_body_frame(1) : 0.f; + const bool full_manual_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled + && !_vehicle_control_mode.flag_control_position_enabled && !_vehicle_control_mode.flag_control_attitude_enabled + && !_vehicle_control_mode.flag_control_rates_enabled; + + if (full_manual_mode_enabled) { // Manual mode + generateSteeringSetpoint(); } - if (_vehicle_angular_velocity_sub.updated()) { - vehicle_angular_velocity_s vehicle_angular_velocity{}; - _vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity); - - // Apply threshold to the yaw rate measurement if the rover is standing still to avoid stuttering due to closed loop yaw(rate) control - if ((fabsf(_vehicle_forward_speed) > FLT_EPSILON && fabsf(_vehicle_lateral_speed) > FLT_EPSILON) - || fabsf(vehicle_angular_velocity.xyz[2]) > YAW_RATE_THRESHOLD) { - _vehicle_yaw_rate = vehicle_angular_velocity.xyz[2]; - - } else { - _vehicle_yaw_rate = 0.f; - } + if (_vehicle_control_mode.flag_armed) { + generateActuatorSetpoint(); } - if (_vehicle_attitude_sub.updated()) { - vehicle_attitude_s vehicle_attitude{}; - _vehicle_attitude_sub.copy(&vehicle_attitude); - _vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); - _vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi(); +} + +void RoverMecanum::generateSteeringSetpoint() +{ + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + rover_steering_setpoint_s rover_steering_setpoint{}; + rover_steering_setpoint.timestamp = _timestamp; + rover_steering_setpoint.normalized_speed_diff = manual_control_setpoint.yaw; + _rover_steering_setpoint_pub.publish(rover_steering_setpoint); + rover_throttle_setpoint_s rover_throttle_setpoint{}; + rover_throttle_setpoint.timestamp = _timestamp; + rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle; + rover_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll; + _rover_throttle_setpoint_pub.publish(rover_throttle_setpoint); + } +} + +void RoverMecanum::generateActuatorSetpoint() +{ + if (_rover_throttle_setpoint_sub.updated()) { + _rover_throttle_setpoint_sub.copy(&_rover_throttle_setpoint); } + if (_actuator_motors_sub.updated()) { + actuator_motors_s actuator_motors{}; + _actuator_motors_sub.copy(&actuator_motors); + _current_throttle_body_x = (actuator_motors.control[0] + actuator_motors.control[1]) / 2.f; + _current_throttle_body_y = (actuator_motors.control[2] - actuator_motors.control[0]) / 2.f; + } + + if (_rover_steering_setpoint_sub.updated()) { + _rover_steering_setpoint_sub.copy(&_rover_steering_setpoint); + } + + const float throttle_body_x = RoverControl::throttleControl(_throttle_body_x_setpoint, + _rover_throttle_setpoint.throttle_body_x, _current_throttle_body_x, _param_ro_accel_limit.get(), + _param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), _dt); + const float throttle_body_y = RoverControl::throttleControl(_throttle_body_y_setpoint, + _rover_throttle_setpoint.throttle_body_y, _current_throttle_body_y, _param_ro_accel_limit.get(), + _param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), _dt); + actuator_motors_s actuator_motors{}; + actuator_motors.reversible_flags = _param_r_rev.get(); + computeInverseKinematics(throttle_body_x, throttle_body_y, + _rover_steering_setpoint.normalized_speed_diff).copyTo(actuator_motors.control); + actuator_motors.timestamp = _timestamp; + _actuator_motors_pub.publish(actuator_motors); + + +} + +Vector4f RoverMecanum::computeInverseKinematics(float throttle_body_x, float throttle_body_y, + const float speed_diff_normalized) +{ + const float total_speed = fabsf(throttle_body_x) + fabsf(throttle_body_y) + fabsf(speed_diff_normalized); + + if (total_speed > 1.f) { // Adjust speed setpoints if infeasible + const float theta = atan2f(fabsf(throttle_body_y), fabsf(throttle_body_x)); + const float magnitude = (1.f - fabsf(speed_diff_normalized)) / (sinf(theta) + cosf(theta)); + const float normalization = 1.f / (sqrtf(powf(throttle_body_x, 2.f) + powf(throttle_body_y, 2.f))); + throttle_body_x *= magnitude * normalization; + throttle_body_y *= magnitude * normalization; + + } + + // Calculate motor commands + const float input_data[3] = {throttle_body_x, throttle_body_y, speed_diff_normalized}; + const Matrix input(input_data); + const float m_data[12] = {1.f, -1.f, -1.f, 1.f, 1.f, 1.f, 1.f, 1.f, -1.f, 1.f, -1.f, 1.f}; + const Matrix m(m_data); + const Vector4f motor_commands = m * input; + + return motor_commands; } int RoverMecanum::task_spawn(int argc, char *argv[]) @@ -340,7 +192,7 @@ int RoverMecanum::task_spawn(int argc, char *argv[]) int RoverMecanum::custom_command(int argc, char *argv[]) { - return print_usage("unk_timestampn command"); + return print_usage("unknown command"); } int RoverMecanum::print_usage(const char *reason) @@ -352,7 +204,7 @@ int RoverMecanum::print_usage(const char *reason) PRINT_MODULE_DESCRIPTION( R"DESCR_STR( ### Description -Rover Mecanum controller. +Rover mecanum module. )DESCR_STR"); PRINT_MODULE_USAGE_NAME("rover_mecanum", "controller"); diff --git a/src/modules/rover_mecanum/RoverMecanum.hpp b/src/modules/rover_mecanum/RoverMecanum.hpp index 7c57a1138f..1812e1c7c4 100644 --- a/src/modules/rover_mecanum/RoverMecanum.hpp +++ b/src/modules/rover_mecanum/RoverMecanum.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * Copyright (c) 2025 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -39,41 +39,34 @@ #include #include #include -#include + +// Libraries +#include +#include // uORB includes -#include #include -#include +#include +#include #include -#include -#include -#include -#include -#include - -// Standard libraries -#include -#include +#include +#include +#include +#include +#include // Local includes -#include "RoverMecanumGuidance/RoverMecanumGuidance.hpp" -#include "RoverMecanumControl/RoverMecanumControl.hpp" - -// Constants -static constexpr float YAW_RATE_THRESHOLD = - 0.02f; // [rad/s] Threshold for the yaw rate measurement to avoid stuttering when the rover is standing still -static constexpr float SPEED_THRESHOLD = - 0.1f; // [m/s] Threshold for the speed measurement to cut off measurement noise when the rover is standing still -static constexpr float STICK_DEADZONE = - 0.1f; // [0, 1] Percentage of stick input range that will be interpreted as zero around the stick centered value - -using namespace time_literals; +#include "MecanumRateControl/MecanumRateControl.hpp" +#include "MecanumAttControl/MecanumAttControl.hpp" +#include "MecanumPosVelControl/MecanumPosVelControl.hpp" class RoverMecanum : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: + /** + * @brief Constructor for RoverMecanum + */ RoverMecanum(); ~RoverMecanum() override = default; @@ -89,54 +82,69 @@ public: bool init(); protected: + /** + * @brief Update the parameters of the module. + */ void updateParams() override; private: void Run() override; /** - * @brief Update uORB subscriptions. + * @brief Generate and publish roverSteeringSetpoint from manualControlSetpoint (Manual Mode). */ - void updateSubscriptions(); + void generateSteeringSetpoint(); - // uORB Subscriptions - uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + /** + * @brief Generate and publish actuatorMotors setpoints from roverThrottleSetpoint/roverSteeringSetpoint. + */ + void generateActuatorSetpoint(); + + /** + * @brief Compute normalized motor commands based on normalized setpoints. + * @param throttle_body_x Normalized speed in body x direction [-1, 1]. + * @param throttle_body_y Normalized speed in body y direction [-1, 1]. + * @param speed_diff_normalized Speed difference between left and right wheels [-1, 1]. + * @return Motor speeds [-1, 1]. + */ + Vector4f computeInverseKinematics(float throttle_body_x, float throttle_body_y, const float speed_diff_normalized); + + // uORB subscriptions uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; - uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; - uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; - uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)}; + uORB::Subscription _rover_throttle_setpoint_sub{ORB_ID(rover_throttle_setpoint)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)}; + vehicle_control_mode_s _vehicle_control_mode{}; + rover_steering_setpoint_s _rover_steering_setpoint{}; + rover_throttle_setpoint_s _rover_throttle_setpoint{}; - // uORB Publications - uORB::Publication _rover_mecanum_setpoint_pub{ORB_ID(rover_mecanum_setpoint)}; + // uORB publications + uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; + uORB::Publication _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)}; + uORB::Publication _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)}; - // Instances - RoverMecanumGuidance _rover_mecanum_guidance{this}; - RoverMecanumControl _rover_mecanum_control{this}; - PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library + // Class instances + MecanumRateControl _mecanum_rate_control{this}; + MecanumAttControl _mecanum_att_control{this}; + MecanumPosVelControl _mecanum_pos_vel_control{this}; // Variables - matrix::Quatf _vehicle_attitude_quaternion{}; - float _vehicle_yaw_rate{0.f}; - float _vehicle_forward_speed{0.f}; - float _vehicle_lateral_speed{0.f}; - float _vehicle_yaw{0.f}; - float _max_yaw_rate{0.f}; - int _nav_state{0}; - bool _yaw_ctl{false}; // Indicates if the rover is doing yaw or yaw rate control in position mode - float _desired_yaw{0.f}; // Yaw setpoint for position mode - Vector2f _pos_ctl_start_position_ned{}; - Vector2f _pos_ctl_course_direction{}; - Vector2f _curr_pos_ned{}; - float _prev_throttle{0.f}; - float _prev_roll{0.f}; - bool _armed{false}; + hrt_abstime _timestamp{0}; + float _dt{0.f}; + float _current_throttle_body_x{0.f}; + float _current_throttle_body_y{0.f}; + // Controllers + SlewRate _throttle_body_x_setpoint{0.f}; + SlewRate _throttle_body_y_setpoint{0.f}; + + // Parameters DEFINE_PARAMETERS( - (ParamFloat) _param_rm_max_speed, - (ParamFloat) _param_rm_wheel_track, - (ParamFloat) _param_rm_max_thr_yaw_r, - (ParamFloat) _param_rm_max_yaw_rate, - (ParamFloat) _param_pp_lookahd_max + (ParamInt) _param_r_rev, + (ParamFloat) _param_ro_accel_limit, + (ParamFloat) _param_ro_decel_limit, + (ParamFloat) _param_ro_max_thr_speed ) }; diff --git a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp deleted file mode 100644 index c581236784..0000000000 --- a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp +++ /dev/null @@ -1,323 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "RoverMecanumControl.hpp" - -#include - -using namespace matrix; -using namespace time_literals; - -RoverMecanumControl::RoverMecanumControl(ModuleParams *parent) : ModuleParams(parent) -{ - updateParams(); - _rover_mecanum_status_pub.advertise(); -} - -void RoverMecanumControl::updateParams() -{ - ModuleParams::updateParams(); - - _pid_yaw_rate.setGains(_param_rm_yaw_rate_p.get(), _param_rm_yaw_rate_i.get(), 0.f); - _pid_yaw_rate.setIntegralLimit(1.f); - _pid_yaw_rate.setOutputLimit(1.f); - - _pid_forward_throttle.setGains(_param_rm_p_gain_speed.get(), _param_rm_i_gain_speed.get(), 0.f); - _pid_forward_throttle.setIntegralLimit(1.f); - _pid_forward_throttle.setOutputLimit(1.f); - - _pid_lateral_throttle.setGains(_param_rm_p_gain_speed.get(), _param_rm_i_gain_speed.get(), 0.f); - _pid_lateral_throttle.setIntegralLimit(1.f); - _pid_lateral_throttle.setOutputLimit(1.f); - - _max_yaw_rate = _param_rm_max_yaw_rate.get() * M_DEG_TO_RAD_F; - _max_yaw_accel = _param_rm_max_yaw_accel.get() * M_DEG_TO_RAD_F; - _pid_yaw.setGains(_param_rm_p_gain_yaw.get(), _param_rm_i_gain_yaw.get(), 0.f); - _pid_yaw.setIntegralLimit(_max_yaw_rate); - _pid_yaw.setOutputLimit(_max_yaw_rate); - - // Update slew rates - if (_max_yaw_rate > FLT_EPSILON) { - _yaw_setpoint_with_yaw_rate_limit.setSlewRate(_max_yaw_rate); - } - - if (_max_yaw_accel > FLT_EPSILON) { - _yaw_rate_with_accel_limit.setSlewRate(_max_yaw_accel); - } -} - -void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const float vehicle_yaw_rate, - const float vehicle_forward_speed, const float vehicle_lateral_speed) -{ - // Timestamps - hrt_abstime timestamp_prev = _timestamp; - _timestamp = hrt_absolute_time(); - const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - - // Update mecanum setpoint - _rover_mecanum_setpoint_sub.update(&_rover_mecanum_setpoint); - - // Closed loop yaw control - if (PX4_ISFINITE(_rover_mecanum_setpoint.yaw_setpoint)) { - _yaw_setpoint_with_yaw_rate_limit.update(matrix::wrap_pi(_rover_mecanum_setpoint.yaw_setpoint), dt); - _rover_mecanum_status.adjusted_yaw_setpoint = matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState()); - _pid_yaw.setSetpoint( - matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState() - - vehicle_yaw)); // error as setpoint to take care of wrapping - _rover_mecanum_setpoint.yaw_rate_setpoint = _pid_yaw.update(0.f, dt); - _rover_mecanum_status.clyaw_yaw_rate_setpoint = _rover_mecanum_setpoint.yaw_rate_setpoint; - - } else { - _pid_yaw.resetIntegral(); - _yaw_setpoint_with_yaw_rate_limit.setForcedValue(vehicle_yaw); - } - - // Yaw rate control - float speed_diff_normalized{0.f}; - - if (PX4_ISFINITE(_rover_mecanum_setpoint.yaw_rate_setpoint)) { // Closed loop yaw rate control - speed_diff_normalized = calcNormalizedSpeedDiff(_rover_mecanum_setpoint.yaw_rate_setpoint, vehicle_yaw_rate, - _param_rm_max_thr_yaw_r.get(), _max_yaw_accel, _param_rm_wheel_track.get(), dt, _yaw_rate_with_accel_limit, - _pid_yaw_rate, false); - _rover_mecanum_status.adjusted_yaw_rate_setpoint = _yaw_rate_with_accel_limit.getState(); - - } else { // Use normalized setpoint - speed_diff_normalized = calcNormalizedSpeedDiff(_rover_mecanum_setpoint.speed_diff_setpoint_normalized, - vehicle_yaw_rate, - _param_rm_max_thr_yaw_r.get(), _max_yaw_accel, _param_rm_wheel_track.get(), dt, _yaw_rate_with_accel_limit, - _pid_yaw_rate, true); - } - - // Speed control - float forward_speed_normalized{0.f}; - float lateral_speed_normalized{0.f}; - - if (PX4_ISFINITE(_rover_mecanum_setpoint.forward_speed_setpoint) - && PX4_ISFINITE(_rover_mecanum_setpoint.lateral_speed_setpoint)) { // Closed loop speed control - - if (_param_rm_max_thr_spd.get() > FLT_EPSILON) { - - forward_speed_normalized = math::interpolate(_rover_mecanum_setpoint.forward_speed_setpoint, - -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get(), -1.f, 1.f); - - lateral_speed_normalized = math::interpolate(_rover_mecanum_setpoint.lateral_speed_setpoint, - -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get(), -1.f, 1.f); - - const float total_speed = fabsf(forward_speed_normalized) + fabsf(lateral_speed_normalized) + fabsf( - speed_diff_normalized); - - if (total_speed > 1.f) { // Adjust speed setpoints if infeasible - const float theta = atan2f(fabsf(lateral_speed_normalized), fabsf(forward_speed_normalized)); - const float magnitude = (1.f - fabsf(speed_diff_normalized)) / (sinf(theta) + cosf(theta)); - const float normalization = 1.f / (sqrtf(powf(forward_speed_normalized, 2.f) + powf(lateral_speed_normalized, 2.f))); - forward_speed_normalized *= magnitude * normalization; - lateral_speed_normalized *= magnitude * normalization; - _rover_mecanum_setpoint.forward_speed_setpoint = math::interpolate(forward_speed_normalized, -1.f, 1.f, - -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get()); - _rover_mecanum_setpoint.lateral_speed_setpoint = math::interpolate(lateral_speed_normalized, -1.f, 1.f, - -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get()); - } - } - - forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_mecanum_setpoint.forward_speed_setpoint, - vehicle_forward_speed, _param_rm_max_thr_spd.get(), _forward_speed_setpoint_with_accel_limit, _pid_forward_throttle, - _param_rm_max_accel.get(), _param_rm_max_decel.get(), dt, false); - lateral_speed_normalized = calcNormalizedSpeedSetpoint(_rover_mecanum_setpoint.lateral_speed_setpoint, - vehicle_lateral_speed, _param_rm_max_thr_spd.get(), _lateral_speed_setpoint_with_accel_limit, _pid_lateral_throttle, - _param_rm_max_accel.get(), _param_rm_max_decel.get(), dt, false); - _rover_mecanum_status.adjusted_forward_speed_setpoint = _forward_speed_setpoint_with_accel_limit.getState(); - _rover_mecanum_status.adjusted_lateral_speed_setpoint = _lateral_speed_setpoint_with_accel_limit.getState(); - - - } else if (PX4_ISFINITE(_rover_mecanum_setpoint.forward_speed_setpoint_normalized) - && PX4_ISFINITE(_rover_mecanum_setpoint.lateral_speed_setpoint_normalized)) { // Use normalized setpoint - forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_mecanum_setpoint.forward_speed_setpoint_normalized, - vehicle_forward_speed, _param_rm_max_thr_spd.get(), _forward_speed_setpoint_with_accel_limit, _pid_forward_throttle, - _param_rm_max_accel.get(), _param_rm_max_decel.get(), dt, true); - lateral_speed_normalized = calcNormalizedSpeedSetpoint(_rover_mecanum_setpoint.lateral_speed_setpoint_normalized, - vehicle_lateral_speed, _param_rm_max_thr_spd.get(), _lateral_speed_setpoint_with_accel_limit, _pid_lateral_throttle, - _param_rm_max_accel.get(), _param_rm_max_decel.get(), dt, true); - - } - - // Publish rover mecanum status (logging) - _rover_mecanum_status.timestamp = _timestamp; - _rover_mecanum_status.measured_forward_speed = vehicle_forward_speed; - _rover_mecanum_status.measured_lateral_speed = vehicle_lateral_speed; - _rover_mecanum_status.measured_yaw_rate = vehicle_yaw_rate; - _rover_mecanum_status.measured_yaw = vehicle_yaw; - _rover_mecanum_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral(); - _rover_mecanum_status.pid_yaw_integral = _pid_yaw.getIntegral(); - _rover_mecanum_status.pid_forward_throttle_integral = _pid_forward_throttle.getIntegral(); - _rover_mecanum_status.pid_lateral_throttle_integral = _pid_lateral_throttle.getIntegral(); - _rover_mecanum_status_pub.publish(_rover_mecanum_status); - - // Publish to motors - actuator_motors_s actuator_motors{}; - actuator_motors.reversible_flags = _param_r_rev.get(); - computeInverseKinematics(forward_speed_normalized, lateral_speed_normalized, - speed_diff_normalized).copyTo(actuator_motors.control); - actuator_motors.timestamp = _timestamp; - _actuator_motors_pub.publish(actuator_motors); - -} - -float RoverMecanumControl::calcNormalizedSpeedDiff(const float yaw_rate_setpoint, const float vehicle_yaw_rate, - const float max_thr_yaw_r, - const float max_yaw_accel, const float wheel_track, const float dt, SlewRate &yaw_rate_with_accel_limit, - PID &pid_yaw_rate, const bool normalized) -{ - float slew_rate_normalization{1.f}; - - if (normalized) { // Slew rate needs to be normalized if the setpoint is normalized - slew_rate_normalization = max_thr_yaw_r > FLT_EPSILON ? max_thr_yaw_r : 0.f; - } - - if (max_yaw_accel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) { - yaw_rate_with_accel_limit.setSlewRate(max_yaw_accel / slew_rate_normalization); - yaw_rate_with_accel_limit.update(yaw_rate_setpoint, dt); - - } else { - yaw_rate_with_accel_limit.setForcedValue(yaw_rate_setpoint); - } - - // Transform yaw rate into speed difference - float speed_diff_normalized{0.f}; - - if (normalized) { - speed_diff_normalized = yaw_rate_with_accel_limit.getState(); - - } else { - if (wheel_track > FLT_EPSILON && max_thr_yaw_r > FLT_EPSILON) { // Feedforward - const float speed_diff = yaw_rate_with_accel_limit.getState() * wheel_track / - 2.f; - speed_diff_normalized = math::interpolate(speed_diff, -max_thr_yaw_r, - max_thr_yaw_r, -1.f, 1.f); - } - - _pid_yaw_rate.setSetpoint(yaw_rate_with_accel_limit.getState()); - speed_diff_normalized = math::constrain(speed_diff_normalized + - _pid_yaw_rate.update(vehicle_yaw_rate, dt), - -1.f, 1.f); // Feedback - - - } - - return math::constrain(speed_diff_normalized, -1.f, 1.f); - -} - -float RoverMecanumControl::calcNormalizedSpeedSetpoint(const float speed_setpoint, - const float vehicle_speed, const float max_thr_spd, SlewRate &speed_setpoint_with_accel_limit, - PID &pid_throttle, const float max_accel, const float max_decel, const float dt, const bool normalized) -{ - float slew_rate_normalization{1.f}; - - if (normalized) { // Slew rate needs to be normalized if the setpoint is normalized - slew_rate_normalization = max_thr_spd > FLT_EPSILON ? max_thr_spd : 0.f; - } - - // Apply acceleration and deceleration limit - if (fabsf(speed_setpoint) >= fabsf(speed_setpoint_with_accel_limit.getState())) { - if (max_accel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) { - speed_setpoint_with_accel_limit.setSlewRate(max_accel / slew_rate_normalization); - speed_setpoint_with_accel_limit.update(speed_setpoint, dt); - - } else { - speed_setpoint_with_accel_limit.setForcedValue(speed_setpoint); - - } - - } else if (max_decel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) { - speed_setpoint_with_accel_limit.setSlewRate(max_decel / slew_rate_normalization); - speed_setpoint_with_accel_limit.update(speed_setpoint, dt); - - } else { - speed_setpoint_with_accel_limit.setForcedValue(speed_setpoint); - } - - // Calculate normalized forward speed setpoint - float forward_speed_normalized{0.f}; - - if (normalized) { - forward_speed_normalized = speed_setpoint_with_accel_limit.getState(); - - } else { // Closed loop speed control - - if (_param_rm_max_thr_spd.get() > FLT_EPSILON) { // Feedforward - forward_speed_normalized = math::interpolate(speed_setpoint_with_accel_limit.getState(), - -max_thr_spd, max_thr_spd, - -1.f, 1.f); - } - - pid_throttle.setSetpoint(speed_setpoint_with_accel_limit.getState()); - forward_speed_normalized += pid_throttle.update(vehicle_speed, dt); // Feedback - - } - - return math::constrain(forward_speed_normalized, -1.f, 1.f); - -} - -matrix::Vector4f RoverMecanumControl::computeInverseKinematics(float forward_speed_normalized, - float lateral_speed_normalized, - float speed_diff) -{ - const float total_speed = fabsf(forward_speed_normalized) + fabsf(lateral_speed_normalized) + fabsf(speed_diff); - - if (total_speed > 1.f) { // Adjust speed setpoints if infeasible - const float theta = atan2f(fabsf(lateral_speed_normalized), fabsf(forward_speed_normalized)); - const float magnitude = (1.f - fabsf(speed_diff)) / (sinf(theta) + cosf(theta)); - const float normalization = 1.f / (sqrtf(powf(forward_speed_normalized, 2.f) + powf(lateral_speed_normalized, 2.f))); - forward_speed_normalized *= magnitude * normalization; - lateral_speed_normalized *= magnitude * normalization; - - } - - // Calculate motor commands - const float input_data[3] = {forward_speed_normalized, lateral_speed_normalized, speed_diff}; - const Matrix input(input_data); - const float m_data[12] = {1.f, -1.f, -1.f, 1.f, 1.f, 1.f, 1.f, 1.f, -1.f, 1.f, -1.f, 1.f}; - const Matrix m(m_data); - const Vector4f motor_commands = m * input; - - return motor_commands; -} - -void RoverMecanumControl::resetControllers() -{ - _pid_forward_throttle.resetIntegral(); - _pid_lateral_throttle.resetIntegral(); - _pid_yaw_rate.resetIntegral(); - _pid_yaw.resetIntegral(); -} diff --git a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp deleted file mode 100644 index 53ae266383..0000000000 --- a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.hpp +++ /dev/null @@ -1,177 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2024 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -// PX4 includes -#include - -// uORB includes -#include -#include -#include -#include -#include -#include -#include -#include - -// Standard libraries -#include -#include -#include -#include -#include - -using namespace matrix; - -/** - * @brief Class for mecanum rover guidance. - */ -class RoverMecanumControl : public ModuleParams -{ -public: - /** - * @brief Constructor for RoverMecanumControl. - * @param parent The parent ModuleParams object. - */ - RoverMecanumControl(ModuleParams *parent); - ~RoverMecanumControl() = default; - - /** - * @brief Compute motor commands based on setpoints - * @param vehicle_yaw Yaw of the vehicle [rad] - * @param vehicle_yaw_rate Yaw rate of the vehicle [rad/s] - * @param vehicle_forward_speed Forward speed of the vehicle [m/s] - * @param vehicle_lateral_speed Lateral speed of the vehicle [m/s] - */ - void computeMotorCommands(float vehicle_yaw, float vehicle_yaw_rate, float vehicle_forward_speed, - float vehicle_lateral_speed); - - /** - * @brief Reset PID controllers - */ - void resetControllers(); - -protected: - /** - * @brief Update the parameters of the module. - */ - void updateParams() override; - -private: - /** - * @brief Compute normalized speed diff setpoint between the left and right wheels and apply slew rates. - * @param yaw_rate_setpoint Yaw rate setpoint [rad/s or normalized [-1, 1]]. - * @param vehicle_yaw_rate Measured yaw rate [rad/s]. - * @param max_thr_yaw_r Yaw rate turning left/right wheels at max speed in opposite directions [m/s]. - * @param max_yaw_accel Maximum allowed yaw acceleration for the rover [rad/s^2]. - * @param wheel_track Wheel track [m]. - * @param dt Time since last update [s]. - * @param yaw_rate_with_accel_limit Yaw rate slew rate. - * @param pid_yaw_rate Yaw rate PID - * @param normalized Indicates if the forward speed setpoint is already normalized. - * @return Normalized speed differece setpoint with applied slew rates [-1, 1]. - */ - float calcNormalizedSpeedDiff(float yaw_rate_setpoint, float vehicle_yaw_rate, float max_thr_yaw_r, float max_yaw_accel, - float wheel_track, float dt, SlewRate &yaw_rate_with_accel_limit, PID &pid_yaw_rate, bool normalized); - /** - * @brief Compute normalized speed setpoint and apply slew rates. - * to the speed setpoint and doing closed loop speed control if enabled. - * @param speed_setpoint Speed setpoint [m/s]. - * @param vehicle_speed Actual speed [m/s]. - * @param max_thr_spd Speed the rover drives at maximum throttle [m/s]. - * @param speed_setpoint_with_accel_limit Speed slew rate. - * @param pid_throttle Throttle PID - * @param max_accel Maximum acceleration [m/s^2] - * @param max_decel Maximum deceleration [m/s^2] - * @param dt Time since last update [s]. - * @param normalized Indicates if the speed setpoint is already normalized. - * @return Normalized speed setpoint with applied slew rates [-1, 1]. - */ - float calcNormalizedSpeedSetpoint(float speed_setpoint, float vehicle_forward_speed, float max_thr_spd, - SlewRate &speed_setpoint_with_accel_limit, PID &pid_throttle, float max_accel, float max_decel, - float dt, bool normalized); - - /** - * @brief Turn normalized speed setpoints into normalized motor commands. - * - * @param forward_speed Normalized linear speed in body forward direction [-1, 1]. - * @param lateral_speed Normalized linear speed in body lateral direction [-1, 1]. - * @param speed_diff Normalized speed difference between left and right wheels [-1, 1]. - * @return matrix::Vector4f: Normalized motor inputs [-1, 1] - */ - matrix::Vector4f computeInverseKinematics(float forward_speed, float lateral_speed, float speed_diff); - - // uORB subscriptions - uORB::Subscription _rover_mecanum_setpoint_sub{ORB_ID(rover_mecanum_setpoint)}; - - // uORB publications - uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; - uORB::Publication _rover_mecanum_status_pub{ORB_ID(rover_mecanum_status)}; - rover_mecanum_status_s _rover_mecanum_status{}; - - // Variables - rover_mecanum_setpoint_s _rover_mecanum_setpoint{}; - hrt_abstime _timestamp{0}; - float _max_yaw_rate{0.f}; - float _max_yaw_accel{0.f}; - - // Controllers - PID _pid_forward_throttle; // PID for the closed loop forward speed control - PID _pid_lateral_throttle; // PID for the closed loop lateral speed control - PID _pid_yaw; // PID for the closed loop yaw control - PID _pid_yaw_rate; // PID for the closed loop yaw rate control - SlewRate _forward_speed_setpoint_with_accel_limit{0.f}; - SlewRate _lateral_speed_setpoint_with_accel_limit{0.f}; - SlewRate _yaw_rate_with_accel_limit{0.f}; - SlewRateYaw _yaw_setpoint_with_yaw_rate_limit; - - // Parameters - DEFINE_PARAMETERS( - (ParamFloat) _param_rm_wheel_track, - (ParamFloat) _param_rm_max_thr_spd, - (ParamFloat) _param_rm_max_accel, - (ParamFloat) _param_rm_max_decel, - (ParamFloat) _param_rm_max_thr_yaw_r, - (ParamFloat) _param_rm_max_yaw_rate, - (ParamFloat) _param_rm_max_yaw_accel, - (ParamFloat) _param_rm_yaw_rate_p, - (ParamFloat) _param_rm_yaw_rate_i, - (ParamFloat) _param_rm_p_gain_speed, - (ParamFloat) _param_rm_i_gain_speed, - (ParamFloat) _param_rm_p_gain_yaw, - (ParamFloat) _param_rm_i_gain_yaw, - (ParamInt) _param_r_rev - ) -}; diff --git a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp deleted file mode 100644 index b72478d746..0000000000 --- a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp +++ /dev/null @@ -1,206 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include "RoverMecanumGuidance.hpp" - -#include - -using namespace matrix; -using namespace time_literals; - -RoverMecanumGuidance::RoverMecanumGuidance(ModuleParams *parent) : ModuleParams(parent) -{ - updateParams(); - _max_velocity_magnitude = _param_rm_max_speed.get(); - _rover_mecanum_guidance_status_pub.advertise(); -} - -void RoverMecanumGuidance::updateParams() -{ - ModuleParams::updateParams(); -} - -void RoverMecanumGuidance::computeGuidance(const float yaw, const int nav_state) -{ - updateSubscriptions(); - - // Calculate desired velocity magnitude - float desired_velocity_magnitude{_max_velocity_magnitude}; - const float distance_to_curr_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), - _curr_wp(0), _curr_wp(1)); - - if (_param_rm_max_jerk.get() > FLT_EPSILON && _param_rm_max_accel.get() > FLT_EPSILON - && _param_rm_miss_vel_gain.get() > FLT_EPSILON) { - const float speed_reduction = math::constrain(_param_rm_miss_vel_gain.get() * math::interpolate( - M_PI_F - _waypoint_transition_angle, 0.f, - M_PI_F, 0.f, 1.f), 0.f, 1.f); - desired_velocity_magnitude = math::trajectory::computeMaxSpeedFromDistance(_param_rm_max_jerk.get(), - _param_rm_max_accel.get(), distance_to_curr_wp, _max_velocity_magnitude * (1.f - speed_reduction)); - desired_velocity_magnitude = math::constrain(desired_velocity_magnitude, -_max_velocity_magnitude, - _max_velocity_magnitude); - } - - - // Calculate heading error - const float desired_heading = _pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, - desired_velocity_magnitude); - const float heading_error = matrix::wrap_pi(desired_heading - yaw); - - // Catch return to launch - const float distance_to_next_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), - _next_wp(0), _next_wp(1)); - - if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { - _mission_finished = distance_to_next_wp < _param_nav_acc_rad.get(); - } - - // Guidance logic - Vector2f desired_velocity(0.f, 0.f); - - if (!_mission_finished && distance_to_curr_wp > _param_nav_acc_rad.get()) { - desired_velocity = desired_velocity_magnitude * Vector2f(cosf(heading_error), sinf(heading_error)); - } - - // Timestamp - hrt_abstime timestamp = hrt_absolute_time(); - - // Publish mecanum controller status (logging) - rover_mecanum_guidance_status_s rover_mecanum_guidance_status{}; - rover_mecanum_guidance_status.timestamp = timestamp; - rover_mecanum_guidance_status.lookahead_distance = _pure_pursuit.getLookaheadDistance(); - rover_mecanum_guidance_status.heading_error = heading_error; - rover_mecanum_guidance_status.desired_speed = desired_velocity_magnitude; - _rover_mecanum_guidance_status_pub.publish(rover_mecanum_guidance_status); - - // Publish setpoints - rover_mecanum_setpoint_s rover_mecanum_setpoint{}; - rover_mecanum_setpoint.timestamp = timestamp; - rover_mecanum_setpoint.forward_speed_setpoint = desired_velocity(0); - rover_mecanum_setpoint.forward_speed_setpoint_normalized = NAN; - rover_mecanum_setpoint.lateral_speed_setpoint = desired_velocity(1); - rover_mecanum_setpoint.lateral_speed_setpoint_normalized = NAN; - rover_mecanum_setpoint.yaw_rate_setpoint = NAN; - rover_mecanum_setpoint.speed_diff_setpoint_normalized = NAN; - rover_mecanum_setpoint.yaw_setpoint = _desired_yaw; - _rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint); -} - -void RoverMecanumGuidance::updateSubscriptions() -{ - if (_vehicle_global_position_sub.updated()) { - vehicle_global_position_s vehicle_global_position{}; - _vehicle_global_position_sub.copy(&vehicle_global_position); - _curr_pos = Vector2d(vehicle_global_position.lat, vehicle_global_position.lon); - } - - if (_local_position_sub.updated()) { - vehicle_local_position_s local_position{}; - _local_position_sub.copy(&local_position); - - if (!_global_ned_proj_ref.isInitialized() - || (_global_ned_proj_ref.getProjectionReferenceTimestamp() != local_position.ref_timestamp)) { - _global_ned_proj_ref.initReference(local_position.ref_lat, local_position.ref_lon, local_position.ref_timestamp); - } - - _curr_pos_ned = Vector2f(local_position.x, local_position.y); - } - - if (_position_setpoint_triplet_sub.updated()) { - updateWaypoints(); - } - - if (_mission_result_sub.updated()) { - mission_result_s mission_result{}; - _mission_result_sub.copy(&mission_result); - _mission_finished = mission_result.finished; - } - - if (_home_position_sub.updated()) { - home_position_s home_position{}; - _home_position_sub.copy(&home_position); - _home_position = Vector2d(home_position.lat, home_position.lon); - } -} - -void RoverMecanumGuidance::updateWaypoints() -{ - position_setpoint_triplet_s position_setpoint_triplet{}; - _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); - - // Global waypoint coordinates - if (position_setpoint_triplet.current.valid && PX4_ISFINITE(position_setpoint_triplet.current.lat) - && PX4_ISFINITE(position_setpoint_triplet.current.lon)) { - _curr_wp = Vector2d(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon); - - } else { - _curr_wp = Vector2d(0, 0); - } - - if (position_setpoint_triplet.previous.valid && PX4_ISFINITE(position_setpoint_triplet.previous.lat) - && PX4_ISFINITE(position_setpoint_triplet.previous.lon)) { - _prev_wp = Vector2d(position_setpoint_triplet.previous.lat, position_setpoint_triplet.previous.lon); - - } else { - _prev_wp = _curr_pos; - } - - if (position_setpoint_triplet.next.valid && PX4_ISFINITE(position_setpoint_triplet.next.lat) - && PX4_ISFINITE(position_setpoint_triplet.next.lon)) { - _next_wp = Vector2d(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon); - - } else { - _next_wp = _home_position; - } - - // NED waypoint coordinates - _curr_wp_ned = _global_ned_proj_ref.project(_curr_wp(0), _curr_wp(1)); - _prev_wp_ned = _global_ned_proj_ref.project(_prev_wp(0), _prev_wp(1)); - _next_wp_ned = _global_ned_proj_ref.project(_next_wp(0), _next_wp(1)); - - // Distances - const Vector2f curr_to_next_wp_ned = _next_wp_ned - _curr_wp_ned; - const Vector2f curr_to_prev_wp_ned = _prev_wp_ned - _curr_wp_ned; - float cosin = curr_to_prev_wp_ned.unit_or_zero() * curr_to_next_wp_ned.unit_or_zero(); - cosin = math::constrain(cosin, -1.f, 1.f); // Protect against float precision problem - _waypoint_transition_angle = acosf(cosin); - - // Waypoint cruising speed - _max_velocity_magnitude = position_setpoint_triplet.current.cruising_speed > FLT_EPSILON ? math::constrain( - position_setpoint_triplet.current.cruising_speed, 0.f, - _param_rm_max_speed.get()) : _param_rm_max_speed.get(); - - // Waypoint yaw setpoint - if (PX4_ISFINITE(position_setpoint_triplet.current.yaw)) { - _desired_yaw = position_setpoint_triplet.current.yaw; - } -} diff --git a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp deleted file mode 100644 index 0970b83646..0000000000 --- a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp +++ /dev/null @@ -1,139 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2024 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -// PX4 includes -#include -#include - -// uORB includes -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// Standard libraries -#include -#include -#include -#include -#include - -using namespace matrix; - -/** - * @brief Class for mecanum rover guidance. - */ -class RoverMecanumGuidance : public ModuleParams -{ -public: - /** - * @brief Constructor for RoverMecanumGuidance. - * @param parent The parent ModuleParams object. - */ - RoverMecanumGuidance(ModuleParams *parent); - ~RoverMecanumGuidance() = default; - - /** - * @brief Compute guidance for the vehicle. - * @param yaw The yaw orientation of the vehicle in radians. - * @param nav_state Navigation state of the rover. - */ - void computeGuidance(float yaw, int nav_state); - - void setDesiredYaw(float desired_yaw) { _desired_yaw = desired_yaw; }; - -protected: - /** - * @brief Update the parameters of the module. - */ - void updateParams() override; - -private: - /** - * @brief Update uORB subscriptions - */ - void updateSubscriptions(); - - /** - * @brief Update global/ned waypoint coordinates - */ - void updateWaypoints(); - - // uORB subscriptions - uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; - uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; - uORB::Subscription _mission_result_sub{ORB_ID(mission_result)}; - uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)}; - uORB::Subscription _home_position_sub{ORB_ID(home_position)}; - - // uORB publications - uORB::Publication _rover_mecanum_guidance_status_pub{ORB_ID(rover_mecanum_guidance_status)}; - uORB::Publication _rover_mecanum_setpoint_pub{ORB_ID(rover_mecanum_setpoint)}; - - // Variables - MapProjection _global_ned_proj_ref{}; // Transform global to ned coordinates. - PurePursuit _pure_pursuit{this}; // Pure pursuit library - bool _mission_finished{false}; - - // Waypoints - Vector2d _curr_pos{}; - Vector2f _curr_pos_ned{}; - Vector2d _prev_wp{}; - Vector2f _prev_wp_ned{}; - Vector2d _curr_wp{}; - Vector2f _curr_wp_ned{}; - Vector2d _next_wp{}; - Vector2f _next_wp_ned{}; - Vector2d _home_position{}; - float _max_velocity_magnitude{0.f}; - float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad] - float _desired_yaw{0.f}; - - // Parameters - DEFINE_PARAMETERS( - (ParamFloat) _param_rm_max_speed, - (ParamFloat) _param_nav_acc_rad, - (ParamFloat) _param_rm_max_jerk, - (ParamFloat) _param_rm_max_accel, - (ParamFloat) _param_rm_max_yaw_rate, - (ParamFloat) _param_rm_miss_vel_gain - ) -}; diff --git a/src/modules/rover_mecanum/module.yaml b/src/modules/rover_mecanum/module.yaml index 8d70ec90f8..ff58891ea1 100644 --- a/src/modules/rover_mecanum/module.yaml +++ b/src/modules/rover_mecanum/module.yaml @@ -3,47 +3,17 @@ module_name: Rover Mecanum parameters: - group: Rover Mecanum definitions: - RM_WHEEL_TRACK: - description: - short: Wheel track - long: Distance from the center of the right wheels to the center of the left wheels. - type: float - unit: m - min: 0.001 - max: 100 - increment: 0.001 - decimal: 3 - default: 0.5 - - RM_MAX_YAW_RATE: - description: - short: Maximum allowed yaw rate for the rover. - long: | - This parameter is used to cap desired yaw rates and map controller inputs to desired yaw rates in acro mode. - type: float - unit: deg/s - min: 0.01 - max: 1000 - increment: 0.01 - decimal: 2 - default: 90 - - RM_MAX_YAW_ACCEL: - description: - short: Maximum allowed yaw acceleration for the rover - long: | - This parameter is used to cap desired yaw acceleration. This is used to adjust incoming yaw rate setpoints - to a feasible yaw rate setpoint based on the physical limitation on how fast the yaw rate can change. - This leads to a smooth setpoint trajectory for the closed loop yaw rate controller to track. - Set to -1 to disable. - type: float - unit: deg/s^2 - min: -1 - max: 1000 - increment: 0.01 - decimal: 2 - default: -1 + description: + short: Wheel track + long: Distance from the center of the right wheel to the center of the left wheel. + type: float + unit: m + min: 0 + max: 100 + increment: 0.001 + decimal: 3 + default: 0 RM_MAX_THR_YAW_R: description: @@ -52,7 +22,7 @@ parameters: This parameter is used to calculate the feedforward term of the closed loop yaw rate control. The controller first calculates the required speed difference between the left and right motor to achieve the desired yaw rate. This desired speed difference is then linearly mapped to normalized motor commands. - A good starting point is twice the speed the rover drives at maximum throttle (RM_MAX_THRTL_SPD)). + A good starting point is half the speed the rover drives at maximum throttle (RD_MAX_THR_SPD)). Increase this parameter if the rover turns faster than the setpoint, and decrease if the rover turns slower. type: float unit: m/s @@ -60,149 +30,37 @@ parameters: max: 100 increment: 0.01 decimal: 2 - default: 2 - - RM_YAW_RATE_P: - description: - short: Proportional gain for the closed-loop yaw rate controller. - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 1 - - RM_YAW_RATE_I: - description: - short: Integral gain for the closed-loop yaw rate controller. - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 2 default: 0 - RM_YAW_P: + RM_MISS_SPD_GAIN: description: - short: Proportional gain for closed loop yaw controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 1 - - RM_YAW_I: - description: - short: Integral gain for closed loop yaw controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 0.1 - - RM_MAX_SPEED: - description: - short: Maximum speed the rover is allowed to drive + short: Tuning parameter for the speed reduction during waypoint transition long: | - This parameter is used cap the maximum speed the rover is allowed to drive - and to map stick inputs to desired speeds in position mode. + The waypoint transition speed is calculated as: + Transition_speed = Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN) + The normalized transition angle is the angle between the line segment from prev-curr waypoint and + curr-next waypoint interpolated from [0, 180] -> [0, 1]. + Higher value -> More speed reduction during waypoint transitions. + Set to -1 to disable any speed reduction during waypoint transition. type: float - unit: m/s - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 7 - - RM_MAX_THR_SPD: - description: - short: Speed the rover drives at maximum throttle - long: | - This parameter is used to calculate the feedforward term of the closed loop speed control which linearly - maps desired speeds to normalized motor commands [-1. 1]. - A good starting point is the observed ground speed when the rover drives at maximum throttle in manual mode. - Increase this parameter if the rover is faster than the setpoint, and decrease if the rover is slower. - type: float - unit: m/s - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 1 - - RM_SPEED_P: - description: - short: Proportional gain for speed controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 1 - - RM_SPEED_I: - description: - short: Integral gain for ground speed controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 0 - - RM_MAX_JERK: - description: - short: Maximum jerk - long: Limit for forwards acc/deceleration change. - type: float - unit: m/s^3 - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 0.5 - - RM_MAX_ACCEL: - description: - short: Maximum acceleration - long: Maximum acceleration is used to limit the acceleration of the rover - type: float - unit: m/s^2 - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 0.5 - - RM_MAX_DECEL: - description: - short: Maximum deceleration - long: | - Maximum decelaration is used to limit the deceleration of the rover. - Set to -1 to disable, causing the rover to decelerate as fast as possible. - Caution: Disabling the deceleration limit also disables the slow down effect in auto modes. - type: float - unit: m/s^2 min: -1 max: 100 increment: 0.01 decimal: 2 default: -1 - RM_MISS_VEL_GAIN: + RM_COURSE_CTL_TH: description: - short: Tuning parameter for the velocity reduction during waypoint transition + short: Threshold to update course control in manual position mode long: | - The waypoint transition speed is calculated as: - Transition_speed = Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN) - The normalized transition angle is the angle between the line segment from prev-curr WP and curr-next WP - interpolated from [0, 180] -> [0, 1]. - Higher value -> More velocity reduction during cornering. + Threshold for the angle between the active cruise direction and the cruise direction given + by the stick inputs. + This can be understood as a deadzone for the combined stick inputs for forward/backwards + and lateral speed which defines a course direction. type: float - min: 0.05 - max: 100 + unit: rad + min: 0 + max: 3.14 increment: 0.01 decimal: 2 - default: 1 + default: 0.17 From 65a80dc8e66a387618239c20ab65a8b2a498e7f1 Mon Sep 17 00:00:00 2001 From: Pernilla Date: Tue, 25 Feb 2025 20:06:16 +0100 Subject: [PATCH 56/91] VTOL: Don't overwrite attitude setpoint in Stabilized transition modes (#24406) Co-authored-by: Silvan Fuhrer --- src/modules/vtol_att_control/standard.cpp | 16 ++++++++++------ src/modules/vtol_att_control/tiltrotor.cpp | 15 +++++++++------ 2 files changed, 19 insertions(+), 12 deletions(-) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 5422a01898..3681c2b17c 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -172,11 +172,6 @@ void Standard::update_transition_state() VtolType::update_transition_state(); - const Eulerf attitude_setpoint_euler(Quatf(_v_att_sp->q_d)); - float roll_body = attitude_setpoint_euler.phi(); - float pitch_body = attitude_setpoint_euler.theta(); - float yaw_body = attitude_setpoint_euler.psi(); - // we get attitude setpoint from a multirotor flighttask if climbrate is controlled. // in any other case the fixed wing attitude controller publishes attitude setpoint from manual stick input. if (_v_control_mode->flag_control_climb_rate_enabled) { @@ -186,7 +181,6 @@ void Standard::update_transition_state() } memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); - roll_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).phi(); } else { // we need a recent incoming (fw virtual) attitude setpoint, otherwise return (means the previous setpoint stays active) @@ -198,6 +192,16 @@ void Standard::update_transition_state() _v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0]; } + + const Eulerf attitude_setpoint_euler(Quatf(_v_att_sp->q_d)); + float roll_body = attitude_setpoint_euler.phi(); + float pitch_body = attitude_setpoint_euler.theta(); + float yaw_body = attitude_setpoint_euler.psi(); + + if (_v_control_mode->flag_control_climb_rate_enabled) { + roll_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).phi(); + } + if (_vtol_mode == vtol_mode::TRANSITION_TO_FW) { if (_param_vt_psher_slew.get() <= FLT_EPSILON) { // just set the final target throttle value diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 9a05cf1034..7fb52b4688 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -203,11 +203,6 @@ void Tiltrotor::update_transition_state() const hrt_abstime now = hrt_absolute_time(); - const Eulerf attitude_setpoint_euler(Quatf(_v_att_sp->q_d)); - float roll_body = attitude_setpoint_euler.phi(); - float pitch_body = attitude_setpoint_euler.theta(); - float yaw_body = attitude_setpoint_euler.psi(); - // we get attitude setpoint from a multirotor flighttask if altitude is controlled. // in any other case the fixed wing attitude controller publishes attitude setpoint from manual stick input. if (_v_control_mode->flag_control_climb_rate_enabled) { @@ -217,7 +212,6 @@ void Tiltrotor::update_transition_state() } memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); - roll_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).phi(); _thrust_transition = -_mc_virtual_att_sp->thrust_body[2]; } else { @@ -231,6 +225,15 @@ void Tiltrotor::update_transition_state() } + const Eulerf attitude_setpoint_euler(Quatf(_v_att_sp->q_d)); + float roll_body = attitude_setpoint_euler.phi(); + float pitch_body = attitude_setpoint_euler.theta(); + float yaw_body = attitude_setpoint_euler.psi(); + + if (_v_control_mode->flag_control_climb_rate_enabled) { + roll_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).phi(); + } + if (_vtol_mode == vtol_mode::TRANSITION_FRONT_P1) { // for the first part of the transition all rotors are enabled From e6b80d8800a83c03e0a38917b33d2312a8c2a86a Mon Sep 17 00:00:00 2001 From: Ramon Roche Date: Mon, 17 Feb 2025 19:43:25 -0800 Subject: [PATCH 57/91] readme: minor cleanup --- README.md | 74 +++++++------------------------------------------------ 1 file changed, 9 insertions(+), 65 deletions(-) diff --git a/README.md b/README.md index dac25f0ea1..8353f3add6 100644 --- a/README.md +++ b/README.md @@ -17,17 +17,19 @@ PX4 is highly portable, OS-independent and supports Linux, NuttX and MacOS out o * [VTOL](https://docs.px4.io/main/en/frames_vtol/) * [Autogyro](https://docs.px4.io/main/en/frames_autogyro/) * [Rover](https://docs.px4.io/main/en/frames_rover/) - * many more experimental types (Blimps, Boats, Submarines, High altitude balloons, etc) + * many more experimental types (Blimps, Boats, Submarines, High Altitude Balloons, Spacecraft, etc) * Releases: [Downloads](https://github.com/PX4/PX4-Autopilot/releases) +## Releases + +Release notes and supporting information for PX4 releases can be found on the [Developer Guide](https://docs.px4.io/main/en/releases/). ## Building a PX4 based drone, rover, boat or robot -The [PX4 User Guide](https://docs.px4.io/main/en/) explains how to assemble [supported vehicles](https://docs.px4.io/main/en/airframes/airframe_reference.html) and fly drones with PX4. -See the [forum and chat](https://docs.px4.io/main/en/#getting-help) if you need help! +The [PX4 User Guide](https://docs.px4.io/main/en/) explains how to assemble [supported vehicles](https://docs.px4.io/main/en/airframes/airframe_reference.html) and fly drones with PX4. See the [forum and chat](https://docs.px4.io/main/en/#getting-help) if you need help! -## Changing code and contributing +## Changing Code and Contributing This [Developer Guide](https://docs.px4.io/main/en/development/development.html) is for software developers who want to modify the flight stack and middleware (e.g. to add new flight modes), hardware integrators who want to support new flight controller boards and peripherals, and anyone who wants to get PX4 working on a new (unsupported) airframe/vehicle. @@ -35,7 +37,7 @@ Developers should read the [Guide for Contributions](https://docs.px4.io/main/en See the [forum and chat](https://docs.px4.io/main/en/#getting-help) if you need help! -### Weekly Dev Call +## Weekly Dev Call The PX4 Dev Team syncs up on a [weekly dev call](https://docs.px4.io/main/en/contribute/). @@ -50,69 +52,11 @@ For the latest stats on contributors please see the latest stats for the Droneco ## Supported Hardware -Pixhawk standard boards and proprietary boards are shown below (discontinued boards aren't listed). - -For the most up to date information, please visit [PX4 user Guide > Autopilot Hardware](https://docs.px4.io/main/en/flight_controller/). - -### Pixhawk Standard Boards - -These boards fully comply with Pixhawk Standard, and are maintained by the PX4-Autopilot maintainers and Dronecode team - -* FMUv6X and FMUv6C - * [CUAV Pixahwk V6X (FMUv6X)](https://docs.px4.io/main/en/flight_controller/cuav_pixhawk_v6x.html) - * [Holybro Pixhawk 6X (FMUv6X)](https://docs.px4.io/main/en/flight_controller/pixhawk6x.html) - * [Holybro Pixhawk 6C (FMUv6C)](https://docs.px4.io/main/en/flight_controller/pixhawk6c.html) - * [Holybro Pix32 v6 (FMUv6C)](https://docs.px4.io/main/en/flight_controller/holybro_pix32_v6.html) -* FMUv5 and FMUv5X (STM32F7, 2019/20) - * [Pixhawk 4 (FMUv5)](https://docs.px4.io/main/en/flight_controller/pixhawk4.html) - * [Pixhawk 4 mini (FMUv5)](https://docs.px4.io/main/en/flight_controller/pixhawk4_mini.html) - * [CUAV V5+ (FMUv5)](https://docs.px4.io/main/en/flight_controller/cuav_v5_plus.html) - * [CUAV V5 nano (FMUv5)](https://docs.px4.io/main/en/flight_controller/cuav_v5_nano.html) - * [Auterion Skynode (FMUv5X)](https://docs.auterion.com/avionics/skynode) -* FMUv4 (STM32F4, 2015) - * [Pixracer](https://docs.px4.io/main/en/flight_controller/pixracer.html) - * [Pixhawk 3 Pro](https://docs.px4.io/main/en/flight_controller/pixhawk3_pro.html) -* FMUv3 (STM32F4, 2014) - * [Pixhawk 2](https://docs.px4.io/main/en/flight_controller/pixhawk-2.html) - * [Pixhawk Mini](https://docs.px4.io/main/en/flight_controller/pixhawk_mini.html) - * [CUAV Pixhack v3](https://docs.px4.io/main/en/flight_controller/pixhack_v3.html) -* FMUv2 (STM32F4, 2013) - * [Pixhawk](https://docs.px4.io/main/en/flight_controller/pixhawk.html) - -### Manufacturer supported - -These boards are maintained to be compatible with PX4-Autopilot by the Manufacturers. - -* [ARK Electronics ARKV6X](https://docs.px4.io/main/en/flight_controller/ark_v6x.html) -* [CubePilot Cube Orange+](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_orangeplus.html) -* [CubePilot Cube Orange](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_orange.html) -* [CubePilot Cube Yellow](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_yellow.html) -* [Holybro Durandal](https://docs.px4.io/main/en/flight_controller/durandal.html) -* [Airmind MindPX V2.8](http://www.mindpx.net/assets/accessories/UserGuide_MindPX.pdf) -* [Airmind MindRacer V1.2](http://mindpx.net/assets/accessories/mindracer_user_guide_v1.2.pdf) -* [Holybro Kakute F7](https://docs.px4.io/main/en/flight_controller/kakutef7.html) - -### Community supported - -These boards don't fully comply industry standards, and thus is solely maintained by the PX4 public community members. - -### Experimental - -These boards are not maintained by PX4 team nor Manufacturer, and is not guaranteed to be compatible with up to date PX4 releases. - -* [Raspberry PI with Navio 2](https://docs.px4.io/main/en/flight_controller/raspberry_pi_navio2.html) -* [Bitcraze Crazyflie 2.0](https://docs.px4.io/main/en/complete_vehicles/crazyflie2.html) - -## Project Roadmap - -**Note: Outdated** - -A high level project roadmap is available [here](https://github.com/orgs/PX4/projects/25). +For the most up to date information, please visit [PX4 User Guide > Autopilot Hardware](https://docs.px4.io/main/en/flight_controller/). ## Project Governance The PX4 Autopilot project including all of its trademarks is hosted under [Dronecode](https://www.dronecode.org/), part of the Linux Foundation. -Dronecode Logo -Linux Foundation Logo +Dronecode Logo
 
From 0cb7b8b5255d2938aef1b46fb73acc3f83c7fc6e Mon Sep 17 00:00:00 2001 From: Eric Katzfey <53063038+katzfey@users.noreply.github.com> Date: Tue, 25 Feb 2025 18:54:43 -0800 Subject: [PATCH 58/91] voxl2: Changed from old CONFIG_BOARD_ROOTFSDIR to new CONFIG_BOARD_ROOT_PATH in Posix builds (#24392) --- Kconfig | 7 ------- boards/modalai/voxl2/default.px4board | 2 +- platforms/common/include/px4_platform_common/defines.h | 2 +- 3 files changed, 2 insertions(+), 9 deletions(-) diff --git a/Kconfig b/Kconfig index f9f2e5e8ec..20ee4a2266 100644 --- a/Kconfig +++ b/Kconfig @@ -73,13 +73,6 @@ menu "Toolchain" help relative path to the ROMFS root directory - config BOARD_ROOTFSDIR - string "Root directory" - depends on PLATFORM_POSIX - default "." - help - Configure the root directory in the file system for PX4 files - config BOARD_IO string "IO board name" default "px4_io-v2_default" diff --git a/boards/modalai/voxl2/default.px4board b/boards/modalai/voxl2/default.px4board index 20860e3fce..63023ff4b9 100644 --- a/boards/modalai/voxl2/default.px4board +++ b/boards/modalai/voxl2/default.px4board @@ -1,7 +1,7 @@ CONFIG_PLATFORM_POSIX=y CONFIG_BOARD_LINUX_TARGET=y CONFIG_BOARD_TOOLCHAIN="aarch64-linux-gnu" -CONFIG_BOARD_ROOTFSDIR="/data/px4" +CONFIG_BOARD_ROOT_PATH="/data/px4" CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_OSD_MSP_OSD=y diff --git a/platforms/common/include/px4_platform_common/defines.h b/platforms/common/include/px4_platform_common/defines.h index 3b420f95f1..eba5e944d9 100644 --- a/platforms/common/include/px4_platform_common/defines.h +++ b/platforms/common/include/px4_platform_common/defines.h @@ -97,7 +97,7 @@ __BEGIN_DECLS extern long PX4_TICKS_PER_SEC; __END_DECLS -#define PX4_ROOTFSDIR CONFIG_BOARD_ROOTFSDIR +#define PX4_ROOTFSDIR CONFIG_BOARD_ROOT_PATH // Qurt doesn't have an SD card for storage #ifndef __PX4_QURT From 2169ea561b5e109463f24a017b8218f50f562c5e Mon Sep 17 00:00:00 2001 From: Marco Hauswirth <58551738+haumarco@users.noreply.github.com> Date: Wed, 26 Feb 2025 08:41:12 +0100 Subject: [PATCH 59/91] PR: add optical flow arming check (#24375) * add optical flow arming check * removed deprecated mavlink_log_critical * change SYS_HAS_NUM_OF description, keep max sensor at 1 since multiple instances are currently not support. * restructure if/else blocks --- src/lib/systemlib/system_params.c | 12 ++++ .../HealthAndArmingChecks/CMakeLists.txt | 1 + .../HealthAndArmingChecks.hpp | 3 + .../checks/opticalFlowCheck.cpp | 69 +++++++++++++++++++ .../checks/opticalFlowCheck.hpp | 55 +++++++++++++++ 5 files changed, 140 insertions(+) create mode 100644 src/modules/commander/HealthAndArmingChecks/checks/opticalFlowCheck.cpp create mode 100644 src/modules/commander/HealthAndArmingChecks/checks/opticalFlowCheck.hpp diff --git a/src/lib/systemlib/system_params.c b/src/lib/systemlib/system_params.c index d396039f3f..644e849d9d 100644 --- a/src/lib/systemlib/system_params.c +++ b/src/lib/systemlib/system_params.c @@ -240,6 +240,18 @@ PARAM_DEFINE_INT32(SYS_HAS_NUM_ASPD, 0); */ PARAM_DEFINE_INT32(SYS_HAS_NUM_DIST, 0); +/** + * Number of optical flow sensors required to be available + * + * The preflight check will fail if fewer than this number of optical flow sensors with valid data are present. + * + * @group System + * @min 0 + * @max 1 + */ + +PARAM_DEFINE_INT32(SYS_HAS_NUM_OF, 0); + /** * Enable factory calibration mode * diff --git a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt index 7265e042b4..d62d577ca4 100644 --- a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt +++ b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt @@ -42,6 +42,7 @@ px4_add_library(health_and_arming_checks checks/batteryCheck.cpp checks/cpuResourceCheck.cpp checks/distanceSensorChecks.cpp + checks/opticalFlowCheck.cpp checks/escCheck.cpp checks/estimatorCheck.cpp checks/failureDetectorCheck.cpp diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp index d04b8d088e..d4f7136f1d 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp @@ -46,6 +46,7 @@ #include "checks/baroCheck.hpp" #include "checks/cpuResourceCheck.hpp" #include "checks/distanceSensorChecks.hpp" +#include "checks/opticalFlowCheck.hpp" #include "checks/escCheck.hpp" #include "checks/estimatorCheck.hpp" #include "checks/failureDetectorCheck.hpp" @@ -130,6 +131,7 @@ private: BaroChecks _baro_checks; CpuResourceChecks _cpu_resource_checks; DistanceSensorChecks _distance_sensor_checks; + OpticalFlowCheck _optical_flow_check; EscChecks _esc_checks; EstimatorChecks _estimator_checks; FailureDetectorChecks _failure_detector_checks; @@ -169,6 +171,7 @@ private: &_baro_checks, &_cpu_resource_checks, &_distance_sensor_checks, + &_optical_flow_check, &_esc_checks, &_estimator_checks, &_failure_detector_checks, diff --git a/src/modules/commander/HealthAndArmingChecks/checks/opticalFlowCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/opticalFlowCheck.cpp new file mode 100644 index 0000000000..54e7d2c78a --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/opticalFlowCheck.cpp @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "opticalFlowCheck.hpp" + +void OpticalFlowCheck::checkAndReport(const Context &context, Report &reporter) +{ + if (!_param_sys_has_num_of.get()) { + return; + } + + const bool exists = _vehicle_optical_flow_sub.advertised(); + bool valid = false; + + if (exists) { + vehicle_optical_flow_s flow_sens; + valid = _vehicle_optical_flow_sub.copy(&flow_sens) && (hrt_elapsed_time(&flow_sens.timestamp) < 1_s); + reporter.setIsPresent(health_component_t::optical_flow); + + if (!valid) { + /* EVENT + */ + reporter.healthFailure(NavModes::All, health_component_t::optical_flow, + events::ID("check_optical_flow_sensor_invalid"), + events::Log::Error, "No valid data from optical flow sensor"); + } + + } else { + /* EVENT + * @description + * + * This check can be configured via SYS_HAS_NUM_OF parameter. + * + */ + reporter.healthFailure(NavModes::All, health_component_t::optical_flow, + events::ID("check_optical_sensor_missing"), + events::Log::Error, "Optical flow sensor missing"); + } +} diff --git a/src/modules/commander/HealthAndArmingChecks/checks/opticalFlowCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/opticalFlowCheck.hpp new file mode 100644 index 0000000000..4eaa47f1f4 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/opticalFlowCheck.hpp @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "../Common.hpp" + +#include +#include + +class OpticalFlowCheck : public HealthAndArmingCheckBase +{ +public: + OpticalFlowCheck() = default; + ~OpticalFlowCheck() = default; + + void checkAndReport(const Context &context, Report &reporter) override; + +private: + uORB::Subscription _vehicle_optical_flow_sub{ORB_ID::vehicle_optical_flow}; + + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, + (ParamInt) _param_sys_has_num_of + ) +}; From 5d2814f6c91bb60896b6770637ccc2e0aa8856ec Mon Sep 17 00:00:00 2001 From: Alexander Lerach Date: Wed, 26 Feb 2025 12:48:23 +0100 Subject: [PATCH 60/91] dataman KConfig for persistent storage --- src/modules/dataman/Kconfig | 8 +++++++ src/modules/dataman/dataman.cpp | 41 ++++++++++++++++++++++++++++++++- 2 files changed, 48 insertions(+), 1 deletion(-) diff --git a/src/modules/dataman/Kconfig b/src/modules/dataman/Kconfig index e858521d86..a0e9c0852f 100644 --- a/src/modules/dataman/Kconfig +++ b/src/modules/dataman/Kconfig @@ -11,3 +11,11 @@ menuconfig USER_DATAMAN depends on BOARD_PROTECTED && MODULES_DATAMAN ---help--- Put dataman in userspace memory + + +menuconfig DATAMAN_PERSISTENT_STORAGE + bool "dataman supports persistent storage" + default y + depends on MODULES_DATAMAN + ---help--- + Dataman supports reading/writing to persistent storage diff --git a/src/modules/dataman/dataman.cpp b/src/modules/dataman/dataman.cpp index b1c1a057e1..8cefa477bc 100644 --- a/src/modules/dataman/dataman.cpp +++ b/src/modules/dataman/dataman.cpp @@ -65,12 +65,14 @@ __END_DECLS static constexpr int TASK_STACK_SIZE = 1420; +#ifdef CONFIG_DATAMAN_PERSISTENT_STORAGE /* Private File based Operations */ static ssize_t _file_write(dm_item_t item, unsigned index, const void *buf, size_t count); static ssize_t _file_read(dm_item_t item, unsigned index, void *buf, size_t count); static int _file_clear(dm_item_t item); static int _file_initialize(unsigned max_offset); static void _file_shutdown(); +#endif /* Private Ram based Operations */ static ssize_t _ram_write(dm_item_t item, unsigned index, const void *buf, size_t count); @@ -88,6 +90,7 @@ typedef struct dm_operations_t { int (*wait)(px4_sem_t *sem); } dm_operations_t; +#ifdef CONFIG_DATAMAN_PERSISTENT_STORAGE static constexpr dm_operations_t dm_file_operations = { .write = _file_write, .read = _file_read, @@ -96,6 +99,7 @@ static constexpr dm_operations_t dm_file_operations = { .shutdown = _file_shutdown, .wait = px4_sem_wait, }; +#endif static constexpr dm_operations_t dm_ram_operations = { .write = _ram_write, @@ -149,9 +153,11 @@ static uint8_t dataman_clients_count = 1; static perf_counter_t _dm_read_perf{nullptr}; static perf_counter_t _dm_write_perf{nullptr}; +#ifdef CONFIG_DATAMAN_PERSISTENT_STORAGE /* The data manager store file handle and file name */ static const char *default_device_path = PX4_STORAGEDIR "/dataman"; static char *k_data_manager_device_path = nullptr; +#endif static enum { BACKEND_NONE = 0, @@ -241,6 +247,7 @@ static ssize_t _ram_write(dm_item_t item, unsigned index, const void *buf, size_ return count; } +#ifdef CONFIG_DATAMAN_PERSISTENT_STORAGE /* write to the data manager file */ static ssize_t _file_write(dm_item_t item, unsigned index, const void *buf, size_t count) @@ -318,6 +325,7 @@ _file_write(dm_item_t item, unsigned index, const void *buf, size_t count) /* All is well... return the number of user data written */ return count - DM_SECTOR_HDR_SIZE; } +#endif /* Retrieve from the data manager RAM buffer*/ static ssize_t _ram_read(dm_item_t item, unsigned index, void *buf, size_t count) @@ -362,6 +370,7 @@ static ssize_t _ram_read(dm_item_t item, unsigned index, void *buf, size_t count return buffer[0]; } +#ifdef CONFIG_DATAMAN_PERSISTENT_STORAGE /* Retrieve from the data manager file */ static ssize_t _file_read(dm_item_t item, unsigned index, void *buf, size_t count) @@ -442,6 +451,7 @@ _file_read(dm_item_t item, unsigned index, void *buf, size_t count) /* Return the number of bytes of caller data read */ return buffer[0]; } +#endif static int _ram_clear(dm_item_t item) { @@ -475,6 +485,7 @@ static int _ram_clear(dm_item_t item) return result; } +#ifdef CONFIG_DATAMAN_PERSISTENT_STORAGE static int _file_clear(dm_item_t item) { @@ -528,7 +539,9 @@ _file_clear(dm_item_t item) fsync(dm_operations_data.file.fd); return result; } +#endif +#ifdef CONFIG_DATAMAN_PERSISTENT_STORAGE static int _file_initialize(unsigned max_offset) { @@ -594,6 +607,7 @@ _file_initialize(unsigned max_offset) return 0; } +#endif static int _ram_initialize(unsigned max_offset) @@ -614,12 +628,14 @@ _ram_initialize(unsigned max_offset) return 0; } +#ifdef CONFIG_DATAMAN_PERSISTENT_STORAGE static void _file_shutdown() { close(dm_operations_data.file.fd); dm_operations_data.running = false; } +#endif static void _ram_shutdown() @@ -633,9 +649,12 @@ task_main(int argc, char *argv[]) { /* Dataman can use disk or RAM */ switch (backend) { +#ifdef CONFIG_DATAMAN_PERSISTENT_STORAGE + case BACKEND_FILE: g_dm_ops = &dm_file_operations; break; +#endif case BACKEND_RAM: g_dm_ops = &dm_ram_operations; @@ -680,10 +699,13 @@ task_main(int argc, char *argv[]) } switch (backend) { +#ifdef CONFIG_DATAMAN_PERSISTENT_STORAGE + case BACKEND_FILE: PX4_INFO("data manager file '%s' size is %u bytes", k_data_manager_device_path, max_offset); break; +#endif case BACKEND_RAM: PX4_INFO("data manager RAM size is %u bytes", max_offset); @@ -871,7 +893,7 @@ usage() R"DESCR_STR( ### Description Module to provide persistent storage for the rest of the system in form of a simple database through a C API. -Multiple backends are supported: +Multiple backends are supported depending on the board: - a file (eg. on the SD card) - RAM (this is obviously not persistent) @@ -885,9 +907,13 @@ Reading and writing a single item is always atomic. PRINT_MODULE_USAGE_NAME("dataman", "system"); PRINT_MODULE_USAGE_COMMAND("start"); +#ifdef CONFIG_DATAMAN_PERSISTENT_STORAGE PRINT_MODULE_USAGE_PARAM_STRING('f', nullptr, "", "Storage file", true); +#endif PRINT_MODULE_USAGE_PARAM_FLAG('r', "Use RAM backend (NOT persistent)", true); +#ifdef CONFIG_DATAMAN_PERSISTENT_STORAGE PRINT_MODULE_USAGE_PARAM_COMMENT("The options -f and -r are mutually exclusive. If nothing is specified, a file 'dataman' is used"); +#endif PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } @@ -930,9 +956,14 @@ dataman_main(int argc, char *argv[]) return -1; } +#ifdef CONFIG_DATAMAN_PERSISTENT_STORAGE backend = BACKEND_FILE; k_data_manager_device_path = strdup(dmoptarg); PX4_INFO("dataman file set to: %s", k_data_manager_device_path); +#else + backend = BACKEND_RAM; + PX4_WARN("dataman does not support persistent storage. Falling back to RAM."); +#endif break; case 'r': @@ -951,16 +982,22 @@ dataman_main(int argc, char *argv[]) } if (backend == BACKEND_NONE) { +#ifdef CONFIG_DATAMAN_PERSISTENT_STORAGE backend = BACKEND_FILE; k_data_manager_device_path = strdup(default_device_path); +#else + backend = BACKEND_RAM; +#endif } start(); if (!is_running()) { PX4_ERR("dataman start failed"); +#ifdef CONFIG_DATAMAN_PERSISTENT_STORAGE free(k_data_manager_device_path); k_data_manager_device_path = nullptr; +#endif return -1; } @@ -976,8 +1013,10 @@ dataman_main(int argc, char *argv[]) if (!strcmp(argv[1], "stop")) { stop(); +#ifdef CONFIG_DATAMAN_PERSISTENT_STORAGE free(k_data_manager_device_path); k_data_manager_device_path = nullptr; +#endif } else if (!strcmp(argv[1], "status")) { status(); From f69361f742cb3b736b1e290b3c8e8cdf7bdcaf70 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 26 Feb 2025 12:37:21 +0000 Subject: [PATCH 61/91] vtol_type: remove unused variable and function for deceleration pitch integral (#24419) This functionality was moved to FlightTaskTransition and the variable was forgotten in 079b756f1b79866720c76417b8edd24547894703 --- src/modules/vtol_att_control/vtol_type.cpp | 3 --- src/modules/vtol_att_control/vtol_type.h | 3 --- 2 files changed, 6 deletions(-) diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index e7ec2d3e01..a4355cfcbc 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -100,8 +100,6 @@ void VtolType::parameters_update() void VtolType::update_mc_state() { - resetAccelToPitchPitchIntegrator(); - // copy virtual attitude setpoint to real attitude setpoint memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); @@ -113,7 +111,6 @@ void VtolType::update_mc_state() void VtolType::update_fw_state() { - resetAccelToPitchPitchIntegrator(); _last_thr_in_fw_mode = _vehicle_thrust_setpoint_virtual_fw->xyz[0]; // copy virtual attitude setpoint to real attitude setpoint diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 737746a78b..592779ecfd 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -313,8 +313,6 @@ protected: float _quadchute_ref_alt{NAN}; // altitude (AMSL) reference to compute quad-chute altitude loss condition - float _accel_to_pitch_integ = 0; - bool _quadchute_command_treated{false}; bool isFrontTransitionCompleted(); @@ -364,7 +362,6 @@ protected: private: hrt_abstime _throttle_blend_start_ts{0}; // time at which we start blending between transition throttle and fixed wing throttle - void resetAccelToPitchPitchIntegrator() { _accel_to_pitch_integ = 0.f; } bool shouldBlendThrottleAfterFrontTransition() { return _throttle_blend_start_ts != 0; }; void stopBlendingThrottleAfterFrontTransition() { _throttle_blend_start_ts = 0; } From 84134e5123d871de98a01d2d3fc45151e545ccdd Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Wed, 26 Feb 2025 11:33:43 +0100 Subject: [PATCH 62/91] clean up variable declaration --- .../HealthAndArmingChecks/checks/opticalFlowCheck.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/opticalFlowCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/opticalFlowCheck.cpp index 54e7d2c78a..c01c8cf565 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/opticalFlowCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/opticalFlowCheck.cpp @@ -40,11 +40,10 @@ void OpticalFlowCheck::checkAndReport(const Context &context, Report &reporter) } const bool exists = _vehicle_optical_flow_sub.advertised(); - bool valid = false; if (exists) { vehicle_optical_flow_s flow_sens; - valid = _vehicle_optical_flow_sub.copy(&flow_sens) && (hrt_elapsed_time(&flow_sens.timestamp) < 1_s); + const bool valid = _vehicle_optical_flow_sub.copy(&flow_sens) && (hrt_elapsed_time(&flow_sens.timestamp) < 1_s); reporter.setIsPresent(health_component_t::optical_flow); if (!valid) { From 75c0089c2637336db45d4d58b69a2624f133e13e Mon Sep 17 00:00:00 2001 From: Andrew Brahim <35986980+dirksavage88@users.noreply.github.com> Date: Wed, 26 Feb 2025 12:52:07 -0500 Subject: [PATCH 63/91] Faster than Real -Time support in GZ (#23783) * add rtf service to gzbridge Signed-off-by: dirksavage88 * physics before model spawn Signed-off-by: dirksavage88 --------- Signed-off-by: dirksavage88 --- src/modules/simulation/gz_bridge/GZBridge.cpp | 39 +++++++++++++++++++ src/modules/simulation/gz_bridge/GZBridge.hpp | 2 + 2 files changed, 41 insertions(+) diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 23684e76c1..f5d10989fa 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -70,6 +70,22 @@ int GZBridge::init() { if (!_model_sim.empty()) { + // Set Physics rtf + const char *speed_factor_str = std::getenv("PX4_SIM_SPEED_FACTOR"); + + if (speed_factor_str) { + double speed_factor = std::atof(speed_factor_str); + gz::msgs::Physics p_req; + p_req.set_max_step_size(speed_factor * 0.004); + p_req.set_real_time_factor(-1.0); + std::string world_physics = "/world/" + _world_name + "/set_physics"; + std::string physics_service{world_physics}; + + if (!callPhysicsMsgService(physics_service, p_req)) { + return PX4_ERROR; + } + } + // service call to create model gz::msgs::EntityFactory req{}; req.set_sdf_filename(_model_sim + "/model.sdf"); @@ -205,6 +221,7 @@ int GZBridge::init() return PX4_ERROR; } + // Laser Scan: optional std::string laser_scan_topic = "/world/" + _world_name + "/model/" + _model_name + "/link/link/sensor/lidar_2d_v2/scan"; @@ -943,6 +960,28 @@ bool GZBridge::callSceneInfoMsgService(const std::string &service) return true; } +bool GZBridge::callPhysicsMsgService(const std::string &service, const gz::msgs::Physics &req) +{ + bool result; + gz::msgs::Boolean rep; + + if (_node.Request(service, req, 5000, rep, result)) { + if (!result) { + PX4_ERR("Physics service call failed."); + return false; + + } else { + return true; + } + + } else { + PX4_ERR("Physics Service call timed out. Check GZ_SIM_RESOURCE_PATH is set correctly."); + return false; + } + + return true; +} + bool GZBridge::callStringMsgService(const std::string &service, const gz::msgs::StringMsg &req) { bool result; diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 27ef424956..32b445decd 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -166,6 +166,8 @@ private: */ static void rotateQuaternion(gz::math::Quaterniond &q_FRD_to_NED, const gz::math::Quaterniond q_FLU_to_ENU); + bool callPhysicsMsgService(const std::string &service, const gz::msgs::Physics &req); + // Subscriptions uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; From f23ae924dec9ab9c16a8276893337455ddbd2d39 Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Wed, 26 Feb 2025 09:22:39 -0900 Subject: [PATCH 64/91] cmake: bump min version (#24386) * cmake: bump min version to 3.16.3, which is what Ubuntu 20.04 ships with * reduce to cmake 3.10 --------- Co-authored-by: Alex Klimaj --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 55362b1425..75fc8afb67 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -99,7 +99,7 @@ # #============================================================================= -cmake_minimum_required(VERSION 3.9 FATAL_ERROR) +cmake_minimum_required(VERSION 3.10 FATAL_ERROR) set(PX4_SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}" CACHE FILEPATH "PX4 source directory" FORCE) set(PX4_BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}" CACHE FILEPATH "PX4 binary directory" FORCE) From 1ba9eeafd99139c94da846d41a421ee7749fea82 Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Wed, 26 Feb 2025 09:23:13 -0900 Subject: [PATCH 65/91] ekf2: silence output from symforce module check (#24384) Co-authored-by: Alex Klimaj --- src/modules/ekf2/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 112b920ba2..181978792c 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -38,6 +38,7 @@ execute_process( COMMAND ${PYTHON_EXECUTABLE} -m symforce.symbolic RESULT_VARIABLE PYTHON_SYMFORCE_EXIT_CODE OUTPUT_QUIET + ERROR_QUIET ) # for now only provide symforce target helper if derivation.py generation isn't default From d4918ea118d8945b173b68c11209ae6bd8e6a4ea Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Thu, 20 Feb 2025 14:30:57 -0800 Subject: [PATCH 66/91] mavlink: updated to latest --- src/modules/mavlink/mavlink | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink b/src/modules/mavlink/mavlink index 619947d8bc..8690e10164 160000 --- a/src/modules/mavlink/mavlink +++ b/src/modules/mavlink/mavlink @@ -1 +1 @@ -Subproject commit 619947d8bc29e80eecff18b0f4fecc42d9e171dd +Subproject commit 8690e10164da864d7d36a3daad5547662e7a4103 From 5fb810a5ea0bf6c8d9e0f69251a44ad8e3d4b2ce Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Thu, 20 Feb 2025 15:11:09 -0800 Subject: [PATCH 67/91] voxl_esc: Added Mavlink tunnel UART pass-through mechanism --- msg/MavlinkTunnel.msg | 3 +++ src/drivers/actuators/voxl_esc/voxl_esc.cpp | 14 ++++++++++++++ src/drivers/actuators/voxl_esc/voxl_esc.hpp | 2 ++ src/modules/mavlink/mavlink_receiver.cpp | 11 ++++++++++- src/modules/mavlink/mavlink_receiver.h | 1 + 5 files changed, 30 insertions(+), 1 deletion(-) diff --git a/msg/MavlinkTunnel.msg b/msg/MavlinkTunnel.msg index 16934a9522..140bca5a1b 100644 --- a/msg/MavlinkTunnel.msg +++ b/msg/MavlinkTunnel.msg @@ -18,3 +18,6 @@ uint8 target_system # System ID (can be 0 for broadcast, but this is discou uint8 target_component # Component ID (can be 0 for broadcast, but this is discouraged) uint8 payload_length # Length of the data transported in payload uint8[128] payload # Data itself + +# Topic aliases for known payload types +# TOPICS mavlink_tunnel esc_serial_passthru diff --git a/src/drivers/actuators/voxl_esc/voxl_esc.cpp b/src/drivers/actuators/voxl_esc/voxl_esc.cpp index 740211c4e8..67aa579eda 100644 --- a/src/drivers/actuators/voxl_esc/voxl_esc.cpp +++ b/src/drivers/actuators/voxl_esc/voxl_esc.cpp @@ -1334,6 +1334,20 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], _esc_status_pub.publish(_esc_status); + uint8_t num_writes = 0; + + while (_esc_serial_passthru_sub.updated() && (num_writes < 4)) { + mavlink_tunnel_s uart_passthru{}; + _esc_serial_passthru_sub.copy(&uart_passthru); + + if (_uart_port.write(uart_passthru.payload, uart_passthru.payload_length) != uart_passthru.payload_length) { + PX4_ERR("Failed to send mavlink tunnel data to esc"); + return false; + } + + num_writes++; + } + perf_count(_output_update_perf); return true; diff --git a/src/drivers/actuators/voxl_esc/voxl_esc.hpp b/src/drivers/actuators/voxl_esc/voxl_esc.hpp index 0fe6f9fd42..c28d1e1ab9 100644 --- a/src/drivers/actuators/voxl_esc/voxl_esc.hpp +++ b/src/drivers/actuators/voxl_esc/voxl_esc.hpp @@ -49,6 +49,7 @@ #include #include #include +#include #include @@ -212,6 +213,7 @@ private: uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _actuator_test_sub{ORB_ID(actuator_test)}; uORB::Subscription _led_update_sub{ORB_ID(led_control)}; + uORB::Subscription _esc_serial_passthru_sub{ORB_ID(esc_serial_passthru)}; uORB::Publication _outputs_debug_pub{ORB_ID(actuator_outputs_debug)}; uORB::Publication _esc_status_pub{ORB_ID(esc_status)}; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 0b06a24b76..cc20e9f355 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1976,7 +1976,16 @@ MavlinkReceiver::handle_message_tunnel(mavlink_message_t *msg) memcpy(tunnel.payload, mavlink_tunnel.payload, sizeof(tunnel.payload)); static_assert(sizeof(tunnel.payload) == sizeof(mavlink_tunnel.payload), "mavlink_tunnel.payload size mismatch"); - _mavlink_tunnel_pub.publish(tunnel); + switch (mavlink_tunnel.payload_type) { + case MAV_TUNNEL_PAYLOAD_TYPE_MODALAI_ESC_UART_PASSTHRU: + _esc_serial_passthru_pub.publish(tunnel); + break; + + default: + _mavlink_tunnel_pub.publish(tunnel); + break; + } + } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 3d617a7731..6c7b3065e2 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -307,6 +307,7 @@ private: uORB::Publication _landing_target_pose_pub{ORB_ID(landing_target_pose)}; uORB::Publication _log_message_pub{ORB_ID(log_message)}; uORB::Publication _mavlink_tunnel_pub{ORB_ID(mavlink_tunnel)}; + uORB::Publication _esc_serial_passthru_pub{ORB_ID(esc_serial_passthru)}; uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance)}; uORB::Publication _offboard_control_mode_pub{ORB_ID(offboard_control_mode)}; uORB::Publication _onboard_computer_status_pub{ORB_ID(onboard_computer_status)}; From 93b8bc15151e1d9ca9b155a1f66ba434de476ee2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 27 Feb 2025 10:05:48 +0100 Subject: [PATCH 68/91] commander: add hysteresis for avionics power low/high check We had a setup where the voltage was right at the threshold and the check toggled continuously. It still triggers immediately, and then keeps for 15 seconds --- .../HealthAndArmingChecks/checks/powerCheck.cpp | 16 ++++++++++++++-- .../HealthAndArmingChecks/checks/powerCheck.hpp | 5 ++++- 2 files changed, 18 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.cpp index 69afd1c12c..a27d17f1be 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.cpp @@ -36,6 +36,14 @@ using namespace time_literals; +PowerChecks::PowerChecks() +{ + _voltage_low_hysteresis.set_hysteresis_time_from(false, 0_s); + _voltage_low_hysteresis.set_hysteresis_time_from(true, 15_s); + _voltage_high_hysteresis.set_hysteresis_time_from(false, 0_s); + _voltage_high_hysteresis.set_hysteresis_time_from(true, 15_s); +} + void PowerChecks::checkAndReport(const Context &context, Report &reporter) { if (circuit_breaker_enabled_by_val(_param_cbrk_supply_chk.get(), CBRK_SUPPLY_CHK_KEY)) { @@ -77,7 +85,11 @@ void PowerChecks::checkAndReport(const Context &context, Report &reporter) const float low_error_threshold = 4.7f; const float high_error_threshold = 5.4f; - if (avionics_power_rail_voltage < low_error_threshold) { + const auto now = hrt_absolute_time(); + _voltage_low_hysteresis.set_state_and_update(avionics_power_rail_voltage < low_error_threshold, now); + _voltage_high_hysteresis.set_state_and_update(avionics_power_rail_voltage > high_error_threshold, now); + + if (_voltage_low_hysteresis.get_state()) { /* EVENT * @description @@ -96,7 +108,7 @@ void PowerChecks::checkAndReport(const Context &context, Report &reporter) (double)avionics_power_rail_voltage); } - } else if (avionics_power_rail_voltage > high_error_threshold) { + } else if (_voltage_high_hysteresis.get_state()) { /* EVENT * @description * Check the voltage supply to the FMU, it must be below {2:.2} Volt. diff --git a/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.hpp index b81fadaee0..76d8b9aae0 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.hpp @@ -35,13 +35,14 @@ #include "../Common.hpp" +#include #include #include class PowerChecks : public HealthAndArmingCheckBase { public: - PowerChecks() = default; + PowerChecks(); ~PowerChecks() = default; void checkAndReport(const Context &context, Report &reporter) override; @@ -49,6 +50,8 @@ public: private: uORB::Subscription _system_power_sub{ORB_ID(system_power)}; bool _overcurrent_warning_sent{false}; + systemlib::Hysteresis _voltage_low_hysteresis{false}; + systemlib::Hysteresis _voltage_high_hysteresis{false}; DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, (ParamInt) _param_cbrk_supply_chk, From ba31054992b00a5e59b73d1a1ce22b5b02efe312 Mon Sep 17 00:00:00 2001 From: Tony Cake Date: Fri, 28 Feb 2025 10:53:32 +0100 Subject: [PATCH 69/91] Add RSSI in dBm support, plus LQ, to GHST protocol (#24351) --- src/drivers/rc/ghst_rc/GhstRc.cpp | 13 +++++++------ src/drivers/rc_input/RCInput.cpp | 7 ++++--- src/lib/rc/ghst.cpp | 22 ++++++++++++++-------- src/lib/rc/ghst.hpp | 9 ++++++++- src/lib/rc/rc_tests/RCTest.cpp | 4 ++-- 5 files changed, 35 insertions(+), 20 deletions(-) diff --git a/src/drivers/rc/ghst_rc/GhstRc.cpp b/src/drivers/rc/ghst_rc/GhstRc.cpp index 1960c84ce6..1aa3c23e89 100644 --- a/src/drivers/rc/ghst_rc/GhstRc.cpp +++ b/src/drivers/rc/ghst_rc/GhstRc.cpp @@ -34,6 +34,7 @@ #include "GhstRc.hpp" #include +#include GhstRc::GhstRc(const char *device) : ModuleParams(nullptr), @@ -174,16 +175,18 @@ void GhstRc::Run() if (newBytes > 0) { uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {}; uint16_t raw_rc_count = 0; - int8_t ghst_rssi = -1; + ghstLinkStatistics_t link_stats = { .rssi_pct = -1, .rssi_dbm = NAN, .link_quality = 0 }; - if (ghst_parse(cycle_timestamp, &rcs_buf[0], newBytes, &raw_rc_values[0], &ghst_rssi, + if (ghst_parse(cycle_timestamp, &rcs_buf[0], newBytes, &raw_rc_values[0], &link_stats, &raw_rc_count, input_rc_s::RC_INPUT_MAX_CHANNELS) ) { // we have a new GHST frame. Publish it. input_rc_s input_rc{}; input_rc.timestamp_last_signal = cycle_timestamp; input_rc.channel_count = math::constrain(raw_rc_count, (uint16_t)0, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS); - input_rc.rssi = ghst_rssi; + input_rc.rssi = link_stats.rssi_pct; + input_rc.link_quality = link_stats.link_quality; + input_rc.rssi_dbm = link_stats.rssi_dbm; input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST; unsigned valid_chans = 0; @@ -200,13 +203,11 @@ void GhstRc::Run() if (valid_chans == 0) { input_rc.rssi = 0; + // can't force link quality to zero here, receiver takes care of this } input_rc.rc_lost = (valid_chans == 0); - input_rc.link_quality = -1; - input_rc.rssi_dbm = NAN; - input_rc.timestamp = hrt_absolute_time(); _input_rc_pub.publish(input_rc); perf_count(_publish_interval_perf); diff --git a/src/drivers/rc_input/RCInput.cpp b/src/drivers/rc_input/RCInput.cpp index 12cd1ab0bf..538cb9d8ed 100644 --- a/src/drivers/rc_input/RCInput.cpp +++ b/src/drivers/rc_input/RCInput.cpp @@ -762,14 +762,15 @@ void RCInput::Run() // parse new data if (newBytes > 0) { - int8_t ghst_rssi = -1; - rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &ghst_rssi, + ghstLinkStatistics_t link_stats = { .rssi_pct = -1, .rssi_dbm = NAN, .link_quality = 0 }; + + rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], newBytes, &_raw_rc_values[0], &link_stats, &_raw_rc_count, input_rc_s::RC_INPUT_MAX_CHANNELS); if (rc_updated) { // we have a new GHST frame. Publish it. _input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST; - int32_t valid_chans = fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0, ghst_rssi); + int32_t valid_chans = fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0, link_stats.rssi_pct); // ghst telemetry works on fmu-v5 // on other Pixhawk (-related) boards we cannot write to the RC UART diff --git a/src/lib/rc/ghst.cpp b/src/lib/rc/ghst.cpp index a0bce68718..306004059c 100644 --- a/src/lib/rc/ghst.cpp +++ b/src/lib/rc/ghst.cpp @@ -56,6 +56,7 @@ #include #include #include +#include // TODO: include RSSI dBm to percentage conversion for ghost receiver #include "spektrum_rssi.h" @@ -77,8 +78,8 @@ enum class ghst_parser_state_t : uint8_t { synced }; -// only RSSI frame contains value of RSSI, if it is not received, send last received RSSI -static int8_t ghst_rssi = -1; +// only RSSI frame contains value of RSSI, if it is not received, send last received RSSI/LQ +static ghstLinkStatistics_t last_link_stats = { .rssi_pct = -1, .rssi_dbm = NAN, .link_quality = 0 }; static ghst_frame_t &ghst_frame = rc_decode_buf.ghst_frame; static uint32_t current_frame_position = 0U; @@ -89,7 +90,8 @@ static uint16_t prev_rc_vals[GHST_MAX_NUM_CHANNELS]; /** * parse the current ghst_frame buffer */ -static bool ghst_parse_buffer(uint16_t *values, int8_t *rssi, uint16_t *num_values, uint16_t max_channels); +static bool ghst_parse_buffer(uint16_t *values, ghstLinkStatistics_t *link_stats, uint16_t *num_values, + uint16_t max_channels); int ghst_config(int uart_fd) { @@ -114,7 +116,7 @@ static uint16_t convert_channel_value(unsigned chan_value); bool ghst_parse(const uint64_t now, const uint8_t *frame, unsigned len, uint16_t *values, - int8_t *rssi, uint16_t *num_values, uint16_t max_channels) + ghstLinkStatistics_t *link_stats, uint16_t *num_values, uint16_t max_channels) { bool success = false; uint8_t *ghst_frame_ptr = (uint8_t *)&ghst_frame; @@ -145,7 +147,7 @@ bool ghst_parse(const uint64_t now, const uint8_t *frame, unsigned len, uint16_t len -= current_len; frame += current_len; - if (ghst_parse_buffer(values, rssi, num_values, max_channels)) { + if (ghst_parse_buffer(values, link_stats, num_values, max_channels)) { success = true; } } @@ -182,7 +184,8 @@ static uint16_t convert_channel_value(unsigned int chan_value) return converted_chan_value; } -static bool ghst_parse_buffer(uint16_t *values, int8_t *rssi, uint16_t *num_values, uint16_t max_channels) +static bool ghst_parse_buffer(uint16_t *values, ghstLinkStatistics_t *link_stats, uint16_t *num_values, + uint16_t max_channels) { uint8_t *ghst_frame_ptr = (uint8_t *)&ghst_frame; @@ -299,13 +302,16 @@ static bool ghst_parse_buffer(uint16_t *values, int8_t *rssi, uint16_t *num_valu } else if (ghst_frame.type == static_cast(ghstFrameType::frameTypeRssi)) { const ghstPayloadRssi_t *const rssiValues = (ghstPayloadRssi_t *)&ghst_frame.payload; // TODO: call function for RSSI dBm to percentage conversion for ghost receiver - ghst_rssi = spek_dbm_to_percent(static_cast(rssiValues->rssidBm)); + last_link_stats.rssi_pct = spek_dbm_to_percent(static_cast + (rssiValues->rssidBm)); // rssidBm sign inverted (90 = -90dBm) + last_link_stats.rssi_dbm = -rssiValues->rssidBm; + last_link_stats.link_quality = rssiValues->lq; // 0 - 100 } else { GHST_DEBUG("Frame type: %u", ghst_frame.type); } - *rssi = ghst_rssi; + *link_stats = last_link_stats; memcpy(prev_rc_vals, values, sizeof(uint16_t) * GHST_MAX_NUM_CHANNELS); diff --git a/src/lib/rc/ghst.hpp b/src/lib/rc/ghst.hpp index 6f5a4f4664..c8fbc407c5 100644 --- a/src/lib/rc/ghst.hpp +++ b/src/lib/rc/ghst.hpp @@ -106,6 +106,13 @@ typedef struct { int txPowerdBm: 8; // Tx power [dBm] } __attribute__((__packed__)) ghstPayloadRssi_t; +// Link statistics for internal transport +typedef struct { + int8_t rssi_pct; + float rssi_dbm; + int8_t link_quality; +} ghstLinkStatistics_t; + /** * Configure an UART port to be used for GHST * @param uart_fd UART file descriptor @@ -127,7 +134,7 @@ __EXPORT int ghst_config(int uart_fd); * @return true if channels successfully decoded */ __EXPORT bool ghst_parse(const uint64_t now, const uint8_t *frame, unsigned len, uint16_t *values, - int8_t *rssi, uint16_t *num_values, uint16_t max_channels); + ghstLinkStatistics_t *link_stats, uint16_t *num_values, uint16_t max_channels); /** diff --git a/src/lib/rc/rc_tests/RCTest.cpp b/src/lib/rc/rc_tests/RCTest.cpp index 1997c19e85..4612673b92 100644 --- a/src/lib/rc/rc_tests/RCTest.cpp +++ b/src/lib/rc/rc_tests/RCTest.cpp @@ -159,7 +159,7 @@ bool RCTest::ghstTest() uint16_t rc_values[max_channels]; uint16_t num_values = 0; int line_counter = 1; - int8_t ghst_rssi = -1; + ghstLinkStatistics_t link_stats; ghst_config(uart_fd); while (fgets(line, line_size, fp) != nullptr) { @@ -186,7 +186,7 @@ bool RCTest::ghstTest() // Pipe the data into the parser hrt_abstime now = hrt_absolute_time(); - bool result = ghst_parse(now, frame, frame_len, rc_values, &ghst_rssi, &num_values, max_channels); + bool result = ghst_parse(now, frame, frame_len, rc_values, &link_stats, &num_values, max_channels); if (result) { has_decoded_values = true; From 14941bc270a8f010b2e43e10e0ed8b242f28a8c1 Mon Sep 17 00:00:00 2001 From: Silvan Date: Fri, 28 Feb 2025 11:41:47 +0100 Subject: [PATCH 70/91] Commander: handle mode change rejection the same for RC and MAVLink Previously, when requesting a mode switch to Position without a valid position estimate through an RC button, the mode change to Position mode was not rejected if COM_POSCTL_NAVL was set to 1 and instead the system switched to Altitude mode. If the mode request instead came in through MAVLink it was rejected. This commit aligns the two ways of changing a flight mode. Signed-off-by: Silvan --- src/modules/commander/Commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index f0537c7c6e..4c8dce5618 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1696,7 +1696,7 @@ void Commander::executeActionRequest(const action_request_s &action_request) case action_request_s::ACTION_SWITCH_MODE: - if (!_user_mode_intention.change(action_request.mode, ModeChangeSource::User, true)) { + if (!_user_mode_intention.change(action_request.mode, ModeChangeSource::User, false)) { printRejectMode(action_request.mode); } From 1f5a9e526ca0bc80a59dd98f7b6829d26ce77c95 Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Thu, 27 Feb 2025 15:12:42 +0100 Subject: [PATCH 71/91] dont change mode_change-flag if mode was not allowed to change --- src/modules/commander/UserModeIntention.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/commander/UserModeIntention.cpp b/src/modules/commander/UserModeIntention.cpp index 02b4a7f3f7..3a766640ec 100644 --- a/src/modules/commander/UserModeIntention.cpp +++ b/src/modules/commander/UserModeIntention.cpp @@ -45,7 +45,6 @@ bool UserModeIntention::change(uint8_t user_intended_nav_state, ModeChangeSource bool force) { _ever_had_mode_change = true; - _had_mode_change = true; if (_handler) { // If a replacement mode is selected, select the internal one instead. The replacement will be selected after. From 0a9a1a1552ba5f1b6f038d43152433a710747d9f Mon Sep 17 00:00:00 2001 From: Niklas Hauser <121870655+niklaut@users.noreply.github.com> Date: Fri, 28 Feb 2025 21:09:45 +0100 Subject: [PATCH 72/91] [rcS] Only start CDC/ACM when the module is enabled (#24430) Otherwise sercon and mavlink are attempted to be started and may fail, spamming the console on boot with error messages. --- ROMFS/px4fmu_common/init.d/rcS | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index b850985fd8..9131f83e96 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -488,11 +488,14 @@ else rc_input start $RC_INPUT_ARGS # Manages USB interface - if ! cdcacm_autostart start + if param greater -s SYS_USB_AUTO -1 then - sercon - echo "Starting MAVLink on /dev/ttyACM0" - mavlink start -d /dev/ttyACM0 + if ! cdcacm_autostart start + then + sercon + echo "Starting MAVLink on /dev/ttyACM0" + mavlink start -d /dev/ttyACM0 + fi fi # From 336d055923e291e6f8f9c5f3f0fdf923027e02c9 Mon Sep 17 00:00:00 2001 From: Niklas Hauser <121870655+niklaut@users.noreply.github.com> Date: Fri, 28 Feb 2025 21:14:32 +0100 Subject: [PATCH 73/91] Robustify Ulanding Radar (#24333) * [ulanding_radar] Fix comms error perf counter usage * [ulanding_radar] Workaround for lost messages by lowering sampling rate The current implementation of the Aerotenna uLanding radar driver assumes that the UART frames are received in full. If the driver polls with 10ms this is not always the case and the driver will fail to parse the frame leading to significant packet loss. This workaround polls at 12ms which ensures that at least one entire frame is received. --- .../distance_sensor/ulanding_radar/AerotennaULanding.cpp | 7 ++++++- .../distance_sensor/ulanding_radar/AerotennaULanding.hpp | 2 +- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.cpp b/src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.cpp index 6558aef348..ffd2c81a7a 100644 --- a/src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.cpp +++ b/src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.cpp @@ -125,10 +125,15 @@ int AerotennaULanding::collect() index--; } + + } else { + return -EAGAIN; } if (!checksum_passed) { - return -EAGAIN; + perf_count(_comms_errors); + perf_end(_sample_perf); + return -EBADMSG; } _px4_rangefinder.update(timestamp_sample, distance_m); diff --git a/src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.hpp b/src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.hpp index dc133254bf..98d1c0ab90 100644 --- a/src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.hpp +++ b/src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.hpp @@ -56,7 +56,7 @@ using namespace time_literals; -#define ULANDING_MEASURE_INTERVAL 10_ms +#define ULANDING_MEASURE_INTERVAL 12_ms #define ULANDING_MAX_DISTANCE 50.0f #define ULANDING_MIN_DISTANCE 0.315f #define ULANDING_VERSION 1 From b5f37c9fa6bf1a6786ff390214b12c978d9f52e6 Mon Sep 17 00:00:00 2001 From: Sebastian Domoszlai Date: Fri, 28 Feb 2025 21:42:40 +0100 Subject: [PATCH 74/91] Simplify Battery-related Enum Naming (#24265) * Simplify battery-related enum naming * Fix mistakenly removed string in enum names * Fix missing renamings * Update outdated file * msg: Increase battery_status version since the enum naming was changed * Revert message version increase --------- Co-authored-by: Matthias Grob --- .../bitcraze/crazyflie/syslink/syslink_main.h | 2 +- msg/versioned/BatteryStatus.msg | 44 +++++++++---------- src/drivers/actuators/voxl_esc/voxl_esc.cpp | 4 +- src/drivers/batt_smbus/batt_smbus.cpp | 10 ++--- src/drivers/power_monitor/ina220/ina220.cpp | 2 +- src/drivers/power_monitor/ina226/ina226.cpp | 2 +- src/drivers/power_monitor/ina228/ina228.cpp | 2 +- src/drivers/power_monitor/ina238/ina238.cpp | 2 +- src/drivers/power_monitor/voxlpm/voxlpm.cpp | 2 +- src/drivers/smart_battery/batmon/batmon.cpp | 10 ++--- src/drivers/uavcan/sensors/battery.cpp | 14 +++--- src/drivers/uavcan/sensors/battery.hpp | 8 ++-- src/lib/battery/battery.cpp | 10 ++--- src/lib/battery/battery.h | 2 +- src/modules/battery_status/battery_status.cpp | 4 +- src/modules/commander/Commander.cpp | 10 ++--- src/modules/commander/Commander.hpp | 2 +- .../checks/batteryCheck.cpp | 22 +++++----- src/modules/commander/failsafe/failsafe.cpp | 20 ++++----- src/modules/esc_battery/EscBattery.cpp | 2 +- src/modules/events/set_leds.cpp | 4 +- src/modules/mavlink/mavlink_receiver.cpp | 6 +-- .../mavlink/streams/BATTERY_STATUS.hpp | 14 +++--- src/modules/mavlink/streams/HIGH_LATENCY2.hpp | 2 +- .../battery_simulator/BatterySimulator.cpp | 2 +- 25 files changed, 101 insertions(+), 101 deletions(-) diff --git a/boards/bitcraze/crazyflie/syslink/syslink_main.h b/boards/bitcraze/crazyflie/syslink/syslink_main.h index a54859508d..8bf97b1c6e 100644 --- a/boards/bitcraze/crazyflie/syslink/syslink_main.h +++ b/boards/bitcraze/crazyflie/syslink/syslink_main.h @@ -139,7 +139,7 @@ private: // nrf chip schedules battery updates with SYSLINK_SEND_PERIOD_MS static constexpr uint32_t SYSLINK_BATTERY_STATUS_INTERVAL_US = 10_ms; - Battery _battery{1, nullptr, SYSLINK_BATTERY_STATUS_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE}; + Battery _battery{1, nullptr, SYSLINK_BATTERY_STATUS_INTERVAL_US, battery_status_s::SOURCE_POWER_MODULE}; int32_t _rssi; battery_state _bstate; diff --git a/msg/versioned/BatteryStatus.msg b/msg/versioned/BatteryStatus.msg index 9838786f3f..852db501df 100644 --- a/msg/versioned/BatteryStatus.msg +++ b/msg/versioned/BatteryStatus.msg @@ -12,9 +12,9 @@ float32 time_remaining_s # predicted time in seconds remaining until battery i float32 temperature # Temperature of the battery in degrees Celcius, NaN if unknown uint8 cell_count # Number of cells, 0 if unknown -uint8 BATTERY_SOURCE_POWER_MODULE = 0 -uint8 BATTERY_SOURCE_EXTERNAL = 1 -uint8 BATTERY_SOURCE_ESCS = 2 +uint8 SOURCE_POWER_MODULE = 0 +uint8 SOURCE_EXTERNAL = 1 +uint8 SOURCE_ESCS = 2 uint8 source # Battery source uint8 priority # Zero based priority is the connection on the Power Controller V1..Vn AKA BrickN-1 uint16 capacity # actual capacity of the battery @@ -34,26 +34,26 @@ bool is_powering_off # Power off event imminent indication, false if unknown bool is_required # Set if the battery is explicitly required before arming -uint8 BATTERY_WARNING_NONE = 0 # no battery low voltage warning active -uint8 BATTERY_WARNING_LOW = 1 # warning of low voltage -uint8 BATTERY_WARNING_CRITICAL = 2 # critical voltage, return / abort immediately -uint8 BATTERY_WARNING_EMERGENCY = 3 # immediate landing required -uint8 BATTERY_WARNING_FAILED = 4 # the battery has failed completely -uint8 BATTERY_STATE_UNHEALTHY = 6 # Battery is diagnosed to be defective or an error occurred, usage is discouraged / prohibited. Possible causes (faults) are listed in faults field. -uint8 BATTERY_STATE_CHARGING = 7 # Battery is charging +uint8 WARNING_NONE = 0 # no battery low voltage warning active +uint8 WARNING_LOW = 1 # warning of low voltage +uint8 WARNING_CRITICAL = 2 # critical voltage, return / abort immediately +uint8 WARNING_EMERGENCY = 3 # immediate landing required +uint8 WARNING_FAILED = 4 # the battery has failed completely +uint8 STATE_UNHEALTHY = 6 # Battery is diagnosed to be defective or an error occurred, usage is discouraged / prohibited. Possible causes (faults) are listed in faults field. +uint8 STATE_CHARGING = 7 # Battery is charging -uint8 BATTERY_FAULT_DEEP_DISCHARGE = 0 # Battery has deep discharged -uint8 BATTERY_FAULT_SPIKES = 1 # Voltage spikes -uint8 BATTERY_FAULT_CELL_FAIL= 2 # One or more cells have failed -uint8 BATTERY_FAULT_OVER_CURRENT = 3 # Over-current -uint8 BATTERY_FAULT_OVER_TEMPERATURE = 4 # Over-temperature -uint8 BATTERY_FAULT_UNDER_TEMPERATURE = 5 # Under-temperature fault -uint8 BATTERY_FAULT_INCOMPATIBLE_VOLTAGE = 6 # Vehicle voltage is not compatible with this battery (batteries on same power rail should have similar voltage). -uint8 BATTERY_FAULT_INCOMPATIBLE_FIRMWARE = 7 # Battery firmware is not compatible with current autopilot firmware -uint8 BATTERY_FAULT_INCOMPATIBLE_MODEL = 8 # Battery model is not supported by the system -uint8 BATTERY_FAULT_HARDWARE_FAILURE = 9 # hardware problem -uint8 BATTERY_FAULT_FAILED_TO_ARM = 10 # Battery had a problem while arming -uint8 BATTERY_FAULT_COUNT = 11 # Counter - keep it as last element! +uint8 FAULT_DEEP_DISCHARGE = 0 # Battery has deep discharged +uint8 FAULT_SPIKES = 1 # Voltage spikes +uint8 FAULT_CELL_FAIL= 2 # One or more cells have failed +uint8 FAULT_OVER_CURRENT = 3 # Over-current +uint8 FAULT_OVER_TEMPERATURE = 4 # Over-temperature +uint8 FAULT_UNDER_TEMPERATURE = 5 # Under-temperature fault +uint8 FAULT_INCOMPATIBLE_VOLTAGE = 6 # Vehicle voltage is not compatible with this battery (batteries on same power rail should have similar voltage). +uint8 FAULT_INCOMPATIBLE_FIRMWARE = 7 # Battery firmware is not compatible with current autopilot firmware +uint8 FAULT_INCOMPATIBLE_MODEL = 8 # Battery model is not supported by the system +uint8 FAULT_HARDWARE_FAILURE = 9 # hardware problem +uint8 FAULT_FAILED_TO_ARM = 10 # Battery had a problem while arming +uint8 FAULT_COUNT = 11 # Counter - keep it as last element! uint16 faults # Smart battery supply status/fault flags (bitmask) for health indication. uint8 warning # Current battery warning diff --git a/src/drivers/actuators/voxl_esc/voxl_esc.cpp b/src/drivers/actuators/voxl_esc/voxl_esc.cpp index 67aa579eda..89aebcbbaf 100644 --- a/src/drivers/actuators/voxl_esc/voxl_esc.cpp +++ b/src/drivers/actuators/voxl_esc/voxl_esc.cpp @@ -47,7 +47,7 @@ VoxlEsc::VoxlEsc() : _mixing_output{"VOXL_ESC", VOXL_ESC_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}, _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")), _output_update_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": output update interval")), - _battery(1, nullptr, _battery_report_interval, battery_status_s::BATTERY_SOURCE_POWER_MODULE) + _battery(1, nullptr, _battery_report_interval, battery_status_s::SOURCE_POWER_MODULE) { _device = VOXL_ESC_DEFAULT_PORT; @@ -1636,7 +1636,7 @@ void VoxlEsc::print_params() PX4_INFO("Params: VOXL_ESC_VLOG: %" PRId32, _parameters.verbose_logging); PX4_INFO("Params: VOXL_ESC_PUB_BST: %" PRId32, _parameters.publish_battery_status); - + PX4_INFO("Params: VOXL_ESC_T_WARN: %" PRId32, _parameters.esc_warn_temp_threshold); PX4_INFO("Params: VOXL_ESC_T_OVER: %" PRId32, _parameters.esc_over_temp_threshold); diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index b9059f5445..da47a50905 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -172,19 +172,19 @@ void BATT_SMBUS::RunImpl() // Check if max lifetime voltage delta is greater than allowed. if (_lifetime_max_delta_cell_voltage > BATT_CELL_VOLTAGE_THRESHOLD_FAILED) { - new_report.warning = battery_status_s::BATTERY_WARNING_CRITICAL; + new_report.warning = battery_status_s::WARNING_CRITICAL; } else if (new_report.remaining > _low_thr) { - new_report.warning = battery_status_s::BATTERY_WARNING_NONE; + new_report.warning = battery_status_s::WARNING_NONE; } else if (new_report.remaining > _crit_thr) { - new_report.warning = battery_status_s::BATTERY_WARNING_LOW; + new_report.warning = battery_status_s::WARNING_LOW; } else if (new_report.remaining > _emergency_thr) { - new_report.warning = battery_status_s::BATTERY_WARNING_CRITICAL; + new_report.warning = battery_status_s::WARNING_CRITICAL; } else { - new_report.warning = battery_status_s::BATTERY_WARNING_EMERGENCY; + new_report.warning = battery_status_s::WARNING_EMERGENCY; } new_report.interface_error = perf_event_count(_interface->_interface_errors); diff --git a/src/drivers/power_monitor/ina220/ina220.cpp b/src/drivers/power_monitor/ina220/ina220.cpp index 305d1a61a2..c8f52bf71c 100644 --- a/src/drivers/power_monitor/ina220/ina220.cpp +++ b/src/drivers/power_monitor/ina220/ina220.cpp @@ -52,7 +52,7 @@ INA220::INA220(const I2CSPIDriverConfig &config, int battery_index) : _collection_errors(perf_alloc(PC_COUNT, "ina220_collection_err")), _measure_errors(perf_alloc(PC_COUNT, "ina220_measurement_err")), _ch_type((PM_CH_TYPE)config.custom2), - _battery(battery_index, this, INA220_SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE) + _battery(battery_index, this, INA220_SAMPLE_INTERVAL_US, battery_status_s::SOURCE_POWER_MODULE) { float fvalue = MAX_CURRENT; _max_current = fvalue; diff --git a/src/drivers/power_monitor/ina226/ina226.cpp b/src/drivers/power_monitor/ina226/ina226.cpp index 2c117caf1d..f61a7c7b79 100644 --- a/src/drivers/power_monitor/ina226/ina226.cpp +++ b/src/drivers/power_monitor/ina226/ina226.cpp @@ -49,7 +49,7 @@ INA226::INA226(const I2CSPIDriverConfig &config, int battery_index) : _comms_errors(perf_alloc(PC_COUNT, "ina226_com_err")), _collection_errors(perf_alloc(PC_COUNT, "ina226_collection_err")), _measure_errors(perf_alloc(PC_COUNT, "ina226_measurement_err")), - _battery(battery_index, this, INA226_SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE) + _battery(battery_index, this, INA226_SAMPLE_INTERVAL_US, battery_status_s::SOURCE_POWER_MODULE) { float fvalue = MAX_CURRENT; _max_current = fvalue; diff --git a/src/drivers/power_monitor/ina228/ina228.cpp b/src/drivers/power_monitor/ina228/ina228.cpp index 8c0207b3f2..6505a7d65e 100644 --- a/src/drivers/power_monitor/ina228/ina228.cpp +++ b/src/drivers/power_monitor/ina228/ina228.cpp @@ -49,7 +49,7 @@ INA228::INA228(const I2CSPIDriverConfig &config, int battery_index) : _comms_errors(perf_alloc(PC_COUNT, "ina228_com_err")), _collection_errors(perf_alloc(PC_COUNT, "ina228_collection_err")), _measure_errors(perf_alloc(PC_COUNT, "ina228_measurement_err")), - _battery(battery_index, this, INA228_SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE) + _battery(battery_index, this, INA228_SAMPLE_INTERVAL_US, battery_status_s::SOURCE_POWER_MODULE) { float fvalue = MAX_CURRENT; _max_current = fvalue; diff --git a/src/drivers/power_monitor/ina238/ina238.cpp b/src/drivers/power_monitor/ina238/ina238.cpp index b46b277a84..ff3f193385 100644 --- a/src/drivers/power_monitor/ina238/ina238.cpp +++ b/src/drivers/power_monitor/ina238/ina238.cpp @@ -45,7 +45,7 @@ INA238::INA238(const I2CSPIDriverConfig &config, int battery_index) : _sample_perf(perf_alloc(PC_ELAPSED, "ina238_read")), _comms_errors(perf_alloc(PC_COUNT, "ina238_com_err")), _collection_errors(perf_alloc(PC_COUNT, "ina238_collection_err")), - _battery(battery_index, this, INA238_SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE) + _battery(battery_index, this, INA238_SAMPLE_INTERVAL_US, battery_status_s::SOURCE_POWER_MODULE) { float fvalue = DEFAULT_MAX_CURRENT; _max_current = fvalue; diff --git a/src/drivers/power_monitor/voxlpm/voxlpm.cpp b/src/drivers/power_monitor/voxlpm/voxlpm.cpp index 5fa109d57d..51a496bd6d 100644 --- a/src/drivers/power_monitor/voxlpm/voxlpm.cpp +++ b/src/drivers/power_monitor/voxlpm/voxlpm.cpp @@ -54,7 +54,7 @@ VOXLPM::VOXLPM(const I2CSPIDriverConfig &config) : _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sample")), _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms_errors")), _ch_type((VOXLPM_CH_TYPE)config.custom1), - _battery(1, this, _meas_interval_us, battery_status_s::BATTERY_SOURCE_POWER_MODULE) + _battery(1, this, _meas_interval_us, battery_status_s::SOURCE_POWER_MODULE) { } diff --git a/src/drivers/smart_battery/batmon/batmon.cpp b/src/drivers/smart_battery/batmon/batmon.cpp index 9061a6d3e4..ed424c1e18 100644 --- a/src/drivers/smart_battery/batmon/batmon.cpp +++ b/src/drivers/smart_battery/batmon/batmon.cpp @@ -199,19 +199,19 @@ void Batmon::RunImpl() // TODO: This critical setting should be set with BMS info or through a paramter // Setting a hard coded BATT_CELL_VOLTAGE_THRESHOLD_FAILED may not be appropriate //if (_lifetime_max_delta_cell_voltage > BATT_CELL_VOLTAGE_THRESHOLD_FAILED) { - // new_report.warning = battery_status_s::BATTERY_WARNING_CRITICAL; + // new_report.warning = battery_status_s::WARNING_CRITICAL; if (new_report.remaining > _low_thr) { - new_report.warning = battery_status_s::BATTERY_WARNING_NONE; + new_report.warning = battery_status_s::WARNING_NONE; } else if (new_report.remaining > _crit_thr) { - new_report.warning = battery_status_s::BATTERY_WARNING_LOW; + new_report.warning = battery_status_s::WARNING_LOW; } else if (new_report.remaining > _emergency_thr) { - new_report.warning = battery_status_s::BATTERY_WARNING_CRITICAL; + new_report.warning = battery_status_s::WARNING_CRITICAL; } else { - new_report.warning = battery_status_s::BATTERY_WARNING_EMERGENCY; + new_report.warning = battery_status_s::WARNING_EMERGENCY; } new_report.interface_error = perf_event_count(_interface->_interface_errors); diff --git a/src/drivers/uavcan/sensors/battery.cpp b/src/drivers/uavcan/sensors/battery.cpp index 77698cff16..0c8528670f 100644 --- a/src/drivers/uavcan/sensors/battery.cpp +++ b/src/drivers/uavcan/sensors/battery.cpp @@ -44,7 +44,7 @@ UavcanBatteryBridge::UavcanBatteryBridge(uavcan::INode &node) : ModuleParams(nullptr), _sub_battery(node), _sub_battery_aux(node), - _warning(battery_status_s::BATTERY_WARNING_NONE), + _warning(battery_status_s::WARNING_NONE), _last_timestamp(0) { } @@ -215,14 +215,14 @@ void UavcanBatteryBridge::determineWarning(float remaining) { // propagate warning state only if the state is higher, otherwise remain in current warning state - if (remaining < _param_bat_emergen_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_EMERGENCY)) { - _warning = battery_status_s::BATTERY_WARNING_EMERGENCY; + if (remaining < _param_bat_emergen_thr.get() || (_warning == battery_status_s::WARNING_EMERGENCY)) { + _warning = battery_status_s::WARNING_EMERGENCY; - } else if (remaining < _param_bat_crit_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) { - _warning = battery_status_s::BATTERY_WARNING_CRITICAL; + } else if (remaining < _param_bat_crit_thr.get() || (_warning == battery_status_s::WARNING_CRITICAL)) { + _warning = battery_status_s::WARNING_CRITICAL; - } else if (remaining < _param_bat_low_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_LOW)) { - _warning = battery_status_s::BATTERY_WARNING_LOW; + } else if (remaining < _param_bat_low_thr.get() || (_warning == battery_status_s::WARNING_LOW)) { + _warning = battery_status_s::WARNING_LOW; } } diff --git a/src/drivers/uavcan/sensors/battery.hpp b/src/drivers/uavcan/sensors/battery.hpp index ceb6222da0..a27ad64505 100644 --- a/src/drivers/uavcan/sensors/battery.hpp +++ b/src/drivers/uavcan/sensors/battery.hpp @@ -107,10 +107,10 @@ private: static_assert(battery_status_s::MAX_INSTANCES <= BATTERY_INDEX_4, "Battery array too big"); - Battery battery1 = {BATTERY_INDEX_1, this, SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_EXTERNAL}; - Battery battery2 = {BATTERY_INDEX_2, this, SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_EXTERNAL}; - Battery battery3 = {BATTERY_INDEX_3, this, SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_EXTERNAL}; - Battery battery4 = {BATTERY_INDEX_4, this, SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_EXTERNAL}; + Battery battery1 = {BATTERY_INDEX_1, this, SAMPLE_INTERVAL_US, battery_status_s::SOURCE_EXTERNAL}; + Battery battery2 = {BATTERY_INDEX_2, this, SAMPLE_INTERVAL_US, battery_status_s::SOURCE_EXTERNAL}; + Battery battery3 = {BATTERY_INDEX_3, this, SAMPLE_INTERVAL_US, battery_status_s::SOURCE_EXTERNAL}; + Battery battery4 = {BATTERY_INDEX_4, this, SAMPLE_INTERVAL_US, battery_status_s::SOURCE_EXTERNAL}; Battery *_battery[battery_status_s::MAX_INSTANCES] = { &battery1, &battery2, &battery3, &battery4 }; }; diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index 21421d592d..10dbb93a68 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -307,16 +307,16 @@ void Battery::estimateStateOfCharge() uint8_t Battery::determineWarning(float state_of_charge) { if (state_of_charge < _params.emergen_thr) { - return battery_status_s::BATTERY_WARNING_EMERGENCY; + return battery_status_s::WARNING_EMERGENCY; } else if (state_of_charge < _params.crit_thr) { - return battery_status_s::BATTERY_WARNING_CRITICAL; + return battery_status_s::WARNING_CRITICAL; } else if (state_of_charge < _params.low_thr) { - return battery_status_s::BATTERY_WARNING_LOW; + return battery_status_s::WARNING_LOW; } else { - return battery_status_s::BATTERY_WARNING_NONE; + return battery_status_s::WARNING_NONE; } } @@ -327,7 +327,7 @@ uint16_t Battery::determineFaults() if ((_params.n_cells > 0) && (_voltage_v > (_params.n_cells * _params.v_charged * 1.05f))) { // Reported as a "spike" since "over-voltage" does not exist in MAV_BATTERY_FAULT - faults |= (1 << battery_status_s::BATTERY_FAULT_SPIKES); + faults |= (1 << battery_status_s::FAULT_SPIKES); } return faults; diff --git a/src/lib/battery/battery.h b/src/lib/battery/battery.h index 64649c3c08..baf6e5ecd6 100644 --- a/src/lib/battery/battery.h +++ b/src/lib/battery/battery.h @@ -175,7 +175,7 @@ private: float _state_of_charge_volt_based{-1.f}; // [0,1] float _state_of_charge{-1.f}; // [0,1] float _scale{1.f}; - uint8_t _warning{battery_status_s::BATTERY_WARNING_NONE}; + uint8_t _warning{battery_status_s::WARNING_NONE}; hrt_abstime _last_timestamp{0}; bool _armed{false}; bool _vehicle_status_is_fw{false}; diff --git a/src/modules/battery_status/battery_status.cpp b/src/modules/battery_status/battery_status.cpp index 1d104e4087..8d92ad251b 100644 --- a/src/modules/battery_status/battery_status.cpp +++ b/src/modules/battery_status/battery_status.cpp @@ -137,9 +137,9 @@ private: BatteryStatus::BatteryStatus() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), - _battery1(1, this, SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE, 0), + _battery1(1, this, SAMPLE_INTERVAL_US, battery_status_s::SOURCE_POWER_MODULE, 0), #if BOARD_NUMBER_BRICKS > 1 - _battery2(2, this, SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE, 1), + _battery2(2, this, SAMPLE_INTERVAL_US, battery_status_s::SOURCE_POWER_MODULE, 1), #endif _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME)) { diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 4c8dce5618..9583e7341c 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2193,13 +2193,13 @@ void Commander::updateTunes() } else if (!_vehicle_status.usb_connected && (_vehicle_status.hil_state != vehicle_status_s::HIL_STATE_ON) && - (_battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) { + (_battery_warning == battery_status_s::WARNING_CRITICAL)) { /* play tune on battery critical */ set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_FAST); } else if ((_vehicle_status.hil_state != vehicle_status_s::HIL_STATE_ON) && - (_battery_warning == battery_status_s::BATTERY_WARNING_LOW)) { + (_battery_warning == battery_status_s::WARNING_LOW)) { /* play tune on battery warning */ set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_SLOW); @@ -2493,10 +2493,10 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning) if (_vehicle_status.failsafe) { led_color = led_control_s::COLOR_PURPLE; - } else if (battery_warning == battery_status_s::BATTERY_WARNING_LOW) { + } else if (battery_warning == battery_status_s::WARNING_LOW) { led_color = led_control_s::COLOR_AMBER; - } else if (battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL) { + } else if (battery_warning == battery_status_s::WARNING_CRITICAL) { led_color = led_control_s::COLOR_RED; } else { @@ -2867,7 +2867,7 @@ void Commander::battery_status_check() // Handle shutdown request from emergency battery action if (_battery_warning != _failsafe_flags.battery_warning) { - if (_failsafe_flags.battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) { + if (_failsafe_flags.battery_warning == battery_status_s::WARNING_EMERGENCY) { #if defined(BOARD_HAS_POWER_CONTROL) if (!isArmed() && (px4_shutdown_request(60_s) == 0)) { diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 395e7954cb..af16a7f81d 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -262,7 +262,7 @@ private: hrt_abstime _last_health_and_arming_check{0}; - uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE}; + uint8_t _battery_warning{battery_status_s::WARNING_NONE}; bool _failsafe_user_override_request{false}; ///< override request due to stick movements diff --git a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp index 85a19cf550..c20919ba56 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp @@ -39,7 +39,7 @@ using namespace time_literals; using battery_fault_reason_t = events::px4::enums::battery_fault_reason_t; -static_assert(battery_status_s::BATTERY_FAULT_COUNT == (static_cast(battery_fault_reason_t::_max) + 1) +static_assert(battery_status_s::FAULT_COUNT == (static_cast(battery_fault_reason_t::_max) + 1) , "Battery fault flags mismatch!"); static constexpr const char *battery_fault_reason_str(battery_fault_reason_t battery_fault_reason) @@ -78,7 +78,7 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) // Reset related failsafe flags otherwise failures from before disabling the check cause failsafes without reported reason reporter.failsafeFlags().battery_unhealthy = false; reporter.failsafeFlags().battery_low_remaining_time = false; - reporter.failsafeFlags().battery_warning = battery_status_s::BATTERY_WARNING_NONE; + reporter.failsafeFlags().battery_warning = battery_status_s::WARNING_NONE; return; } @@ -86,7 +86,7 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) // There are possibly multiple batteries, and we can't know which ones serve which purpose. So the safest // option is to check if ANY of them have a warning, and specifically find which one has the most // urgent warning. - uint8_t worst_warning = battery_status_s::BATTERY_WARNING_NONE; + uint8_t worst_warning = battery_status_s::WARNING_NONE; float worst_battery_remaining = 1.f; // To make sure that all connected batteries are being regularly reported, we check which one has the // oldest timestamp. @@ -132,7 +132,7 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) } // trigger a battery failsafe action if a battery disconnects in flight - worst_warning = battery_status_s::BATTERY_WARNING_CRITICAL; + worst_warning = battery_status_s::WARNING_CRITICAL; } if (battery.connected) { @@ -195,13 +195,13 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) reporter.failsafeFlags().battery_warning = worst_warning; } - const bool battery_warning = reporter.failsafeFlags().battery_warning > battery_status_s::BATTERY_WARNING_NONE - && reporter.failsafeFlags().battery_warning < battery_status_s::BATTERY_WARNING_FAILED; + const bool battery_warning = reporter.failsafeFlags().battery_warning > battery_status_s::WARNING_NONE + && reporter.failsafeFlags().battery_warning < battery_status_s::WARNING_FAILED; const bool configured_arm_threshold_in_use = !context.isArmed() && (_param_com_arm_bat_min.get() >= -FLT_EPSILON); const bool below_configured_arm_threshold = (worst_battery_remaining < _param_com_arm_bat_min.get()); if (battery_warning || (configured_arm_threshold_in_use && below_configured_arm_threshold)) { - const bool critical_or_higher = reporter.failsafeFlags().battery_warning >= battery_status_s::BATTERY_WARNING_CRITICAL; + const bool critical_or_higher = reporter.failsafeFlags().battery_warning >= battery_status_s::WARNING_CRITICAL; NavModes affected_modes = (!configured_arm_threshold_in_use && critical_or_higher) || (configured_arm_threshold_in_use && below_configured_arm_threshold) ? NavModes::All : NavModes::None; events::LogLevel log_level = critical_or_higher || below_configured_arm_threshold @@ -209,7 +209,7 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) switch (reporter.failsafeFlags().battery_warning) { default: - case battery_status_s::BATTERY_WARNING_LOW: + case battery_status_s::WARNING_LOW: /* EVENT * @description * The lowest battery state of charge is below the low threshold. @@ -227,7 +227,7 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) break; - case battery_status_s::BATTERY_WARNING_CRITICAL: + case battery_status_s::WARNING_CRITICAL: /* EVENT * @description * The lowest battery state of charge is below the critical threshold. @@ -245,7 +245,7 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) break; - case battery_status_s::BATTERY_WARNING_EMERGENCY: + case battery_status_s::WARNING_EMERGENCY: /* EVENT * @description * The lowest battery state of charge is below the emergency threshold. @@ -275,7 +275,7 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) || is_required_battery_missing // No currently-connected batteries have any fault || battery_has_fault - || reporter.failsafeFlags().battery_warning == battery_status_s::BATTERY_WARNING_FAILED; + || reporter.failsafeFlags().battery_warning == battery_status_s::WARNING_FAILED; if (reporter.failsafeFlags().battery_unhealthy && !is_required_battery_missing && !battery_has_fault) { // missing batteries and faults are reported above already diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index 3ba1131d97..6e6aee83b2 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -192,16 +192,16 @@ FailsafeBase::ActionOptions Failsafe::fromBatteryWarningActParam(int param_value switch (battery_warning) { default: - case battery_status_s::BATTERY_WARNING_NONE: + case battery_status_s::WARNING_NONE: options.action = Action::None; break; - case battery_status_s::BATTERY_WARNING_LOW: + case battery_status_s::WARNING_LOW: options.action = Action::Warn; options.cause = Cause::BatteryLow; break; - case battery_status_s::BATTERY_WARNING_CRITICAL: + case battery_status_s::WARNING_CRITICAL: options.action = Action::Warn; options.cause = Cause::BatteryCritical; @@ -222,7 +222,7 @@ FailsafeBase::ActionOptions Failsafe::fromBatteryWarningActParam(int param_value break; - case battery_status_s::BATTERY_WARNING_EMERGENCY: + case battery_status_s::WARNING_EMERGENCY: options.action = Action::Warn; options.cause = Cause::BatteryEmergency; @@ -550,21 +550,21 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, _param_com_low_bat_act.get() : (int32_t)LowBatteryAction::Warning; switch (status_flags.battery_warning) { - case battery_status_s::BATTERY_WARNING_LOW: + case battery_status_s::WARNING_LOW: _last_state_battery_warning_low = checkFailsafe(_caller_id_battery_warning_low, _last_state_battery_warning_low, - true, fromBatteryWarningActParam(low_battery_action, battery_status_s::BATTERY_WARNING_LOW)); + true, fromBatteryWarningActParam(low_battery_action, battery_status_s::WARNING_LOW)); break; - case battery_status_s::BATTERY_WARNING_CRITICAL: + case battery_status_s::WARNING_CRITICAL: _last_state_battery_warning_critical = checkFailsafe(_caller_id_battery_warning_critical, _last_state_battery_warning_critical, - true, fromBatteryWarningActParam(low_battery_action, battery_status_s::BATTERY_WARNING_CRITICAL)); + true, fromBatteryWarningActParam(low_battery_action, battery_status_s::WARNING_CRITICAL)); break; - case battery_status_s::BATTERY_WARNING_EMERGENCY: + case battery_status_s::WARNING_EMERGENCY: _last_state_battery_warning_emergency = checkFailsafe(_caller_id_battery_warning_emergency, _last_state_battery_warning_emergency, - true, fromBatteryWarningActParam(low_battery_action, battery_status_s::BATTERY_WARNING_EMERGENCY)); + true, fromBatteryWarningActParam(low_battery_action, battery_status_s::WARNING_EMERGENCY)); break; default: diff --git a/src/modules/esc_battery/EscBattery.cpp b/src/modules/esc_battery/EscBattery.cpp index 0fb4a3efad..78d1c2c71f 100644 --- a/src/modules/esc_battery/EscBattery.cpp +++ b/src/modules/esc_battery/EscBattery.cpp @@ -40,7 +40,7 @@ using namespace time_literals; EscBattery::EscBattery() : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::lp_default), - _battery(1, this, ESC_BATTERY_INTERVAL_US, battery_status_s::BATTERY_SOURCE_ESCS) + _battery(1, this, ESC_BATTERY_INTERVAL_US, battery_status_s::SOURCE_ESCS) { } diff --git a/src/modules/events/set_leds.cpp b/src/modules/events/set_leds.cpp index 20541f6fbb..88c2ed43d0 100644 --- a/src/modules/events/set_leds.cpp +++ b/src/modules/events/set_leds.cpp @@ -101,12 +101,12 @@ void StatusDisplay::set_leds() } // handle battery warnings, once a state is reached it can not be reset - if (_battery_status_sub.get().warning == battery_status_s::BATTERY_WARNING_CRITICAL || _critical_battery) { + if (_battery_status_sub.get().warning == battery_status_s::WARNING_CRITICAL || _critical_battery) { _led_control.color = led_control_s::COLOR_RED; _led_control.mode = led_control_s::MODE_BLINK_FAST; _critical_battery = true; - } else if (_battery_status_sub.get().warning == battery_status_s::BATTERY_WARNING_LOW || _low_battery) { + } else if (_battery_status_sub.get().warning == battery_status_s::WARNING_LOW || _low_battery) { _led_control.color = led_control_s::COLOR_RED; _led_control.mode = led_control_s::MODE_FLASH; _low_battery = true; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index cc20e9f355..39b626b4d2 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1814,13 +1814,13 @@ MavlinkReceiver::handle_message_battery_status(mavlink_message_t *msg) // Set the battery warning based on remaining charge. // Note: Smallest values must come first in evaluation. if (battery_status.remaining < _param_bat_emergen_thr.get()) { - battery_status.warning = battery_status_s::BATTERY_WARNING_EMERGENCY; + battery_status.warning = battery_status_s::WARNING_EMERGENCY; } else if (battery_status.remaining < _param_bat_crit_thr.get()) { - battery_status.warning = battery_status_s::BATTERY_WARNING_CRITICAL; + battery_status.warning = battery_status_s::WARNING_CRITICAL; } else if (battery_status.remaining < _param_bat_low_thr.get()) { - battery_status.warning = battery_status_s::BATTERY_WARNING_LOW; + battery_status.warning = battery_status_s::WARNING_LOW; } _battery_pub.publish(battery_status); diff --git a/src/modules/mavlink/streams/BATTERY_STATUS.hpp b/src/modules/mavlink/streams/BATTERY_STATUS.hpp index 633807a86e..5824c534eb 100644 --- a/src/modules/mavlink/streams/BATTERY_STATUS.hpp +++ b/src/modules/mavlink/streams/BATTERY_STATUS.hpp @@ -81,31 +81,31 @@ private: math::max((int)battery_status.time_remaining_s, 1) : 0; switch (battery_status.warning) { - case (battery_status_s::BATTERY_WARNING_NONE): + case (battery_status_s::WARNING_NONE): bat_msg.charge_state = MAV_BATTERY_CHARGE_STATE_OK; break; - case (battery_status_s::BATTERY_WARNING_LOW): + case (battery_status_s::WARNING_LOW): bat_msg.charge_state = MAV_BATTERY_CHARGE_STATE_LOW; break; - case (battery_status_s::BATTERY_WARNING_CRITICAL): + case (battery_status_s::WARNING_CRITICAL): bat_msg.charge_state = MAV_BATTERY_CHARGE_STATE_CRITICAL; break; - case (battery_status_s::BATTERY_WARNING_EMERGENCY): + case (battery_status_s::WARNING_EMERGENCY): bat_msg.charge_state = MAV_BATTERY_CHARGE_STATE_EMERGENCY; break; - case (battery_status_s::BATTERY_WARNING_FAILED): + case (battery_status_s::WARNING_FAILED): bat_msg.charge_state = MAV_BATTERY_CHARGE_STATE_FAILED; break; - case (battery_status_s::BATTERY_STATE_UNHEALTHY): + case (battery_status_s::STATE_UNHEALTHY): bat_msg.charge_state = MAV_BATTERY_CHARGE_STATE_UNHEALTHY; break; - case (battery_status_s::BATTERY_STATE_CHARGING): + case (battery_status_s::STATE_CHARGING): bat_msg.charge_state = MAV_BATTERY_CHARGE_STATE_CHARGING; break; diff --git a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp index 8623950e02..8723f14131 100644 --- a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp +++ b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp @@ -285,7 +285,7 @@ private: updated = true; _batteries[i].connected = battery.connected; - if (battery.warning > battery_status_s::BATTERY_WARNING_LOW) { + if (battery.warning > battery_status_s::WARNING_LOW) { msg->failure_flags |= HL_FAILURE_FLAG_BATTERY; } } diff --git a/src/modules/simulation/battery_simulator/BatterySimulator.cpp b/src/modules/simulation/battery_simulator/BatterySimulator.cpp index 45e8fde168..92c22285bc 100644 --- a/src/modules/simulation/battery_simulator/BatterySimulator.cpp +++ b/src/modules/simulation/battery_simulator/BatterySimulator.cpp @@ -36,7 +36,7 @@ BatterySimulator::BatterySimulator() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), - _battery(1, this, BATTERY_SIMLATOR_SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE) + _battery(1, this, BATTERY_SIMLATOR_SAMPLE_INTERVAL_US, battery_status_s::SOURCE_POWER_MODULE) { } From d4509a6cd43010c6d28a45ba5bd676fd85df963c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 1 Mar 2025 14:21:09 +1300 Subject: [PATCH 75/91] flashfs32: fix result handling (#24371) We need to translate return values here, otherwise this complains being unsuccessful when it was actually ok. --- src/lib/parameters/flashparams/flashfs32.c | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/lib/parameters/flashparams/flashfs32.c b/src/lib/parameters/flashparams/flashfs32.c index 848623b261..25e97d2563 100644 --- a/src/lib/parameters/flashparams/flashfs32.c +++ b/src/lib/parameters/flashparams/flashfs32.c @@ -1128,6 +1128,11 @@ int parameter_flashfs_init(sector_descriptor_t *fconfig, uint8_t *buffer, uint16 if (pf == NULL) { // Parameters can't be found, assume sector is corrupt or empty rv = parameter_flashfs_erase(); + + // A positive return value means flash space has been erased successfully. + if (rv > 0) { + rv = 0; + } } return rv; From 2d1652f499d3e1e22ab2c470f3292885400986ac Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 17 Feb 2025 11:42:24 +0100 Subject: [PATCH 76/91] Commander: fix parachute trigger Setting "lockdown" disables the actuators. In this mode, "force_failsafe" has no effect as the actuators are disabled, so the parachute is not getting released as it requires the output to change to its failsafe value. --- src/modules/commander/Commander.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 9583e7341c..2025d0a26c 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1889,8 +1889,7 @@ void Commander::run() _actuator_armed.armed = isArmed(); _actuator_armed.prearmed = getPrearmState(); _actuator_armed.ready_to_arm = _vehicle_status.pre_flight_checks_pass || isArmed(); - _actuator_armed.lockdown = ((_vehicle_status.nav_state == _vehicle_status.NAVIGATION_STATE_TERMINATION) - || (_vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON) + _actuator_armed.lockdown = ((_vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON) || _multicopter_throw_launch.isThrowLaunchInProgress()); // _actuator_armed.manual_lockdown // action_request_s::ACTION_KILL _actuator_armed.force_failsafe = (_vehicle_status.nav_state == _vehicle_status.NAVIGATION_STATE_TERMINATION); From 3b2d74b0172ce6be931de155cb6bb07b9ee45833 Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Mon, 3 Mar 2025 11:29:21 -0900 Subject: [PATCH 77/91] gz: Refactor GZBridge and px4-rc.simulator (#24421) * disable SENS_EN_GPSSIM for all gz airframes * add GPS + noise to GZBridge * remove mutex from gz callbacks. Callbacks run synchronously after sim update step and run() loop does not share resources. * remove hrt check in callbacks * format * remove param set-default for already default params * update submodule * remove unnecessary comments * overhaul of the GZBridge and px4-rc.simulator script * remove arg * shellcheck disable * add bus/address * start gz_bridge before adjusting sim speed or camera follow --- .../init.d-posix/airframes/4001_gz_x500 | 2 - .../init.d-posix/airframes/4003_gz_rc_cessna | 2 - .../airframes/4004_gz_standard_vtol | 2 - .../init.d-posix/airframes/4006_gz_px4vision | 2 - .../airframes/4008_gz_advanced_plane | 1 - .../init.d-posix/airframes/4009_gz_r1_rover | 3 - .../init.d-posix/airframes/4011_gz_lawnmower | 3 - .../airframes/4012_gz_rover_ackermann | 3 - .../airframes/4015_gz_r1_rover_mecanum | 3 - .../airframes/4018_gz_quadtailsitter | 4 - .../init.d-posix/airframes/4020_gz_tiltrotor | 3 - .../airframes/71002_gz_spacecraft_2d | 2 - .../init.d-posix/airframes/8011_gz_omnicopter | 2 - .../init.d-posix/px4-rc.simulator | 118 +- Tools/simulation/gz | 2 +- src/modules/simulation/gz_bridge/GZBridge.cpp | 1009 ++++++----------- src/modules/simulation/gz_bridge/GZBridge.hpp | 132 +-- src/modules/simulation/gz_bridge/GZGimbal.cpp | 2 + src/modules/simulation/gz_bridge/GZGimbal.hpp | 7 +- .../gz_bridge/GZMixingInterfaceESC.cpp | 2 + .../gz_bridge/GZMixingInterfaceESC.hpp | 7 +- .../gz_bridge/GZMixingInterfaceServo.cpp | 2 + .../gz_bridge/GZMixingInterfaceServo.hpp | 7 +- .../gz_bridge/GZMixingInterfaceWheel.cpp | 2 + .../gz_bridge/GZMixingInterfaceWheel.hpp | 7 +- 25 files changed, 518 insertions(+), 811 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 b/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 index bca3feddfb..3923b44cb4 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 @@ -13,8 +13,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500} param set-default SIM_GZ_EN 1 -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 param set-default CA_AIRFRAME 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna b/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna index 04fc1acfd8..ea7b3a2f03 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna @@ -12,8 +12,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=rc_cessna} param set-default SIM_GZ_EN 1 -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 param set-default SENS_EN_ARSPDSIM 1 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol index 96bb25d69a..69b0cec559 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol @@ -13,8 +13,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=standard_vtol} param set-default SIM_GZ_EN 1 -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 param set-default SENS_EN_ARSPDSIM 1 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision b/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision index 967051b042..e988f1f9b0 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision @@ -14,8 +14,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=px4vision} param set-default SIM_GZ_EN 1 -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 # Commander Parameters diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane b/ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane index 5d5c50df33..8479f2e38c 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane @@ -11,7 +11,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=advanced_plane} param set-default SIM_GZ_EN 1 -param set-default SENS_EN_GPSSIM 1 param set-default SENS_EN_MAGSIM 1 param set-default SENS_EN_ARSPDSIM 1 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover index f05bbdcf45..5ff39b3f96 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover @@ -46,10 +46,7 @@ param set-default PP_LOOKAHD_MAX 10 param set-default PP_LOOKAHD_MIN 1 # Simulated sensors -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 -param set-default SENS_EN_ARSPDSIM 0 # Actuator mapping param set-default SIM_GZ_WH_FUNC1 101 # right wheel diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower index 0fec76b5c4..a56cf93005 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower @@ -12,10 +12,7 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=lawnmower} param set-default SIM_GZ_EN 1 # Gazebo bridge # Simulated sensors -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 -param set-default SENS_EN_ARSPDSIM 0 # We can arm and drive in manual mode when it slides and GPS check fails: param set-default COM_ARM_WO_GPS 1 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann index cd870caabb..5867b4d3e3 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann @@ -45,10 +45,7 @@ param set-default PP_LOOKAHD_MAX 10 param set-default PP_LOOKAHD_MIN 1 # Simulated sensors -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 -param set-default SENS_EN_ARSPDSIM 0 # Wheels param set-default SIM_GZ_WH_FUNC1 101 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum b/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum index 724e7613df..45f80383e8 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum @@ -45,10 +45,7 @@ param set-default PP_LOOKAHD_MAX 10 param set-default PP_LOOKAHD_MIN 1 # Simulated sensors -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 -param set-default SENS_EN_ARSPDSIM 0 # Actuator mapping param set-default SIM_GZ_WH_FUNC1 102 # right wheel front diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter b/ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter index 9856b1bf27..1af6e721bc 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter @@ -13,11 +13,7 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=quadtailsitter} param set-default SIM_GZ_EN 1 # Gazebo bridge -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 -param set-default SENS_EN_ARSPDSIM 0 - param set-default MAV_TYPE 20 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor b/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor index c798650b54..8325f67a2c 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor @@ -13,10 +13,7 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=tiltrotor} param set-default SIM_GZ_EN 1 # Gazebo bridge -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 -param set-default SENS_EN_ARSPDSIM 0 param set-default MAV_TYPE 21 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d b/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d index 480454d72b..4cc0a02e41 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d @@ -15,8 +15,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=spacecraft_2d} param set-default SIM_GZ_EN 1 -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 1 param set-default SENS_EN_MAGSIM 1 param set-default COM_ARM_CHK_ESCS 0 # We don't have ESCs param set-default FD_ESCS_EN 0 # We don't have ESCs - but maybe we need this later? diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter b/ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter index b43b61025c..0f6748edaf 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter @@ -83,8 +83,6 @@ param set-default CA_ROTOR7_AY -0.211325 param set-default CA_ROTOR7_AZ -0.57735 param set-default SIM_GZ_EN 1 -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 param set-default SIM_GZ_EC_FUNC1 101 diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator index fd0954744e..ede64dcd34 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator @@ -73,13 +73,13 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then exit 1 fi - # look for running ${gz_command} gazebo world + # Look for an already running world gz_world=$( ${gz_command} topic -l | grep -m 1 -e "^/world/.*/clock" | sed 's/\/world\///g; s/\/clock//g' ) # shellcheck disable=SC2153 if [ -z "${gz_world}" ] && [ -n "${PX4_GZ_WORLD}" ]; then - # source generated gz_env.sh for GZ_SIM_RESOURCE_PATH + # Setup gz environment variables if [ -f ./gz_env.sh ]; then . ./gz_env.sh @@ -87,62 +87,124 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then . ../gz_env.sh fi - echo "INFO [init] starting gazebo with world: ${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" - + # Start gazebo with default world + echo "INFO [init] Starting gazebo with world: ${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" ${gz_command} ${gz_sub_command} --verbose=1 -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" & if [ -z "${HEADLESS}" ]; then - # HEADLESS not set, starting gui - ${gz_command} ${gz_sub_command} -g & + echo "INFO [init] Starting gz gui" + ${gz_command} ${gz_sub_command} -g > /dev/null 2>&1 & fi - else - # Gazebo is already running, do not start the simulator, nor the GUI + # Gazebo is already running echo "INFO [init] gazebo already running world: ${gz_world}" PX4_GZ_WORLD=${gz_world} fi + else echo "INFO [init] Standalone PX4 launch, waiting for Gazebo" fi - # start gz_bridge + # Start gz_bridge - either spawn a model or connect to existing one if [ -n "${PX4_SIM_MODEL#*gz_}" ] && [ -z "${PX4_GZ_MODEL_NAME}" ]; then - # model specified, gz_bridge will spawn model + # Spawn a model + MODEL_NAME="${PX4_SIM_MODEL#*gz_}" + MODEL_NAME_INSTANCE="${MODEL_NAME}_${px4_instance}" + POSE_ARG="" if [ -n "${PX4_GZ_MODEL_POSE}" ]; then - # model pose provided: [x, y, z, roll, pitch, yaw] + pos_x=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $1}') + pos_y=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $2}') + pos_z=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $3}') + pos_x=${pos_x:-0} + pos_y=${pos_y:-0} + pos_z=${pos_z:-0} - # Clean potential input line formatting. - model_pose="$( echo "${PX4_GZ_MODEL_POSE}" | sed -e 's/^[ \t]*//; s/[ \t]*$//; s/,/ /g; s/ / /g; s/ /,/g' )" - echo "INFO [init] PX4_GZ_MODEL_POSE set, spawning at: ${model_pose}" - - else - # model pose not provided, origin will be used - - echo "WARN [init] PX4_GZ_MODEL_POSE not set, spawning at origin." - model_pose="0,0,0,0,0,0" + POSE_ARG=", pose: { position: { x: ${pos_x}, y: ${pos_y}, z: ${pos_z} } }" + echo "INFO [init] Spawning model at position: ${pos_x} ${pos_y} ${pos_z}" fi - # start gz bridge with pose arg. - if ! gz_bridge start -p "${model_pose}" -m "${PX4_SIM_MODEL#*gz_}" -w "${PX4_GZ_WORLD}" -i "${px4_instance}"; then + # Spawn model + ${gz_command} service -s "/world/${PX4_GZ_WORLD}/create" --reqtype gz.msgs.EntityFactory \ + --reptype gz.msgs.Boolean --timeout 1000 \ + --req "sdf_filename: \"${PX4_GZ_MODELS}/${MODEL_NAME}\", name: \"${MODEL_NAME_INSTANCE}\", allow_renaming: false${POSE_ARG}" > /dev/null 2>&1 + + # Start gz_bridge + if ! gz_bridge start -w "${PX4_GZ_WORLD}" -n "${MODEL_NAME_INSTANCE}"; then echo "ERROR [init] gz_bridge failed to start and spawn model" exit 1 fi - elif [ -n "${PX4_GZ_MODEL_NAME}" ]; then - # model name specificed, gz_bridge will attach to existing model + # Set physics parameters for faster-than-realtime simulation if needed + check_scene_info() { + SERVICE_INFO=$(${gz_command} service -i --service "/world/${PX4_GZ_WORLD}/scene/info" 2>&1) + if echo "$SERVICE_INFO" | grep -q "Service providers"; then + return 0 + else + return 1 + fi + } + ATTEMPTS=30 + while [ $ATTEMPTS -gt 0 ]; do + if check_scene_info; then + echo "INFO [init] Gazebo world is ready" + break + fi + ATTEMPTS=$((ATTEMPTS-1)) + if [ $ATTEMPTS -eq 0 ]; then + echo "ERROR [init] Timed out waiting for Gazebo world" + exit 1 + fi + echo "INFO [init] Waiting for Gazebo world..." + sleep 1 + done + + if [ -n "${PX4_SIM_SPEED_FACTOR}" ]; then + echo "INFO [init] Setting simulation speed factor: ${PX4_SIM_SPEED_FACTOR}" + ${gz_command} service -s "/world/${PX4_GZ_WORLD}/set_physics" --reqtype gz.msgs.Physics \ + --reptype gz.msgs.Boolean --timeout 1000 \ + --req "real_time_factor: ${PX4_SIM_SPEED_FACTOR}" > /dev/null 2>&1 + fi + + + # Set up camera to follow the model if requested + if [ -n "${PX4_GZ_FOLLOW}" ]; then + + # Wait for model to spawn + sleep 1 + + echo "INFO [init] Setting camera to follow ${MODEL_NAME_INSTANCE}" + + # Set camera to follow the model + ${gz_command} service -s "/gui/follow" --reqtype gz.msgs.StringMsg \ + --reptype gz.msgs.Boolean --timeout 5000 \ + --req "data: \"${MODEL_NAME_INSTANCE}\"" > /dev/null 2>&1 + + # Set default camera offset if not specified + follow_x=${PX4_GZ_FOLLOW_OFFSET_X:--2.0} + follow_y=${PX4_GZ_FOLLOW_OFFSET_Y:--2.0} + follow_z=${PX4_GZ_FOLLOW_OFFSET_Z:-2.0} + + # Set camera offset + ${gz_command} service -s "/gui/follow/offset" --reqtype gz.msgs.Vector3d \ + --reptype gz.msgs.Boolean --timeout 5000 \ + --req "x: ${follow_x}, y: ${follow_y}, z: ${follow_z}" > /dev/null 2>&1 + + echo "INFO [init] Camera follow offset set to ${follow_x}, ${follow_y}, ${follow_z}" + fi + + elif [ -n "${PX4_GZ_MODEL_NAME}" ]; then + # Connect to existing model echo "INFO [init] PX4_GZ_MODEL_NAME set, PX4 will attach to existing model" - if ! gz_bridge start -n "${PX4_GZ_MODEL_NAME}" -w "${PX4_GZ_WORLD}"; then + if ! gz_bridge start -w "${PX4_GZ_WORLD}" -n "${PX4_GZ_MODEL_NAME}"; then echo "ERROR [init] gz_bridge failed to start and attach to existing model" exit 1 fi - else - echo "ERROR [init] failed to pass only PX4_GZ_MODEL_NAME or PX4_SIM_MODEL" + echo "ERROR [init] failed to pass either PX4_GZ_MODEL_NAME or PX4_SIM_MODEL" exit 1 fi - # Start the sensor simulator modules if param compare -s SENS_EN_BAROSIM 1 then diff --git a/Tools/simulation/gz b/Tools/simulation/gz index 183cbee9e2..23170a9125 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 183cbee9e2250d1ca5517cd76c5d9a7e47cc0b7a +Subproject commit 23170a91255d99aea8960d1101541afce0f209d9 diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index f5d10989fa..bb2e7282da 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -1,20 +1,20 @@ /**************************************************************************** * - * Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. + * Copyright (c) 2025 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. + * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. + * used to endorse or promote products derived from this software + * without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT @@ -43,24 +43,17 @@ #include #include -GZBridge::GZBridge(const char *world, const char *name, const char *model, - const char *pose_str) : +GZBridge::GZBridge(const std::string &world, const std::string &model_name) : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), _world_name(world), - _model_name(name), - _model_sim(model), - _model_pose(pose_str) + _model_name(model_name) { - pthread_mutex_init(&_node_mutex, nullptr); - updateParams(); } GZBridge::~GZBridge() { - // TODO: unsubscribe - for (auto &sub_topic : _node.SubscribedTopics()) { _node.Unsubscribe(sub_topic); } @@ -68,126 +61,6 @@ GZBridge::~GZBridge() int GZBridge::init() { - if (!_model_sim.empty()) { - - // Set Physics rtf - const char *speed_factor_str = std::getenv("PX4_SIM_SPEED_FACTOR"); - - if (speed_factor_str) { - double speed_factor = std::atof(speed_factor_str); - gz::msgs::Physics p_req; - p_req.set_max_step_size(speed_factor * 0.004); - p_req.set_real_time_factor(-1.0); - std::string world_physics = "/world/" + _world_name + "/set_physics"; - std::string physics_service{world_physics}; - - if (!callPhysicsMsgService(physics_service, p_req)) { - return PX4_ERROR; - } - } - - // service call to create model - gz::msgs::EntityFactory req{}; - req.set_sdf_filename(_model_sim + "/model.sdf"); - - req.set_name(_model_name); // New name for the entity, overrides the name on the SDF. - - req.set_allow_renaming(false); // allowed to rename the entity in case of overlap with existing entities - - if (!_model_pose.empty()) { - PX4_INFO("Requested Model Position: %s", _model_pose.c_str()); - - std::vector model_pose_v; - - std::stringstream ss(_model_pose); - - while (ss.good()) { - std::string substr; - std::getline(ss, substr, ','); - model_pose_v.push_back(std::stod(substr)); - } - - while (model_pose_v.size() < 6) { - model_pose_v.push_back(0.0); - } - - gz::msgs::Pose *p = req.mutable_pose(); - gz::msgs::Vector3d *position = p->mutable_position(); - position->set_x(model_pose_v[0]); - position->set_y(model_pose_v[1]); - position->set_z(model_pose_v[2]); - - gz::math::Quaterniond q(model_pose_v[3], model_pose_v[4], model_pose_v[5]); - - q.Normalize(); - gz::msgs::Quaternion *orientation = p->mutable_orientation(); - orientation->set_x(q.X()); - orientation->set_y(q.Y()); - orientation->set_z(q.Z()); - orientation->set_w(q.W()); - } - - //world/$WORLD/create service. - gz::msgs::Boolean rep; - bool result; - std::string create_service = "/world/" + _world_name + "/create"; - - bool gz_called = false; - // Check if PX4_GZ_STANDALONE has been set. - char *standalone_val = std::getenv("PX4_GZ_STANDALONE"); - - if ((standalone_val != nullptr) && (std::strcmp(standalone_val, "1") == 0)) { - // Check if Gazebo has been called and if not attempt to reconnect. - while (gz_called == false) { - if (_node.Request(create_service, req, 1000, rep, result)) { - if (!rep.data() || !result) { - PX4_ERR("EntityFactory service call failed"); - return PX4_ERROR; - - } else { - gz_called = true; - } - } - - // If Gazebo has not been called, wait 2 seconds and try again. - else { - PX4_WARN("Service call timed out as Gazebo has not been detected. Retrying..."); - system_usleep(2000000); - } - } - } - - - // If PX4_GZ_STANDALONE has been set, you can try to connect but GZ_SIM_RESOURCE_PATH needs to be set correctly to work. - else { - if (!callEntityFactoryService(create_service, req)) { - return PX4_ERROR; - } - - std::string scene_info_service = "/world/" + _world_name + "/scene/info"; - bool scene_created = false; - - while (scene_created == false) { - if (!callSceneInfoMsgService(scene_info_service)) { - PX4_WARN("Service call timed out as Gazebo has not been detected. Retrying..."); - system_usleep(2000000); - - } else { - scene_created = true; - } - } - - gz::msgs::StringMsg follow_msg{}; - follow_msg.set_data(_model_name); - callStringMsgService("/gui/follow", follow_msg); - gz::msgs::Vector3d follow_offset_msg{}; - follow_offset_msg.set_x(-2.0); - follow_offset_msg.set_y(-2.0); - follow_offset_msg.set_z(2.0); - callVector3dService("/gui/follow/offset", follow_offset_msg); - } - } - // clock std::string clock_topic = "/world/" + _world_name + "/clock"; @@ -212,7 +85,6 @@ int GZBridge::init() return PX4_ERROR; } - // IMU: /world/$WORLD/model/$MODEL/link/base_link/sensor/imu_sensor/imu std::string odometry_topic = "/model/" + _model_name + "/odometry_with_covariance"; @@ -221,7 +93,6 @@ int GZBridge::init() return PX4_ERROR; } - // Laser Scan: optional std::string laser_scan_topic = "/world/" + _world_name + "/model/" + _model_name + "/link/link/sensor/lidar_2d_v2/scan"; @@ -288,295 +159,120 @@ int GZBridge::init() return OK; } -int GZBridge::task_spawn(int argc, char *argv[]) +void GZBridge::clockCallback(const gz::msgs::Clock &msg) { - const char *world_name = "default"; - const char *model_name = nullptr; - const char *model_pose = nullptr; - const char *model_sim = nullptr; - const char *px4_instance = nullptr; - std::string model_name_std; - - - bool error_flag = false; - int myoptind = 1; - int ch; - const char *myoptarg = nullptr; - - while ((ch = px4_getopt(argc, argv, "w:m:p:i:n:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'w': - // world - world_name = myoptarg; - break; - - case 'n': - // model - model_name = myoptarg; - break; - - case 'p': - // pose - model_pose = myoptarg; - break; - - case 'm': - // pose - model_sim = myoptarg; - break; - - case 'i': - // pose - px4_instance = myoptarg; - break; - - case '?': - error_flag = true; - break; - - default: - PX4_WARN("unrecognized flag"); - error_flag = true; - break; - } - } - - if (error_flag) { - return PX4_ERROR; - } - - if (!model_pose) { - model_pose = ""; - } - - if (!model_sim) { - model_sim = ""; - } - - if (!px4_instance) { - if (!model_name) { - model_name = model_sim; - } - - } else if (!model_name) { - model_name_std = std::string(model_sim) + "_" + std::string(px4_instance); - model_name = model_name_std.c_str(); - } - - PX4_INFO("world: %s, model name: %s, simulation model: %s", world_name, model_name, model_sim); - - GZBridge *instance = new GZBridge(world_name, model_name, model_sim, model_pose); - - if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; - - if (instance->init() == PX4_OK) { - -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - // lockstep scheduler wait for initial clock set before returning - int sleep_count_limit = 10000; - - while ((instance->world_time_us() == 0) && sleep_count_limit > 0) { - // wait for first clock message - system_usleep(1000); - sleep_count_limit--; - } - - if (instance->world_time_us() == 0) { - PX4_ERR("timed out waiting for clock message"); - instance->request_stop(); - instance->ScheduleNow(); - - } else { - return PX4_OK; - } - -#else - return PX4_OK; -#endif // ENABLE_LOCKSTEP_SCHEDULER - - //return PX4_OK; - } - - } else { - PX4_ERR("alloc failed"); - } - - delete instance; - _object.store(nullptr); - _task_id = -1; - - return PX4_ERROR; -} - -bool GZBridge::updateClock(const uint64_t tv_sec, const uint64_t tv_nsec) -{ -#if defined(ENABLE_LOCKSTEP_SCHEDULER) + // NOTE: PX4-SITL time needs to stay in sync with gz, so this clock-sync will happen on every callback. struct timespec ts; - ts.tv_sec = tv_sec; - ts.tv_nsec = tv_nsec; - - if (px4_clock_settime(CLOCK_MONOTONIC, &ts) == 0) { - _world_time_us.store(ts_to_abstime(&ts)); - return true; - } - -#endif // ENABLE_LOCKSTEP_SCHEDULER - - return false; + ts.tv_sec = msg.sim().sec(); + ts.tv_nsec = msg.sim().nsec(); + px4_clock_settime(CLOCK_MONOTONIC, &ts); } -void GZBridge::clockCallback(const gz::msgs::Clock &clock) +void GZBridge::barometerCallback(const gz::msgs::FluidPressure &msg) { - pthread_mutex_lock(&_node_mutex); + const uint64_t timestamp = hrt_absolute_time(); - const uint64_t time_us = (clock.sim().sec() * 1000000) + (clock.sim().nsec() / 1000); + device::Device::DeviceId id{}; + id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; + id.devid_s.devtype = DRV_BARO_DEVTYPE_BAROSIM; + id.devid_s.bus = 1; + id.devid_s.address = 1; - if (time_us > _world_time_us.load()) { - updateClock(clock.sim().sec(), clock.sim().nsec()); - } - - pthread_mutex_unlock(&_node_mutex); -} - -void GZBridge::barometerCallback(const gz::msgs::FluidPressure &air_pressure) -{ - if (hrt_absolute_time() == 0) { - return; - } - - pthread_mutex_lock(&_node_mutex); - - const uint64_t time_us = (air_pressure.header().stamp().sec() * 1000000) - + (air_pressure.header().stamp().nsec() / 1000); - - // publish - sensor_baro_s sensor_baro{}; - sensor_baro.timestamp_sample = time_us; - sensor_baro.device_id = 6620172; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION - sensor_baro.pressure = air_pressure.pressure(); - sensor_baro.temperature = this->_temperature; - sensor_baro.timestamp = hrt_absolute_time(); - _sensor_baro_pub.publish(sensor_baro); - - pthread_mutex_unlock(&_node_mutex); + sensor_baro_s report{}; + report.timestamp = timestamp; + report.timestamp_sample = timestamp; + report.device_id = id.devid; + report.pressure = msg.pressure(); + report.temperature = this->_temperature; + _sensor_baro_pub.publish(report); } -void GZBridge::airspeedCallback(const gz::msgs::AirSpeed &air_speed) +void GZBridge::airspeedCallback(const gz::msgs::AirSpeed &msg) { - if (hrt_absolute_time() == 0) { - return; - } + const uint64_t timestamp = hrt_absolute_time(); - pthread_mutex_lock(&_node_mutex); - - const uint64_t time_us = (air_speed.header().stamp().sec() * 1000000) - + (air_speed.header().stamp().nsec() / 1000); - - double air_speed_value = air_speed.diff_pressure(); + device::Device::DeviceId id{}; + id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; + id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_SIM; + id.devid_s.bus = 1; + id.devid_s.address = 1; differential_pressure_s report{}; - report.timestamp_sample = time_us; - report.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION - report.differential_pressure_pa = static_cast(air_speed_value); // hPa to Pa; - report.temperature = static_cast(air_speed.temperature()) + atmosphere::kAbsoluteNullCelsius; // K to C - report.timestamp = hrt_absolute_time();; + report.timestamp = timestamp; + report.timestamp_sample = timestamp; + report.device_id = id.devid; + report.differential_pressure_pa = msg.diff_pressure(); // hPa to Pa; + report.temperature = static_cast(msg.temperature()) + atmosphere::kAbsoluteNullCelsius; // K to C _differential_pressure_pub.publish(report); this->_temperature = report.temperature; - - pthread_mutex_unlock(&_node_mutex); } -void GZBridge::imuCallback(const gz::msgs::IMU &imu) +void GZBridge::imuCallback(const gz::msgs::IMU &msg) { - if (hrt_absolute_time() == 0) { - return; - } - pthread_mutex_lock(&_node_mutex); - - const uint64_t time_us = (imu.header().stamp().sec() * 1000000) + (imu.header().stamp().nsec() / 1000); - - if (time_us > _world_time_us.load()) { - updateClock(imu.header().stamp().sec(), imu.header().stamp().nsec()); - } + const uint64_t timestamp = hrt_absolute_time(); // FLU -> FRD static const auto q_FLU_to_FRD = gz::math::Quaterniond(0, 1, 0, 0); gz::math::Vector3d accel_b = q_FLU_to_FRD.RotateVector(gz::math::Vector3d( - imu.linear_acceleration().x(), - imu.linear_acceleration().y(), - imu.linear_acceleration().z())); + msg.linear_acceleration().x(), + msg.linear_acceleration().y(), + msg.linear_acceleration().z())); + + device::Device::DeviceId id{}; + id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; + id.devid_s.devtype = DRV_IMU_DEVTYPE_SIM; + id.devid_s.bus = 1; + id.devid_s.address = 1; // publish accel - sensor_accel_s sensor_accel{}; -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - sensor_accel.timestamp_sample = time_us; - sensor_accel.timestamp = time_us; -#else - sensor_accel.timestamp_sample = hrt_absolute_time(); - sensor_accel.timestamp = hrt_absolute_time(); -#endif - sensor_accel.device_id = 1310988; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION - sensor_accel.x = accel_b.X(); - sensor_accel.y = accel_b.Y(); - sensor_accel.z = accel_b.Z(); - sensor_accel.temperature = NAN; - sensor_accel.samples = 1; - _sensor_accel_pub.publish(sensor_accel); + sensor_accel_s accel{}; + + accel.timestamp_sample = timestamp; + accel.timestamp = timestamp; + accel.device_id = id.devid; + + accel.x = accel_b.X(); + accel.y = accel_b.Y(); + accel.z = accel_b.Z(); + accel.temperature = NAN; + accel.samples = 1; + _sensor_accel_pub.publish(accel); gz::math::Vector3d gyro_b = q_FLU_to_FRD.RotateVector(gz::math::Vector3d( - imu.angular_velocity().x(), - imu.angular_velocity().y(), - imu.angular_velocity().z())); + msg.angular_velocity().x(), + msg.angular_velocity().y(), + msg.angular_velocity().z())); // publish gyro - sensor_gyro_s sensor_gyro{}; -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - sensor_gyro.timestamp_sample = time_us; - sensor_gyro.timestamp = time_us; -#else - sensor_gyro.timestamp_sample = hrt_absolute_time(); - sensor_gyro.timestamp = hrt_absolute_time(); -#endif - sensor_gyro.device_id = 1310988; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION - sensor_gyro.x = gyro_b.X(); - sensor_gyro.y = gyro_b.Y(); - sensor_gyro.z = gyro_b.Z(); - sensor_gyro.temperature = NAN; - sensor_gyro.samples = 1; - _sensor_gyro_pub.publish(sensor_gyro); - - pthread_mutex_unlock(&_node_mutex); + sensor_gyro_s gyro{}; + gyro.timestamp_sample = timestamp; + gyro.timestamp = timestamp; + gyro.device_id = id.devid; + gyro.x = gyro_b.X(); + gyro.y = gyro_b.Y(); + gyro.z = gyro_b.Z(); + gyro.temperature = NAN; + gyro.samples = 1; + _sensor_gyro_pub.publish(gyro); } -void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose) +void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &msg) { - if (hrt_absolute_time() == 0) { - return; - } + const uint64_t timestamp = hrt_absolute_time(); - pthread_mutex_lock(&_node_mutex); + for (int p = 0; p < msg.pose_size(); p++) { + if (msg.pose(p).name() == _model_name) { - for (int p = 0; p < pose.pose_size(); p++) { - if (pose.pose(p).name() == _model_name) { + const double dt = math::constrain((timestamp - _timestamp_prev) * 1e-6, 0.001, 0.1); + _timestamp_prev = timestamp; - const uint64_t time_us = (pose.header().stamp().sec() * 1000000) + (pose.header().stamp().nsec() / 1000); - - if (time_us > _world_time_us.load()) { - updateClock(pose.header().stamp().sec(), pose.header().stamp().nsec()); - } - - const double dt = math::constrain((time_us - _timestamp_prev) * 1e-6, 0.001, 0.1); - _timestamp_prev = time_us; - - gz::msgs::Vector3d pose_position = pose.pose(p).position(); - gz::msgs::Quaternion pose_orientation = pose.pose(p).orientation(); + gz::msgs::Vector3d pose_position = msg.pose(p).position(); + gz::msgs::Quaternion pose_orientation = msg.pose(p).orientation(); // ground truth gz::math::Quaterniond q_gr = gz::math::Quaterniond( @@ -590,39 +286,27 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose) // publish attitude groundtruth vehicle_attitude_s vehicle_attitude_groundtruth{}; -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - vehicle_attitude_groundtruth.timestamp_sample = time_us; -#else - vehicle_attitude_groundtruth.timestamp_sample = hrt_absolute_time(); -#endif + vehicle_attitude_groundtruth.timestamp_sample = timestamp; vehicle_attitude_groundtruth.q[0] = q_nb.W(); vehicle_attitude_groundtruth.q[1] = q_nb.X(); vehicle_attitude_groundtruth.q[2] = q_nb.Y(); vehicle_attitude_groundtruth.q[3] = q_nb.Z(); - vehicle_attitude_groundtruth.timestamp = hrt_absolute_time(); + vehicle_attitude_groundtruth.timestamp = timestamp; _attitude_ground_truth_pub.publish(vehicle_attitude_groundtruth); // publish angular velocity groundtruth const matrix::Eulerf euler{matrix::Quatf(vehicle_attitude_groundtruth.q)}; vehicle_angular_velocity_s vehicle_angular_velocity_groundtruth{}; -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - vehicle_angular_velocity_groundtruth.timestamp_sample = time_us; -#else - vehicle_angular_velocity_groundtruth.timestamp_sample = hrt_absolute_time(); -#endif + vehicle_angular_velocity_groundtruth.timestamp_sample = timestamp; const matrix::Vector3f angular_velocity = (euler - _euler_prev) / dt; _euler_prev = euler; angular_velocity.copyTo(vehicle_angular_velocity_groundtruth.xyz); - vehicle_angular_velocity_groundtruth.timestamp = hrt_absolute_time(); + vehicle_angular_velocity_groundtruth.timestamp = timestamp; _angular_velocity_ground_truth_pub.publish(vehicle_angular_velocity_groundtruth); vehicle_local_position_s local_position_groundtruth{}; -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - local_position_groundtruth.timestamp_sample = time_us; -#else - local_position_groundtruth.timestamp_sample = hrt_absolute_time(); -#endif + local_position_groundtruth.timestamp_sample = timestamp; // position ENU -> NED const matrix::Vector3d position{pose_position.y(), pose_position.x(), -pose_position.z()}; const matrix::Vector3d velocity{(position - _position_prev) / dt}; @@ -661,48 +345,29 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose) local_position_groundtruth.z_global = false; } - local_position_groundtruth.timestamp = hrt_absolute_time(); + local_position_groundtruth.timestamp = timestamp; _lpos_ground_truth_pub.publish(local_position_groundtruth); - - pthread_mutex_unlock(&_node_mutex); return; } } - - pthread_mutex_unlock(&_node_mutex); } -void GZBridge::odometryCallback(const gz::msgs::OdometryWithCovariance &odometry) +void GZBridge::odometryCallback(const gz::msgs::OdometryWithCovariance &msg) { - if (hrt_absolute_time() == 0) { - return; - } + const uint64_t timestamp = hrt_absolute_time(); - pthread_mutex_lock(&_node_mutex); - - const uint64_t time_us = (odometry.header().stamp().sec() * 1000000) + (odometry.header().stamp().nsec() / 1000); - - if (time_us > _world_time_us.load()) { - updateClock(odometry.header().stamp().sec(), odometry.header().stamp().nsec()); - } - - vehicle_odometry_s odom{}; -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - odom.timestamp_sample = time_us; - odom.timestamp = time_us; -#else - odom.timestamp_sample = hrt_absolute_time(); - odom.timestamp = hrt_absolute_time(); -#endif + vehicle_odometry_s report{}; + report.timestamp_sample = timestamp; + report.timestamp = timestamp; // gz odometry position is in ENU frame and needs to be converted to NED - odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED; - odom.position[0] = odometry.pose_with_covariance().pose().position().y(); - odom.position[1] = odometry.pose_with_covariance().pose().position().x(); - odom.position[2] = -odometry.pose_with_covariance().pose().position().z(); + report.pose_frame = vehicle_odometry_s::POSE_FRAME_NED; + report.position[0] = msg.pose_with_covariance().pose().position().y(); + report.position[1] = msg.pose_with_covariance().pose().position().x(); + report.position[2] = -msg.pose_with_covariance().pose().position().z(); // gz odometry orientation is "body FLU->ENU" and needs to be converted in "body FRD->NED" - gz::msgs::Quaternion pose_orientation = odometry.pose_with_covariance().pose().orientation(); + gz::msgs::Quaternion pose_orientation = msg.pose_with_covariance().pose().orientation(); gz::math::Quaterniond q_gr = gz::math::Quaterniond( pose_orientation.w(), pose_orientation.x(), @@ -710,101 +375,214 @@ void GZBridge::odometryCallback(const gz::msgs::OdometryWithCovariance &odometry pose_orientation.z()); gz::math::Quaterniond q_nb; GZBridge::rotateQuaternion(q_nb, q_gr); - odom.q[0] = q_nb.W(); - odom.q[1] = q_nb.X(); - odom.q[2] = q_nb.Y(); - odom.q[3] = q_nb.Z(); + report.q[0] = q_nb.W(); + report.q[1] = q_nb.X(); + report.q[2] = q_nb.Y(); + report.q[3] = q_nb.Z(); // gz odometry linear velocity is in body FLU and needs to be converted in body FRD - odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_BODY_FRD; - odom.velocity[0] = odometry.twist_with_covariance().twist().linear().x(); - odom.velocity[1] = -odometry.twist_with_covariance().twist().linear().y(); - odom.velocity[2] = -odometry.twist_with_covariance().twist().linear().z(); + report.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_BODY_FRD; + report.velocity[0] = msg.twist_with_covariance().twist().linear().x(); + report.velocity[1] = -msg.twist_with_covariance().twist().linear().y(); + report.velocity[2] = -msg.twist_with_covariance().twist().linear().z(); // gz odometry angular velocity is in body FLU and need to be converted in body FRD - odom.angular_velocity[0] = odometry.twist_with_covariance().twist().angular().x(); - odom.angular_velocity[1] = -odometry.twist_with_covariance().twist().angular().y(); - odom.angular_velocity[2] = -odometry.twist_with_covariance().twist().angular().z(); + report.angular_velocity[0] = msg.twist_with_covariance().twist().angular().x(); + report.angular_velocity[1] = -msg.twist_with_covariance().twist().angular().y(); + report.angular_velocity[2] = -msg.twist_with_covariance().twist().angular().z(); // VISION_POSITION_ESTIMATE covariance // pose 6x6 cross-covariance matrix // (states: x, y, z, roll, pitch, yaw). // If unknown, assign NaN value to first element in the array. - odom.position_variance[0] = odometry.pose_with_covariance().covariance().data(7); // Y row 1, col 1 - odom.position_variance[1] = odometry.pose_with_covariance().covariance().data(0); // X row 0, col 0 - odom.position_variance[2] = odometry.pose_with_covariance().covariance().data(14); // Z row 2, col 2 + report.position_variance[0] = msg.pose_with_covariance().covariance().data(7); // Y row 1, col 1 + report.position_variance[1] = msg.pose_with_covariance().covariance().data(0); // X row 0, col 0 + report.position_variance[2] = msg.pose_with_covariance().covariance().data(14); // Z row 2, col 2 - odom.orientation_variance[0] = odometry.pose_with_covariance().covariance().data(21); // R row 3, col 3 - odom.orientation_variance[1] = odometry.pose_with_covariance().covariance().data(28); // P row 4, col 4 - odom.orientation_variance[2] = odometry.pose_with_covariance().covariance().data(35); // Y row 5, col 5 + report.orientation_variance[0] = msg.pose_with_covariance().covariance().data(21); // R row 3, col 3 + report.orientation_variance[1] = msg.pose_with_covariance().covariance().data(28); // P row 4, col 4 + report.orientation_variance[2] = msg.pose_with_covariance().covariance().data(35); // Y row 5, col 5 - odom.velocity_variance[0] = odometry.twist_with_covariance().covariance().data(7); // Y row 1, col 1 - odom.velocity_variance[1] = odometry.twist_with_covariance().covariance().data(0); // X row 0, col 0 - odom.velocity_variance[2] = odometry.twist_with_covariance().covariance().data(14); // Z row 2, col 2 + report.velocity_variance[0] = msg.twist_with_covariance().covariance().data(7); // Y row 1, col 1 + report.velocity_variance[1] = msg.twist_with_covariance().covariance().data(0); // X row 0, col 0 + report.velocity_variance[2] = msg.twist_with_covariance().covariance().data(14); // Z row 2, col 2 - // odom.reset_counter = vpe.reset_counter; - _visual_odometry_pub.publish(odom); - - pthread_mutex_unlock(&_node_mutex); + // report.reset_counter = vpe.reset_counter; + _visual_odometry_pub.publish(report); } -void GZBridge::navSatCallback(const gz::msgs::NavSat &nav_sat) +static float generate_wgn() { - if (hrt_absolute_time() == 0) { - return; + // generate white Gaussian noise sample with std=1 + + // algorithm 1: + // float temp=((float)(rand()+1))/(((float)RAND_MAX+1.0f)); + // return sqrtf(-2.0f*logf(temp))*cosf(2.0f*M_PI_F*rand()/RAND_MAX); + // algorithm 2: from BlockRandGauss.hpp + static float V1, V2, S; + static bool phase = true; + float X; + + if (phase) { + do { + float U1 = (float)rand() / (float)RAND_MAX; + float U2 = (float)rand() / (float)RAND_MAX; + V1 = 2.0f * U1 - 1.0f; + V2 = 2.0f * U2 - 1.0f; + S = V1 * V1 + V2 * V2; + } while (S >= 1.0f || fabsf(S) < 1e-8f); + + X = V1 * float(sqrtf(-2.0f * float(logf(S)) / S)); + + } else { + X = V2 * float(sqrtf(-2.0f * float(logf(S)) / S)); } - pthread_mutex_lock(&_node_mutex); + phase = !phase; + return X; +} - const uint64_t time_us = (nav_sat.header().stamp().sec() * 1000000) + (nav_sat.header().stamp().nsec() / 1000); +void GZBridge::addRealisticGpsNoise(double &latitude, double &longitude, double &altitude, + float &vel_north, float &vel_east, float &vel_down) +{ + _gps_pos_noise_n = _pos_markov_time * _gps_pos_noise_n + + _pos_random_walk * generate_wgn() * _pos_noise_amplitude - + 0.02f * _gps_pos_noise_n; - if (time_us > _world_time_us.load()) { - updateClock(nav_sat.header().stamp().sec(), nav_sat.header().stamp().nsec()); - } + _gps_pos_noise_e = _pos_markov_time * _gps_pos_noise_e + + _pos_random_walk * generate_wgn() * _pos_noise_amplitude - + 0.02f * _gps_pos_noise_e; + + _gps_pos_noise_d = _pos_markov_time * _gps_pos_noise_d + + _pos_random_walk * generate_wgn() * _pos_noise_amplitude * 1.5f - + 0.02f * _gps_pos_noise_d; + + latitude += math::degrees((double)_gps_pos_noise_n / CONSTANTS_RADIUS_OF_EARTH); + longitude += math::degrees((double)_gps_pos_noise_e / CONSTANTS_RADIUS_OF_EARTH); + altitude += (double)_gps_pos_noise_d; + + _gps_vel_noise_n = _vel_markov_time * _gps_vel_noise_n + + _vel_noise_density * generate_wgn() * _vel_noise_amplitude; + + _gps_vel_noise_e = _vel_markov_time * _gps_vel_noise_e + + _vel_noise_density * generate_wgn() * _vel_noise_amplitude; + + _gps_vel_noise_d = _vel_markov_time * _gps_vel_noise_d + + _vel_noise_density * generate_wgn() * _vel_noise_amplitude * 1.2f; + + vel_north += _gps_vel_noise_n; + vel_east += _gps_vel_noise_e; + vel_down += _gps_vel_noise_d; +} + +void GZBridge::navSatCallback(const gz::msgs::NavSat &msg) +{ + const uint64_t timestamp = hrt_absolute_time(); // initialize gps position if (!_pos_ref.isInitialized()) { - _pos_ref.initReference(nav_sat.latitude_deg(), nav_sat.longitude_deg(), hrt_absolute_time()); - _alt_ref = nav_sat.altitude(); - - } else { - // publish GPS groundtruth - vehicle_global_position_s global_position_groundtruth{}; -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - global_position_groundtruth.timestamp_sample = time_us; -#else - global_position_groundtruth.timestamp_sample = hrt_absolute_time(); -#endif - global_position_groundtruth.lat = nav_sat.latitude_deg(); - global_position_groundtruth.lon = nav_sat.longitude_deg(); - global_position_groundtruth.alt = nav_sat.altitude(); - _gpos_ground_truth_pub.publish(global_position_groundtruth); - } - - pthread_mutex_unlock(&_node_mutex); -} -void GZBridge::laserScantoLidarSensorCallback(const gz::msgs::LaserScan &scan) -{ - if (hrt_absolute_time() == 0) { + _pos_ref.initReference(msg.latitude_deg(), msg.longitude_deg(), timestamp); + _alt_ref = msg.altitude(); return; } - distance_sensor_s distance_sensor{}; - distance_sensor.timestamp = hrt_absolute_time(); - device::Device::DeviceId id; - id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; - id.devid_s.bus = 0; - id.devid_s.address = 0; - id.devid_s.devtype = DRV_DIST_DEVTYPE_SIM; - distance_sensor.device_id = id.devid; - distance_sensor.min_distance = static_cast(scan.range_min()); - distance_sensor.max_distance = static_cast(scan.range_max()); - distance_sensor.current_distance = static_cast(scan.ranges()[0]); - distance_sensor.variance = 0.0f; - distance_sensor.signal_quality = -1; - distance_sensor.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; + double latitude = msg.latitude_deg(); + double longitude = msg.longitude_deg(); + double altitude = msg.altitude(); + float vel_north = msg.velocity_north(); + float vel_east = msg.velocity_east(); + float vel_down = -msg.velocity_up(); - gz::msgs::Quaternion pose_orientation = scan.world_pose().orientation(); + vehicle_global_position_s gps_truth{}; + + // Publish GPS groundtruth + gps_truth.timestamp = timestamp; + gps_truth.timestamp_sample = timestamp; + gps_truth.lat = latitude; + gps_truth.lon = longitude; + gps_truth.alt = altitude; + _gpos_ground_truth_pub.publish(gps_truth); + + // Apply noise model (based on ublox F9P) + addRealisticGpsNoise(latitude, longitude, altitude, vel_north, vel_east, vel_down); + + // Device ID + device::Device::DeviceId id{}; + id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; + id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM; + id.devid_s.bus = 1; + id.devid_s.address = 1; + + sensor_gps_s sensor_gps{}; + + if (_sim_gps_used.get() >= 4) { + // fix + sensor_gps.fix_type = 3; // 3D fix + sensor_gps.s_variance_m_s = 0.4f; + sensor_gps.c_variance_rad = 0.1f; + sensor_gps.eph = 0.9f; + sensor_gps.epv = 1.78f; + sensor_gps.hdop = 0.7f; + sensor_gps.vdop = 1.1f; + + } else { + // no fix + sensor_gps.fix_type = 0; // No fix + sensor_gps.s_variance_m_s = 100.f; + sensor_gps.c_variance_rad = 100.f; + sensor_gps.eph = 100.f; + sensor_gps.epv = 100.f; + sensor_gps.hdop = 100.f; + sensor_gps.vdop = 100.f; + } + + sensor_gps.timestamp = timestamp; + sensor_gps.timestamp_sample = timestamp; + sensor_gps.time_utc_usec = 0; + sensor_gps.device_id = id.devid; + sensor_gps.latitude_deg = latitude; + sensor_gps.longitude_deg = longitude; + sensor_gps.altitude_msl_m = altitude; + sensor_gps.altitude_ellipsoid_m = altitude; + sensor_gps.noise_per_ms = 0; + sensor_gps.jamming_indicator = 0; + sensor_gps.vel_m_s = sqrtf(vel_north * vel_north + vel_east * vel_east); + sensor_gps.vel_n_m_s = vel_north; + sensor_gps.vel_e_m_s = vel_east; + sensor_gps.vel_d_m_s = vel_down; + sensor_gps.cog_rad = atan2(vel_east, vel_north); + sensor_gps.timestamp_time_relative = 0; + sensor_gps.heading = NAN; + sensor_gps.heading_offset = NAN; + sensor_gps.heading_accuracy = 0; + sensor_gps.automatic_gain_control = 0; + sensor_gps.jamming_state = 0; + sensor_gps.spoofing_state = 0; + sensor_gps.vel_ned_valid = true; + sensor_gps.satellites_used = _sim_gps_used.get(); + + _sensor_gps_pub.publish(sensor_gps); +} + +void GZBridge::laserScantoLidarSensorCallback(const gz::msgs::LaserScan &msg) +{ + device::Device::DeviceId id{}; + id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; + id.devid_s.devtype = DRV_DIST_DEVTYPE_SIM; + id.devid_s.bus = 1; + id.devid_s.address = 1; + + distance_sensor_s report{}; + report.timestamp = hrt_absolute_time(); + report.device_id = id.devid; + report.min_distance = static_cast(msg.range_min()); + report.max_distance = static_cast(msg.range_max()); + report.current_distance = static_cast(msg.ranges()[0]); + report.variance = 0.0f; + report.signal_quality = -1; + report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; + + gz::msgs::Quaternion pose_orientation = msg.world_pose().orientation(); gz::math::Quaterniond q_sensor = gz::math::Quaterniond( pose_orientation.w(), pose_orientation.x(), @@ -818,34 +596,34 @@ void GZBridge::laserScantoLidarSensorCallback(const gz::msgs::LaserScan &scan) const gz::math::Quaterniond q_down(0, 1, 0, 0); if (q_sensor.Equal(q_front, 0.03)) { - distance_sensor.orientation = distance_sensor_s::ROTATION_FORWARD_FACING; + report.orientation = distance_sensor_s::ROTATION_FORWARD_FACING; } else if (q_sensor.Equal(q_down, 0.03)) { - distance_sensor.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; + report.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; } else if (q_sensor.Equal(q_left, 0.03)) { - distance_sensor.orientation = distance_sensor_s::ROTATION_LEFT_FACING; + report.orientation = distance_sensor_s::ROTATION_LEFT_FACING; } else { - distance_sensor.orientation = distance_sensor_s::ROTATION_CUSTOM; - distance_sensor.q[0] = q_sensor.W(); - distance_sensor.q[1] = q_sensor.X(); - distance_sensor.q[2] = q_sensor.Y(); - distance_sensor.q[3] = q_sensor.Z(); + report.orientation = distance_sensor_s::ROTATION_CUSTOM; + report.q[0] = q_sensor.W(); + report.q[1] = q_sensor.X(); + report.q[2] = q_sensor.Y(); + report.q[3] = q_sensor.Z(); } - _distance_sensor_pub.publish(distance_sensor); + _distance_sensor_pub.publish(report); } -void GZBridge::laserScanCallback(const gz::msgs::LaserScan &scan) +void GZBridge::laserScanCallback(const gz::msgs::LaserScan &msg) { static constexpr int SECTOR_SIZE_DEG = 5; // PX4 Collision Prevention uses 5 degree sectors - double angle_min_deg = scan.angle_min() * 180 / M_PI; - double angle_step_deg = scan.angle_step() * 180 / M_PI; + double angle_min_deg = msg.angle_min() * 180 / M_PI; + double angle_step_deg = msg.angle_step() * 180 / M_PI; int samples_per_sector = std::round(SECTOR_SIZE_DEG / angle_step_deg); - int number_of_sectors = scan.ranges_size() / samples_per_sector; + int number_of_sectors = msg.ranges_size() / samples_per_sector; std::vector ds_array(number_of_sectors, UINT16_MAX); @@ -858,7 +636,7 @@ void GZBridge::laserScanCallback(const gz::msgs::LaserScan &scan) for (int j = 0; j < samples_per_sector; j++) { - double distance = scan.ranges()[i * samples_per_sector + j]; + double distance = msg.ranges()[i * samples_per_sector + j]; // inf values mean no object if (isinf(distance)) { @@ -871,7 +649,7 @@ void GZBridge::laserScanCallback(const gz::msgs::LaserScan &scan) // If all samples in a sector are inf then it means the sector is clear if (samples_used_in_sector == 0) { - ds_array[i] = scan.range_max(); + ds_array[i] = msg.range_max(); } else { ds_array[i] = sum / samples_used_in_sector; @@ -879,20 +657,20 @@ void GZBridge::laserScanCallback(const gz::msgs::LaserScan &scan) } // Publish to uORB - obstacle_distance_s obs {}; + obstacle_distance_s report {}; // Initialize unknown - for (auto &i : obs.distances) { + for (auto &i : report.distances) { i = UINT16_MAX; } - obs.timestamp = hrt_absolute_time(); - obs.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD; - obs.sensor_type = obstacle_distance_s::MAV_DISTANCE_SENSOR_LASER; - obs.min_distance = static_cast(scan.range_min() * 100.); - obs.max_distance = static_cast(scan.range_max() * 100.); - obs.angle_offset = static_cast(angle_min_deg); - obs.increment = static_cast(SECTOR_SIZE_DEG); + report.timestamp = hrt_absolute_time(); + report.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD; + report.sensor_type = obstacle_distance_s::MAV_DISTANCE_SENSOR_LASER; + report.min_distance = static_cast(msg.range_min() * 100.); + report.max_distance = static_cast(msg.range_max() * 100.); + report.angle_offset = static_cast(angle_min_deg); + report.increment = static_cast(SECTOR_SIZE_DEG); // Map samples in FOV into sectors in ObstacleDistance int index = 0; @@ -902,131 +680,22 @@ void GZBridge::laserScanCallback(const gz::msgs::LaserScan &scan) uint16_t distance_cm = (*i) * 100.; - if (distance_cm >= obs.max_distance) { - obs.distances[index] = obs.max_distance + 1; + if (distance_cm >= report.max_distance) { + report.distances[index] = report.max_distance + 1; - } else if (distance_cm < obs.min_distance) { - obs.distances[index] = 0; + } else if (distance_cm < report.min_distance) { + report.distances[index] = 0; } else { - obs.distances[index] = distance_cm; + report.distances[index] = distance_cm; } index++; } - _obstacle_distance_pub.publish(obs); + _obstacle_distance_pub.publish(report); } -bool GZBridge::callEntityFactoryService(const std::string &service, const gz::msgs::EntityFactory &req) -{ - bool result; - gz::msgs::Boolean rep; - - if (_node.Request(service, req, 1000, rep, result)) { - if (!rep.data() || !result) { - PX4_ERR("EntityFactory service call failed."); - return false; - } - - } else { - PX4_WARN("Service call timed out. Check GZ_SIM_RESOURCE_PATH is set correctly."); - return false; - } - - return true; -} - -bool GZBridge::callSceneInfoMsgService(const std::string &service) -{ - bool result; - gz::msgs::Empty req; - gz::msgs::Scene rep; - - if (_node.Request(service, req, 3000, rep, result)) { - if (!result) { - PX4_ERR("Scene Info service call failed."); - return false; - - } else { - return true; - } - - } else { - PX4_WARN("Service call timed out. Check GZ_SIM_RESOURCE_PATH is set correctly."); - return false; - } - - return true; -} - -bool GZBridge::callPhysicsMsgService(const std::string &service, const gz::msgs::Physics &req) -{ - bool result; - gz::msgs::Boolean rep; - - if (_node.Request(service, req, 5000, rep, result)) { - if (!result) { - PX4_ERR("Physics service call failed."); - return false; - - } else { - return true; - } - - } else { - PX4_ERR("Physics Service call timed out. Check GZ_SIM_RESOURCE_PATH is set correctly."); - return false; - } - - return true; -} - -bool GZBridge::callStringMsgService(const std::string &service, const gz::msgs::StringMsg &req) -{ - bool result; - - gz::msgs::Boolean rep; - - if (_node.Request(service, req, 3000, rep, result)) { - if (!rep.data() || !result) { - PX4_ERR("String service call failed"); - return false; - - } - } - - else { - PX4_WARN("Service call timed out: %s", service.c_str()); - return false; - } - - return true; -} - -bool GZBridge::callVector3dService(const std::string &service, const gz::msgs::Vector3d &req) -{ - bool result; - - gz::msgs::Boolean rep; - - if (_node.Request(service, req, 3000, rep, result)) { - if (!rep.data() || !result) { - PX4_ERR("String service call failed"); - return false; - - } - } - - else { - PX4_WARN("Service call timed out: %s", service.c_str()); - return false; - } - - return true; -} - - void GZBridge::rotateQuaternion(gz::math::Quaterniond &q_FRD_to_NED, const gz::math::Quaterniond q_FLU_to_ENU) { // FLU (ROS) to FRD (PX4) static rotation @@ -1059,8 +728,6 @@ void GZBridge::Run() return; } - pthread_mutex_lock(&_node_mutex); - if (_parameter_update_sub.updated()) { parameter_update_s pupdate; _parameter_update_sub.copy(&pupdate); @@ -1074,8 +741,53 @@ void GZBridge::Run() } ScheduleDelayed(10_ms); +} - pthread_mutex_unlock(&_node_mutex); +int GZBridge::task_spawn(int argc, char *argv[]) +{ + std::string world_name; + std::string model_name; + + int myoptind = 1; + int ch; + const char *myoptarg = nullptr; + + while ((ch = px4_getopt(argc, argv, "w:n:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'w': + world_name = myoptarg; + break; + + case 'n': + model_name = myoptarg; + break; + + default: + print_usage(); + return PX4_ERROR; + } + } + + PX4_INFO("world: %s, model: %s", world_name.c_str(), model_name.c_str()); + + GZBridge *instance = new GZBridge(world_name, model_name); + + if (!instance) { + PX4_ERR("alloc failed"); + return PX4_ERROR; + } + + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init() != PX4_OK) { + delete instance; + _object.store(nullptr); + _task_id = -1; + return PX4_ERROR; + } + + return PX4_OK; } int GZBridge::print_status() @@ -1111,11 +823,8 @@ int GZBridge::print_usage(const char *reason) PRINT_MODULE_USAGE_NAME("gz_bridge", "driver"); PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_PARAM_STRING('m', nullptr, nullptr, "Fuel model name", false); - PRINT_MODULE_USAGE_PARAM_STRING('p', nullptr, nullptr, "Model Pose", false); - PRINT_MODULE_USAGE_PARAM_STRING('n', nullptr, nullptr, "Model name", false); - PRINT_MODULE_USAGE_PARAM_STRING('i', nullptr, nullptr, "PX4 instance", false); PRINT_MODULE_USAGE_PARAM_STRING('w', nullptr, nullptr, "World name", true); + PRINT_MODULE_USAGE_PARAM_STRING('n', nullptr, nullptr, "Model name", false); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 32b445decd..5e157f43b2 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. + * Copyright (c) 2025 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -44,24 +44,26 @@ #include #include #include +#include #include + #include #include #include #include #include #include -#include #include #include +#include +#include +#include +#include #include #include #include #include -#include #include -#include -#include #include #include @@ -81,7 +83,7 @@ using namespace time_literals; class GZBridge : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: - GZBridge(const char *world, const char *name, const char *model, const char *pose_str); + GZBridge(const std::string &world, const std::string &model_name); ~GZBridge() override; /** @see ModuleBase */ @@ -98,78 +100,26 @@ public: /** @see ModuleBase::print_status() */ int print_status() override; - uint64_t world_time_us() const { return _world_time_us.load(); } - private: void Run() override; - bool updateClock(const uint64_t tv_sec, const uint64_t tv_nsec); + void clockCallback(const gz::msgs::Clock &msg); + void airspeedCallback(const gz::msgs::AirSpeed &msg); + void barometerCallback(const gz::msgs::FluidPressure &msg); + void imuCallback(const gz::msgs::IMU &msg); + void poseInfoCallback(const gz::msgs::Pose_V &msg); + void odometryCallback(const gz::msgs::OdometryWithCovariance &msg); + void navSatCallback(const gz::msgs::NavSat &msg); + void laserScantoLidarSensorCallback(const gz::msgs::LaserScan &msg); + void laserScanCallback(const gz::msgs::LaserScan &msg); - void clockCallback(const gz::msgs::Clock &clock); - - void airspeedCallback(const gz::msgs::AirSpeed &air_speed); - void barometerCallback(const gz::msgs::FluidPressure &air_pressure); - void imuCallback(const gz::msgs::IMU &imu); - void poseInfoCallback(const gz::msgs::Pose_V &pose); - void odometryCallback(const gz::msgs::OdometryWithCovariance &odometry); - void navSatCallback(const gz::msgs::NavSat &nav_sat); - void laserScantoLidarSensorCallback(const gz::msgs::LaserScan &scan); - void laserScanCallback(const gz::msgs::LaserScan &scan); - - /** - * @brief Call Entityfactory service - * - * @param req - * @return true - * @return false - */ - bool callEntityFactoryService(const std::string &service, const gz::msgs::EntityFactory &req); - - - /** - * @brief Call scene info service - * - * @param service - * @param req - * @return true - * @return false - */ - bool callSceneInfoMsgService(const std::string &service); - - /** - * @brief Call String service - * - * @param service - * @param req - * @return true - * @return false - */ - bool callStringMsgService(const std::string &service, const gz::msgs::StringMsg &req); - - /** - * @brief Call Vector3d Service - * - * @param service - * @param req - * @return true - * @return false - */ - bool callVector3dService(const std::string &service, const gz::msgs::Vector3d &req); - /** - * - * Convert a quaterion from FLU_to_ENU frames (ROS convention) - * to FRD_to_NED frames (PX4 convention) - * - * @param q_FRD_to_NED output quaterion in PX4 conventions - * @param q_FLU_to_ENU input quaterion in ROS conventions - */ static void rotateQuaternion(gz::math::Quaterniond &q_FRD_to_NED, const gz::math::Quaterniond q_FLU_to_ENU); - bool callPhysicsMsgService(const std::string &service, const gz::msgs::Physics &req); + void addRealisticGpsNoise(double &latitude, double &longitude, double &altitude, + float &vel_north, float &vel_east, float &vel_down); - // Subscriptions - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Publication _distance_sensor_pub{ORB_ID(distance_sensor)}; uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; @@ -178,23 +128,21 @@ private: uORB::Publication _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)}; uORB::Publication _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)}; uORB::Publication _lpos_ground_truth_pub{ORB_ID(vehicle_local_position_groundtruth)}; - uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; - uORB::PublicationMulti _sensor_accel_pub{ORB_ID(sensor_accel)}; - uORB::PublicationMulti _sensor_gyro_pub{ORB_ID(sensor_gyro)}; - uORB::PublicationMulti _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; + uORB::PublicationMulti _sensor_gps_pub{ORB_ID(sensor_gps)}; + uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + uORB::PublicationMulti _sensor_accel_pub{ORB_ID(sensor_accel)}; + uORB::PublicationMulti _sensor_gyro_pub{ORB_ID(sensor_gyro)}; + uORB::PublicationMulti _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; - GZMixingInterfaceESC _mixing_interface_esc{_node, _node_mutex}; - GZMixingInterfaceServo _mixing_interface_servo{_node, _node_mutex}; - GZMixingInterfaceWheel _mixing_interface_wheel{_node, _node_mutex}; - GZGimbal _gimbal{_node, _node_mutex}; + GZMixingInterfaceESC _mixing_interface_esc{_node}; + GZMixingInterfaceServo _mixing_interface_servo{_node}; + GZMixingInterfaceWheel _mixing_interface_wheel{_node}; - px4::atomic _world_time_us{0}; - - pthread_mutex_t _node_mutex; + GZGimbal _gimbal{_node}; MapProjection _pos_ref{}; - double _alt_ref{}; // starting altitude reference + double _alt_ref{}; matrix::Vector3d _position_prev{}; matrix::Vector3d _velocity_prev{}; @@ -203,10 +151,26 @@ private: const std::string _world_name; const std::string _model_name; - const std::string _model_sim; - const std::string _model_pose; float _temperature{288.15}; // 15 degrees gz::transport::Node _node; + + // GPS noise model + float _gps_pos_noise_n = 0.0f; + float _gps_pos_noise_e = 0.0f; + float _gps_pos_noise_d = 0.0f; + float _gps_vel_noise_n = 0.0f; + float _gps_vel_noise_e = 0.0f; + float _gps_vel_noise_d = 0.0f; + const float _pos_noise_amplitude = 0.8f; // Position noise amplitude [m] + const float _pos_random_walk = 0.01f; // Position random walk coefficient + const float _pos_markov_time = 0.95f; // Position Markov process coefficient + const float _vel_noise_amplitude = 0.05f; // Velocity noise amplitude [m/s] + const float _vel_noise_density = 0.2f; // Velocity noise process density + const float _vel_markov_time = 0.85f; // Velocity Markov process coefficient + + DEFINE_PARAMETERS( + (ParamInt) _sim_gps_used + ) }; diff --git a/src/modules/simulation/gz_bridge/GZGimbal.cpp b/src/modules/simulation/gz_bridge/GZGimbal.cpp index 976200020e..2d7ed9e069 100644 --- a/src/modules/simulation/gz_bridge/GZGimbal.cpp +++ b/src/modules/simulation/gz_bridge/GZGimbal.cpp @@ -49,6 +49,8 @@ bool GZGimbal::init(const std::string &world_name, const std::string &model_name return false; } + pthread_mutex_init(&_node_mutex, nullptr); + updateParameters(); ScheduleOnInterval(200_ms); // @5Hz diff --git a/src/modules/simulation/gz_bridge/GZGimbal.hpp b/src/modules/simulation/gz_bridge/GZGimbal.hpp index 662268abd2..897c43150f 100644 --- a/src/modules/simulation/gz_bridge/GZGimbal.hpp +++ b/src/modules/simulation/gz_bridge/GZGimbal.hpp @@ -27,18 +27,17 @@ using namespace time_literals; class GZGimbal : public px4::ScheduledWorkItem, public ModuleParams { public: - GZGimbal(gz::transport::Node &node, pthread_mutex_t &node_mutex) : + GZGimbal(gz::transport::Node &node) : px4::ScheduledWorkItem(MODULE_NAME "-gimbal", px4::wq_configurations::rate_ctrl), ModuleParams(nullptr), - _node(node), - _node_mutex(node_mutex) + _node(node) {} private: friend class GZBridge; gz::transport::Node &_node; - pthread_mutex_t &_node_mutex; + pthread_mutex_t _node_mutex; uORB::Subscription _gimbal_device_set_attitude_sub{ORB_ID(gimbal_device_set_attitude)}; uORB::Subscription _gimbal_controls_sub{ORB_ID(gimbal_controls)}; diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp index c1a88f7bda..82ff4cf3da 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp @@ -55,6 +55,8 @@ bool GZMixingInterfaceESC::init(const std::string &model_name) _esc_status_pub.advertise(); + pthread_mutex_init(&_node_mutex, nullptr); + ScheduleNow(); return true; diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp index 316ff6195a..d724c5c6f9 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp @@ -50,10 +50,9 @@ class GZMixingInterfaceESC : public OutputModuleInterface public: static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS; - GZMixingInterfaceESC(gz::transport::Node &node, pthread_mutex_t &node_mutex) : + GZMixingInterfaceESC(gz::transport::Node &node) : OutputModuleInterface(MODULE_NAME "-actuators-esc", px4::wq_configurations::rate_ctrl), - _node(node), - _node_mutex(node_mutex) + _node(node) {} bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], @@ -77,7 +76,7 @@ private: void motorSpeedCallback(const gz::msgs::Actuators &actuators); gz::transport::Node &_node; - pthread_mutex_t &_node_mutex; + pthread_mutex_t _node_mutex; MixingOutput _mixing_output{"SIM_GZ_EC", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp index 2fc656074c..6fd069ec09 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp @@ -121,6 +121,8 @@ bool GZMixingInterfaceServo::init(const std::string &model_name) _angular_range_rad.push_back(max_val - min_val); } + pthread_mutex_init(&_node_mutex, nullptr); + ScheduleNow(); return true; diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp index 180f6cbca2..9980918233 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp @@ -44,10 +44,9 @@ class GZMixingInterfaceServo : public OutputModuleInterface { public: - GZMixingInterfaceServo(gz::transport::Node &node, pthread_mutex_t &node_mutex) : + GZMixingInterfaceServo(gz::transport::Node &node) : OutputModuleInterface(MODULE_NAME "-actuators-servo", px4::wq_configurations::rate_ctrl), - _node(node), - _node_mutex(node_mutex) + _node(node) {} bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], @@ -83,7 +82,7 @@ private: float get_servo_angle_min(const size_t index); gz::transport::Node &_node; - pthread_mutex_t &_node_mutex; + pthread_mutex_t _node_mutex; MixingOutput _mixing_output{"SIM_GZ_SV", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp index 2198bacbaa..cd015892de 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp @@ -51,6 +51,8 @@ bool GZMixingInterfaceWheel::init(const std::string &model_name) return false; } + pthread_mutex_init(&_node_mutex, nullptr); + _wheel_encoders_pub.advertise(); ScheduleNow(); diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp index 281606dd61..8e14ddf958 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp @@ -51,10 +51,9 @@ class GZMixingInterfaceWheel : public OutputModuleInterface public: static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS; - GZMixingInterfaceWheel(gz::transport::Node &node, pthread_mutex_t &node_mutex) : + GZMixingInterfaceWheel(gz::transport::Node &node) : OutputModuleInterface(MODULE_NAME "-actuators-wheel", px4::wq_configurations::rate_ctrl), - _node(node), - _node_mutex(node_mutex) + _node(node) {} bool updateOutputs(bool stop_wheels, uint16_t outputs[MAX_ACTUATORS], @@ -78,7 +77,7 @@ private: void wheelSpeedCallback(const gz::msgs::Actuators &actuators); gz::transport::Node &_node; - pthread_mutex_t &_node_mutex; + pthread_mutex_t _node_mutex; MixingOutput _mixing_output{"SIM_GZ_WH", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; From 6dc39d9deb6f1de4c1bb97461193aba8ff679c59 Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Mon, 3 Mar 2025 12:21:28 -0900 Subject: [PATCH 78/91] [wip] gz plugins (#24153) * added optical flow to gz bridge * log high rate sensor data * it builds * it builds and publishes, need to figure out build system now * single library * rename files * add gz_msg for proto, fix build, test basic flow impl * update rate, no blur * PX4-OpticalFlow impl * rename OpticalFlowSensor * rename plugins * disable gps, add plugin path * cleanup * fix plugin path export * properly add OpticalFlowSystem dependency to gz * move everything under gz_bridge * cleanup * add GZ_VEBOSE * cleanup model/world build target cmake * added GZ_DISTRO env, harmonic or ionic * fix gz transport, unstage ark fpv bootloader * unstage logged_topics.cpp * cleanup * make format * ci fixes * fix cmake * remove required for gz-transport * use model/world namespace for multi vehicle sim. Make format * make format * license * remove needless member var * made separate Kconfig for gz_msgs, gz_plugins, and gz_bridge * move OpticalFlow build to it's own cmake * fix clang * cleanup comments * fix rebase --- .../init.d-posix/airframes/4021_gz_x500_flow | 15 ++ .../init.d-posix/airframes/CMakeLists.txt | 1 + .../init.d-posix/px4-rc.simulator | 9 +- Tools/simulation/gz | 2 +- boards/px4/sitl/default.px4board | 2 + .../simulation/gz_bridge/CMakeLists.txt | 103 ++++++----- src/modules/simulation/gz_bridge/GZBridge.cpp | 42 +++++ src/modules/simulation/gz_bridge/GZBridge.hpp | 6 +- src/modules/simulation/gz_bridge/Kconfig | 2 +- src/modules/simulation/gz_bridge/gz_env.sh.in | 2 + src/modules/simulation/gz_msgs/CMakeLists.txt | 55 ++++++ src/modules/simulation/gz_msgs/Kconfig | 6 + .../simulation/gz_msgs/opticalflow.proto | 19 ++ .../simulation/gz_plugins/CMakeLists.txt | 70 ++++++++ src/modules/simulation/gz_plugins/Kconfig | 6 + .../gz_plugins/OpticalFlowSensor.cpp | 165 ++++++++++++++++++ .../gz_plugins/OpticalFlowSensor.hpp | 75 ++++++++ .../gz_plugins/OpticalFlowSystem.cpp | 125 +++++++++++++ .../gz_plugins/OpticalFlowSystem.hpp | 59 +++++++ .../simulation/gz_plugins/optical_flow.cmake | 53 ++++++ 20 files changed, 764 insertions(+), 53 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/4021_gz_x500_flow create mode 100644 src/modules/simulation/gz_msgs/CMakeLists.txt create mode 100644 src/modules/simulation/gz_msgs/Kconfig create mode 100644 src/modules/simulation/gz_msgs/opticalflow.proto create mode 100644 src/modules/simulation/gz_plugins/CMakeLists.txt create mode 100644 src/modules/simulation/gz_plugins/Kconfig create mode 100644 src/modules/simulation/gz_plugins/OpticalFlowSensor.cpp create mode 100644 src/modules/simulation/gz_plugins/OpticalFlowSensor.hpp create mode 100644 src/modules/simulation/gz_plugins/OpticalFlowSystem.cpp create mode 100644 src/modules/simulation/gz_plugins/OpticalFlowSystem.hpp create mode 100644 src/modules/simulation/gz_plugins/optical_flow.cmake diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4021_gz_x500_flow b/ROMFS/px4fmu_common/init.d-posix/airframes/4021_gz_x500_flow new file mode 100644 index 0000000000..5f0aababbd --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4021_gz_x500_flow @@ -0,0 +1,15 @@ +#!/bin/sh +# +# @name Gazebo x500 with downward optical flow and distance sensor +# +# @type Quadrotor +# + +PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_flow} + +. ${R}etc/init.d-posix/airframes/4001_gz_x500 + +echo "Disabling Sim GPS" +param set-default SYS_HAS_GPS 0 +param set-default SIM_GPS_USED 0 +param set-default EKF2_GPS_CTRL 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index 4ae56f51e9..b4e107799f 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -91,6 +91,7 @@ px4_add_romfs_files( 4018_gz_quadtailsitter 4019_gz_x500_gimbal 4020_gz_tiltrotor + 4021_gz_x500_flow 6011_gazebo-classic_typhoon_h480 6011_gazebo-classic_typhoon_h480.post diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator index ede64dcd34..b396115491 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator @@ -87,9 +87,9 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then . ../gz_env.sh fi - # Start gazebo with default world echo "INFO [init] Starting gazebo with world: ${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" - ${gz_command} ${gz_sub_command} --verbose=1 -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" & + + ${gz_command} ${gz_sub_command} --verbose=${GZ_VERBOSE:=1} -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" & if [ -z "${HEADLESS}" ]; then echo "INFO [init] Starting gz gui" @@ -124,9 +124,10 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then echo "INFO [init] Spawning model at position: ${pos_x} ${pos_y} ${pos_z}" fi + echo "INFO [init] Spawning model" # Spawn model ${gz_command} service -s "/world/${PX4_GZ_WORLD}/create" --reqtype gz.msgs.EntityFactory \ - --reptype gz.msgs.Boolean --timeout 1000 \ + --reptype gz.msgs.Boolean --timeout 5000 \ --req "sdf_filename: \"${PX4_GZ_MODELS}/${MODEL_NAME}\", name: \"${MODEL_NAME_INSTANCE}\", allow_renaming: false${POSE_ARG}" > /dev/null 2>&1 # Start gz_bridge @@ -163,7 +164,7 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then if [ -n "${PX4_SIM_SPEED_FACTOR}" ]; then echo "INFO [init] Setting simulation speed factor: ${PX4_SIM_SPEED_FACTOR}" ${gz_command} service -s "/world/${PX4_GZ_WORLD}/set_physics" --reqtype gz.msgs.Physics \ - --reptype gz.msgs.Boolean --timeout 1000 \ + --reptype gz.msgs.Boolean --timeout 5000 \ --req "real_time_factor: ${PX4_SIM_SPEED_FACTOR}" > /dev/null 2>&1 fi diff --git a/Tools/simulation/gz b/Tools/simulation/gz index 23170a9125..5bbae38b4f 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 23170a91255d99aea8960d1101541afce0f209d9 +Subproject commit 5bbae38b4f942521b4f3288c298083571ea5718c diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index b0b7a8a2b3..8eb8ebec8c 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -50,7 +50,9 @@ CONFIG_MODULES_ROVER_MECANUM=y CONFIG_MODULES_ROVER_POS_CONTROL=y CONFIG_MODULES_SENSORS=y CONFIG_COMMON_SIMULATION=y +CONFIG_MODULES_SIMULATION_GZ_MSGS=y CONFIG_MODULES_SIMULATION_GZ_BRIDGE=y +CONFIG_MODULES_SIMULATION_GZ_PLUGINS=y CONFIG_MODULES_SIMULATION_SENSOR_AGP_SIM=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_UUV_ATT_CONTROL=y diff --git a/src/modules/simulation/gz_bridge/CMakeLists.txt b/src/modules/simulation/gz_bridge/CMakeLists.txt index e57c602045..784c5a5ca8 100644 --- a/src/modules/simulation/gz_bridge/CMakeLists.txt +++ b/src/modules/simulation/gz_bridge/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. +# Copyright (c) 2025 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -31,21 +31,42 @@ # ############################################################################ -# Find the gz_Transport library -# Look for GZ Ionic or Harmonic -find_package(gz-transport NAMES gz-transport14 gz-transport13) +if(NOT DEFINED ENV{GZ_DISTRO}) + set(GZ_DISTRO "harmonic" CACHE STRING "Gazebo distribution to use") +else() + set(GZ_DISTRO $ENV{GZ_DISTRO}) +endif() -if(gz-transport_FOUND) +# Define library version combinations for different Gazebo distributions +# https://github.com/gazebo-tooling/gazebodistro/blob/master/collection-harmonic.yaml +if(GZ_DISTRO STREQUAL "harmonic") + set(GZ_CMAKE_VERSION "3") + set(GZ_MSGS_VERSION "10") + set(GZ_TRANSPORT_VERSION "13") + set(GZ_PLUGIN_VERSION "2") + set(GZ_SIM_VERSION "8") + set(GZ_SENSORS_VERSION "8") + message(STATUS "Using Gazebo Harmonic (cmake:${GZ_CMAKE_VERSION}, msgs:${GZ_MSGS_VERSION}, transport:${GZ_TRANSPORT_VERSION})") +elseif(GZ_DISTRO STREQUAL "ionic") + set(GZ_CMAKE_VERSION "4") + set(GZ_MSGS_VERSION "11") + set(GZ_TRANSPORT_VERSION "14") + set(GZ_PLUGIN_VERSION "3") + set(GZ_SIM_VERSION "9") + set(GZ_SENSORS_VERSION "9") + message(STATUS "Using Gazebo Ionic (cmake:${GZ_CMAKE_VERSION}, msgs:${GZ_MSGS_VERSION}, transport:${GZ_TRANSPORT_VERSION})") +else() + message(FATAL_ERROR "Unknown Gazebo distribution: ${GZ_DISTRO}. Valid options are: harmonic or ionic") +endif() - add_compile_options(-frtti -fexceptions) +# Use gz-transport as litmus test for prescence of gz +find_package(gz-transport${GZ_TRANSPORT_VERSION}) - set(GZ_TRANSPORT_VER ${gz-transport_VERSION_MAJOR}) +if (gz-transport${GZ_TRANSPORT_VERSION}_FOUND) - if(GZ_TRANSPORT_VER GREATER_EQUAL 12) - set(GZ_TRANSPORT_LIB gz-transport${GZ_TRANSPORT_VER}::core) - else() - set(GZ_TRANSPORT_LIB ignition-transport${GZ_TRANSPORT_VER}::core) - endif() + find_package(gz-cmake${GZ_CMAKE_VERSION} REQUIRED) + find_package(gz-msgs${GZ_MSGS_VERSION} REQUIRED) + find_package(Protobuf REQUIRED) px4_add_module( MODULE modules__simulation__gz_bridge @@ -66,11 +87,19 @@ if(gz-transport_FOUND) DEPENDS mixer_module px4_work_queue - ${GZ_TRANSPORT_LIB} + gz-transport${GZ_TRANSPORT_VERSION}::core MODULE_CONFIG module.yaml ) + target_include_directories(modules__simulation__gz_bridge + PUBLIC + ${PX4_GZ_MSGS_BINARY_DIR} + ) + + target_include_directories(modules__simulation__gz_bridge PUBLIC px4_gz_msgs) + target_link_libraries(modules__simulation__gz_bridge PUBLIC px4_gz_msgs) + px4_add_git_submodule(TARGET git_gz PATH "${PX4_SOURCE_DIR}/Tools/simulation/gz") include(ExternalProject) ExternalProject_Add(gz @@ -82,59 +111,41 @@ if(gz-transport_FOUND) USES_TERMINAL_CONFIGURE true USES_TERMINAL_BUILD true EXCLUDE_FROM_ALL true - BUILD_ALWAYS 1 ) - set(gz_worlds - default - windy - baylands - lawn - aruco - rover - walls - ) - - # find corresponding airframes - file(GLOB gz_airframes - RELATIVE ${PX4_SOURCE_DIR}/ROMFS/px4fmu_common/init.d-posix/airframes - ${PX4_SOURCE_DIR}/ROMFS/px4fmu_common/init.d-posix/airframes/*_gz_* - ) - - # remove any .post files - foreach(gz_airframe IN LISTS gz_airframes) - if(gz_airframe MATCHES ".post") - list(REMOVE_ITEM gz_airframes ${gz_airframe}) - endif() - endforeach() - list(REMOVE_DUPLICATES gz_airframes) + # Below we setup the build targets for our worlds and models + # Syntax: gz__ + # Example: gz_x500_flow_forest + file(GLOB gz_worlds ${PX4_SOURCE_DIR}/Tools/simulation/gz/worlds/*.sdf) + file(GLOB gz_airframes ${PX4_SOURCE_DIR}/ROMFS/px4fmu_common/init.d-posix/airframes/*_gz_*) foreach(gz_airframe IN LISTS gz_airframes) - set(model_only) - string(REGEX REPLACE ".*_gz_" "" model_only ${gz_airframe}) + set(model_name) + string(REGEX REPLACE ".*_gz_" "" model_name ${gz_airframe}) foreach(world ${gz_worlds}) get_filename_component("world_name" ${world} NAME_WE) if(world_name STREQUAL "default") - add_custom_target(gz_${model_only} - COMMAND ${CMAKE_COMMAND} -E env PX4_SIM_MODEL=gz_${model_only} $ + add_custom_target(gz_${model_name} + COMMAND ${CMAKE_COMMAND} -E env PX4_SIM_MODEL=gz_${model_name} $ WORKING_DIRECTORY ${SITL_WORKING_DIR} USES_TERMINAL - DEPENDS px4 + DEPENDS px4 OpticalFlowSystem ) else() - add_custom_target(gz_${model_only}_${world_name} - COMMAND ${CMAKE_COMMAND} -E env PX4_SIM_MODEL=gz_${model_only} PX4_GZ_WORLD=${world_name} $ + add_custom_target(gz_${model_name}_${world_name} + COMMAND ${CMAKE_COMMAND} -E env PX4_SIM_MODEL=gz_${model_name} PX4_GZ_WORLD=${world_name} $ WORKING_DIRECTORY ${SITL_WORKING_DIR} USES_TERMINAL - DEPENDS px4 + DEPENDS px4 OpticalFlowSystem ) endif() endforeach() endforeach() - # PX4_GZ_MODELS, PX4_GZ_WORLDS, GZ_SIM_RESOURCE_PATH + + # Setup the environment variables: PX4_GZ_MODELS, PX4_GZ_WORLDS, GZ_SIM_RESOURCE_PATH configure_file(gz_env.sh.in ${PX4_BINARY_DIR}/rootfs/gz_env.sh) endif() diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index bb2e7282da..c890ad8d5c 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -135,6 +135,14 @@ int GZBridge::init() return PX4_ERROR; } + std::string flow_topic = "/world/" + _world_name + "/model/" + _model_name + + "/link/flow_link/sensor/optical_flow/optical_flow"; + + if (!_node.Subscribe(flow_topic, &GZBridge::opticalFlowCallback, this)) { + PX4_ERR("failed to subscribe to %s", flow_topic.c_str()); + return PX4_ERROR; + } + if (!_mixing_interface_esc.init(_model_name)) { PX4_ERR("failed to init ESC output"); return PX4_ERROR; @@ -168,6 +176,40 @@ void GZBridge::clockCallback(const gz::msgs::Clock &msg) px4_clock_settime(CLOCK_MONOTONIC, &ts); } +void GZBridge::opticalFlowCallback(const px4::msgs::OpticalFlow &flow) +{ + sensor_optical_flow_s msg = {}; + + msg.timestamp = hrt_absolute_time(); + msg.timestamp_sample = flow.time_usec(); + msg.pixel_flow[0] = flow.integrated_x(); + msg.pixel_flow[1] = flow.integrated_y(); + msg.quality = flow.quality(); + msg.integration_timespan_us = flow.integration_time_us(); + + // Static data + device::Device::DeviceId id; + id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; + id.devid_s.bus = 0; + id.devid_s.address = 0; + id.devid_s.devtype = DRV_FLOW_DEVTYPE_SIM; + msg.device_id = id.devid; + + // values taken from PAW3902 + msg.mode = sensor_optical_flow_s::MODE_LOWLIGHT; + msg.max_flow_rate = 7.4f; + msg.min_ground_distance = 0.f; + msg.max_ground_distance = 30.f; + msg.error_count = 0; + + // No delta angle + // No distance + // This means that delta angle will come from vehicle gyro + // Distance will come from vehicle distance sensor + + _optical_flow_pub.publish(msg); +} + void GZBridge::barometerCallback(const gz::msgs::FluidPressure &msg) { const uint64_t timestamp = hrt_absolute_time(); diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 5e157f43b2..6128e8c175 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -57,6 +57,7 @@ #include #include #include +#include #include #include #include @@ -77,6 +78,8 @@ #include #include #include +// Custom PX4 proto +#include using namespace time_literals; @@ -113,6 +116,7 @@ private: void navSatCallback(const gz::msgs::NavSat &msg); void laserScantoLidarSensorCallback(const gz::msgs::LaserScan &msg); void laserScanCallback(const gz::msgs::LaserScan &msg); + void opticalFlowCallback(const px4::msgs::OpticalFlow &image_msg); static void rotateQuaternion(gz::math::Quaterniond &q_FRD_to_NED, const gz::math::Quaterniond q_FLU_to_ENU); @@ -128,12 +132,12 @@ private: uORB::Publication _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)}; uORB::Publication _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)}; uORB::Publication _lpos_ground_truth_pub{ORB_ID(vehicle_local_position_groundtruth)}; - uORB::PublicationMulti _sensor_gps_pub{ORB_ID(sensor_gps)}; uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; uORB::PublicationMulti _sensor_accel_pub{ORB_ID(sensor_accel)}; uORB::PublicationMulti _sensor_gyro_pub{ORB_ID(sensor_gyro)}; uORB::PublicationMulti _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; + uORB::PublicationMulti _optical_flow_pub{ORB_ID(sensor_optical_flow)}; GZMixingInterfaceESC _mixing_interface_esc{_node}; GZMixingInterfaceServo _mixing_interface_servo{_node}; diff --git a/src/modules/simulation/gz_bridge/Kconfig b/src/modules/simulation/gz_bridge/Kconfig index 0d4ab457d1..dd34a251b3 100644 --- a/src/modules/simulation/gz_bridge/Kconfig +++ b/src/modules/simulation/gz_bridge/Kconfig @@ -1,6 +1,6 @@ menuconfig MODULES_SIMULATION_GZ_BRIDGE bool "gz_bridge" default n - depends on PLATFORM_POSIX + depends on PLATFORM_POSIX && MODULES_SIMULATION_GZ_MSGS ---help--- Enable support for gz_bridge diff --git a/src/modules/simulation/gz_bridge/gz_env.sh.in b/src/modules/simulation/gz_bridge/gz_env.sh.in index 810bc88948..3267318bfc 100644 --- a/src/modules/simulation/gz_bridge/gz_env.sh.in +++ b/src/modules/simulation/gz_bridge/gz_env.sh.in @@ -2,5 +2,7 @@ export PX4_GZ_MODELS=@PX4_SOURCE_DIR@/Tools/simulation/gz/models export PX4_GZ_WORLDS=@PX4_SOURCE_DIR@/Tools/simulation/gz/worlds +export PX4_GZ_PLUGINS=@PX4_BINARY_DIR@/src/modules/simulation/gz_plugins export GZ_SIM_RESOURCE_PATH=$GZ_SIM_RESOURCE_PATH:$PX4_GZ_MODELS:$PX4_GZ_WORLDS +export GZ_SIM_SYSTEM_PLUGIN_PATH=$GZ_SIM_SYSTEM_PLUGIN_PATH:$PX4_GZ_PLUGINS diff --git a/src/modules/simulation/gz_msgs/CMakeLists.txt b/src/modules/simulation/gz_msgs/CMakeLists.txt new file mode 100644 index 0000000000..c74c010f91 --- /dev/null +++ b/src/modules/simulation/gz_msgs/CMakeLists.txt @@ -0,0 +1,55 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +############################################################################ + +# message(FATAL_ERROR "JAKE JAKE JAKE JAKE JAKE JAKE JAKE JAKE JAKE JAKE JAKE JAKE JAKE JAKE JAKE JAKE JAKE JAKE ") + +# Check for Gazebo first +if(NOT DEFINED ENV{GZ_DISTRO}) + set(GZ_DISTRO "harmonic" CACHE STRING "Gazebo distribution to use") +else() + set(GZ_DISTRO $ENV{GZ_DISTRO}) +endif() + +# Set versions based on distribution +if(GZ_DISTRO STREQUAL "harmonic") + set(GZ_CMAKE_VERSION "3") + set(GZ_MSGS_VERSION "10") + set(GZ_TRANSPORT_VERSION "13") +elseif(GZ_DISTRO STREQUAL "ionic") + set(GZ_CMAKE_VERSION "4") + set(GZ_MSGS_VERSION "11") + set(GZ_TRANSPORT_VERSION "14") +else() + message(FATAL_ERROR "Unknown Gazebo distribution: ${GZ_DISTRO}") +endif() + +# Find required packages +find_package(gz-transport${GZ_TRANSPORT_VERSION}) +if(gz-transport${GZ_TRANSPORT_VERSION}_FOUND) + find_package(gz-cmake${GZ_CMAKE_VERSION} REQUIRED) + find_package(gz-msgs${GZ_MSGS_VERSION} REQUIRED) + find_package(Protobuf REQUIRED) + + # Generate protobuf messages + file(GLOB MSGS_PROTOS "${CMAKE_CURRENT_SOURCE_DIR}/*.proto") + PROTOBUF_GENERATE_CPP(PROTO_SRCS PROTO_HDRS ${MSGS_PROTOS}) + + # Create library + add_library(px4_gz_msgs STATIC + ${PROTO_SRCS} + ${PROTO_HDRS} + ) + + target_include_directories(px4_gz_msgs + PUBLIC + ${CMAKE_CURRENT_BINARY_DIR} + ${Protobuf_INCLUDE_DIRS} + ) + target_link_libraries(px4_gz_msgs PUBLIC ${PROTOBUF_LIBRARIES}) + + # Export the binary dir for other modules + set(PX4_GZ_MSGS_BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR} CACHE INTERNAL "") +endif() diff --git a/src/modules/simulation/gz_msgs/Kconfig b/src/modules/simulation/gz_msgs/Kconfig new file mode 100644 index 0000000000..65b2a6453b --- /dev/null +++ b/src/modules/simulation/gz_msgs/Kconfig @@ -0,0 +1,6 @@ +menuconfig MODULES_SIMULATION_GZ_MSGS + bool "gz_msgs" + default n + depends on PLATFORM_POSIX + ---help--- + Enable proto generation for custom PX4 messages diff --git a/src/modules/simulation/gz_msgs/opticalflow.proto b/src/modules/simulation/gz_msgs/opticalflow.proto new file mode 100644 index 0000000000..c03a96b9a8 --- /dev/null +++ b/src/modules/simulation/gz_msgs/opticalflow.proto @@ -0,0 +1,19 @@ +syntax = "proto3"; +package px4.msgs; + +message OpticalFlow { + // Timestamp (microseconds, since system start) + int64 time_usec = 1; + + // Integration time + uint32 integration_time_us = 2; + + // Integrated x-axis flow (rad) + float integrated_x = 3; + + // Integrated y-axis flow (rad) + float integrated_y = 4; + + // Quality of optical flow measurement (0: bad, 255: maximum quality) + float quality = 5; +} diff --git a/src/modules/simulation/gz_plugins/CMakeLists.txt b/src/modules/simulation/gz_plugins/CMakeLists.txt new file mode 100644 index 0000000000..c4400b0204 --- /dev/null +++ b/src/modules/simulation/gz_plugins/CMakeLists.txt @@ -0,0 +1,70 @@ +project(OpticalFlowSystem) + +if(NOT DEFINED ENV{GZ_DISTRO}) + set(GZ_DISTRO "harmonic" CACHE STRING "Gazebo distribution to use") +else() + set(GZ_DISTRO $ENV{GZ_DISTRO}) +endif() + +# Define library version combinations for different Gazebo distributions +# https://github.com/gazebo-tooling/gazebodistro/blob/master/collection-harmonic.yaml +if(GZ_DISTRO STREQUAL "harmonic") + set(GZ_CMAKE_VERSION "3") + set(GZ_MSGS_VERSION "10") + set(GZ_TRANSPORT_VERSION "13") + set(GZ_PLUGIN_VERSION "2") + set(GZ_SIM_VERSION "8") + set(GZ_SENSORS_VERSION "8") + message(STATUS "Using Gazebo Harmonic (cmake:${GZ_CMAKE_VERSION}, msgs:${GZ_MSGS_VERSION}, transport:${GZ_TRANSPORT_VERSION})") +elseif(GZ_DISTRO STREQUAL "ionic") + set(GZ_CMAKE_VERSION "4") + set(GZ_MSGS_VERSION "11") + set(GZ_TRANSPORT_VERSION "14") + set(GZ_PLUGIN_VERSION "3") + set(GZ_SIM_VERSION "9") + set(GZ_SENSORS_VERSION "9") + message(STATUS "Using Gazebo Ionic (cmake:${GZ_CMAKE_VERSION}, msgs:${GZ_MSGS_VERSION}, transport:${GZ_TRANSPORT_VERSION})") +else() + message(FATAL_ERROR "Unknown Gazebo distribution: ${GZ_DISTRO}. Valid options are: harmonic or ionic") +endif() + +# Use gz-transport as litmus test for prescence of gz +find_package(gz-transport${GZ_TRANSPORT_VERSION}) + +if (gz-transport${GZ_TRANSPORT_VERSION}_FOUND) + + gz_find_package(gz-cmake${GZ_CMAKE_VERSION} REQUIRED) + gz_find_package(gz-msgs${GZ_MSGS_VERSION} REQUIRED) + gz_find_package(Protobuf REQUIRED) + gz_find_package(gz-plugin${GZ_PLUGIN_VERSION} REQUIRED COMPONENTS register) + gz_find_package(gz-sim${GZ_SIM_VERSION} REQUIRED) + gz_find_package(gz-sensors${GZ_SENSORS_VERSION} REQUIRED) + + include(${CMAKE_CURRENT_SOURCE_DIR}/optical_flow.cmake) + + add_library(${PROJECT_NAME} SHARED + OpticalFlowSensor.cpp + OpticalFlowSystem.cpp + ) + + target_link_libraries(${PROJECT_NAME} + PUBLIC px4_gz_msgs + PUBLIC gz-sensors${GZ_SENSORS_VERSION}::gz-sensors${GZ_SENSORS_VERSION} + PUBLIC gz-plugin${GZ_PLUGIN_VERSION}::gz-plugin${GZ_PLUGIN_VERSION} + PUBLIC gz-sim${GZ_SIM_VERSION}::gz-sim${GZ_SIM_VERSION} + PUBLIC gz-transport${GZ_TRANSPORT_VERSION}::gz-transport${GZ_TRANSPORT_VERSION} + PUBLIC ${OpenCV_LIBS} + PUBLIC ${OpticalFlow_LIBS} + ) + + target_include_directories(${PROJECT_NAME} + PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} + PUBLIC ${CMAKE_CURRENT_BINARY_DIR} + PUBLIC ${OpenCV_INCLUDE_DIRS} + PUBLIC ${OpticalFlow_INCLUDE_DIRS} + PUBLIC px4_gz_msgs + ) + + add_dependencies(${PROJECT_NAME} OpticalFlow) + +endif() diff --git a/src/modules/simulation/gz_plugins/Kconfig b/src/modules/simulation/gz_plugins/Kconfig new file mode 100644 index 0000000000..eae335fbc5 --- /dev/null +++ b/src/modules/simulation/gz_plugins/Kconfig @@ -0,0 +1,6 @@ +menuconfig MODULES_SIMULATION_GZ_PLUGINS + bool "gz_plugins" + default n + depends on PLATFORM_POSIX && MODULES_SIMULATION_GZ_MSGS + ---help--- + Enable support for gz_plugins diff --git a/src/modules/simulation/gz_plugins/OpticalFlowSensor.cpp b/src/modules/simulation/gz_plugins/OpticalFlowSensor.cpp new file mode 100644 index 0000000000..4c0dac72d7 --- /dev/null +++ b/src/modules/simulation/gz_plugins/OpticalFlowSensor.cpp @@ -0,0 +1,165 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include + +#include "OpticalFlowSensor.hpp" +#include "opticalflow.pb.h" + +using namespace custom; + +bool OpticalFlowSensor::Load(const sdf::Sensor &_sdf) +{ + auto type = gz::sensors::customType(_sdf); + + if ("optical_flow" != type) { + gzerr << "Trying to load [optical_flow] sensor, but got type [" << type << "] instead." << std::endl; + return false; + } + + gz::sensors::Sensor::Load(_sdf); + + std::string output_topic = this->Topic(); + _publisher = _node.Advertise(output_topic); + gzdbg << "Advertising optical flow data on: " << output_topic << std::endl; + + std::string camera_topic = output_topic; + size_t last_segment = camera_topic.rfind("/optical_flow/optical_flow"); + + if (last_segment != std::string::npos) { + camera_topic = camera_topic.substr(0, last_segment) + "/flow_camera/image"; + } + + int image_width = 0; + int image_height = 0; + int update_rate = 0; + float hfov = 0; + + auto sensorElem = _sdf.Element()->GetParent()->GetElement("sensor"); + + while (sensorElem) { + if (sensorElem->Get("name") == "flow_camera") { + auto cameraElem = sensorElem->GetElement("camera"); + update_rate = sensorElem->GetElement("update_rate")->Get(); + hfov = cameraElem->GetElement("horizontal_fov")->Get(); + + auto imageElem = cameraElem->GetElement("image"); + image_width = imageElem->GetElement("width")->Get(); + image_height = imageElem->GetElement("height")->Get(); + break; + } + + sensorElem = sensorElem->GetNextElement("sensor"); + } + + gzdbg << "Camera parameters:" << std::endl + << " image_width: " << image_width << std::endl + << " image_height: " << image_height << std::endl + << " update_rate: " << update_rate << std::endl + << " hfov: " << hfov << std::endl; + + gzdbg << "Subscribing to camera topic for flow: " << camera_topic << std::endl; + + if (!_node.Subscribe(camera_topic, &OpticalFlowSensor::OnImage, this)) { + gzerr << "Failed to subscribe to camera topic: " << camera_topic << std::endl; + return false; + } + + // Assume pinhole camera and 1:1 aspect ratio + float focal_length = (image_width / 2.0f) / tan(hfov / 2.0f); + _optical_flow = std::make_shared(focal_length, focal_length, + update_rate, image_width, image_height); + + return true; +} + +void OpticalFlowSensor::OnImage(const gz::msgs::Image &image_msg) +{ + if (image_msg.width() == 0 || image_msg.height() == 0) { + gzerr << "Invalid image dimensions" << std::endl; + return; + } + + if (image_msg.pixel_format_type() == gz::msgs::PixelFormatType::RGB_INT8) { + cv::Mat temp(image_msg.height(), image_msg.width(), CV_8UC3); + std::memcpy(temp.data, image_msg.data().c_str(), image_msg.data().size()); + cv::cvtColor(temp, _last_image_gray, cv::COLOR_RGB2GRAY); + + } else if (image_msg.pixel_format_type() == gz::msgs::PixelFormatType::L_INT8) { + std::memcpy(_last_image_gray.data, image_msg.data().c_str(), image_msg.data().size()); + + } else { + gzerr << "Unsupported image format" << std::endl; + return; + } + + uint32_t current_timestamp = (image_msg.header().stamp().sec() * 1000000ULL + + image_msg.header().stamp().nsec() / 1000ULL) & 0xFFFFFFFF; + + if (_last_image_timestamp != 0) { + _integration_time_us = (current_timestamp - _last_image_timestamp) & 0xFFFFFFFF; + } + + _last_image_timestamp = current_timestamp; + _new_image_available = true; +} + +bool OpticalFlowSensor::Update(const std::chrono::steady_clock::duration &_now) +{ + if (!_new_image_available) { + return true; + } + + px4::msgs::OpticalFlow msg; + msg.set_time_usec(_last_image_timestamp); + + float flow_x = 0.f; + float flow_y = 0.f; + + int quality = _optical_flow->calcFlow(_last_image_gray.data, _last_image_timestamp, + _integration_time_us, flow_x, flow_y); + + msg.set_integrated_x(flow_x); + msg.set_integrated_y(flow_y); + msg.set_integration_time_us(_integration_time_us); + msg.set_quality(quality); + + if (!_publisher.Publish(msg)) { + gzwarn << "Failed to publish optical flow message" << std::endl; + } + + _new_image_available = false; + return true; +} diff --git a/src/modules/simulation/gz_plugins/OpticalFlowSensor.hpp b/src/modules/simulation/gz_plugins/OpticalFlowSensor.hpp new file mode 100644 index 0000000000..497fd79276 --- /dev/null +++ b/src/modules/simulation/gz_plugins/OpticalFlowSensor.hpp @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "flow_opencv.hpp" + +namespace custom +{ +class OpticalFlowSensor : public gz::sensors::Sensor +{ +public: + virtual bool Load(const sdf::Sensor &_sdf) override; + virtual bool Update(const std::chrono::steady_clock::duration &_now) override; + +private: + void OnImage(const gz::msgs::Image &_msg); + + gz::transport::Node _node; + gz::transport::Node::Publisher _publisher; + + // Flow + std::shared_ptr _optical_flow {nullptr}; + int _integration_time_us; + + // Camera + double _horizontal_fov {0.0}; + double _vertical_fov {0.0}; + + cv::Mat _last_image_gray; + uint32_t _last_image_timestamp {0}; + bool _new_image_available {false}; +}; + +} // end namespace custom diff --git a/src/modules/simulation/gz_plugins/OpticalFlowSystem.cpp b/src/modules/simulation/gz_plugins/OpticalFlowSystem.cpp new file mode 100644 index 0000000000..3459f8a386 --- /dev/null +++ b/src/modules/simulation/gz_plugins/OpticalFlowSystem.cpp @@ -0,0 +1,125 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "OpticalFlowSensor.hpp" +#include "OpticalFlowSystem.hpp" + +using namespace custom; + +void OpticalFlowSystem::PreUpdate(const gz::sim::UpdateInfo &, gz::sim::EntityComponentManager &_ecm) +{ + // Register each new custom sensor + _ecm.EachNew( + [&](const gz::sim::Entity & _entity, + const gz::sim::components::CustomSensor * _custom, + const gz::sim::components::ParentEntity * _parent)->bool { + auto sensorScopedName = gz::sim::removeParentScope(gz::sim::scopedName(_entity, _ecm, "::", false), "::"); + + sdf::Sensor data = _custom->Data(); + data.SetName(sensorScopedName); + + if (data.Topic().empty()) + { + std::string topic = scopedName(_entity, _ecm) + "/optical_flow"; + data.SetTopic(topic); + } + + gz::sensors::SensorFactory sensorFactory; + auto sensor = sensorFactory.CreateSensor(data); + + if (sensor == nullptr) + { + gzerr << "Failed to create optical flow sensor [" << sensorScopedName << "]" << std::endl; + return false; + } + + auto parentName = _ecm.Component(_parent->Data())->Data(); + + sensor->SetParent(parentName); + + _ecm.CreateComponent(_entity, gz::sim::components::SensorTopic(sensor->Topic())); + + this->entitySensorMap.insert(std::make_pair(_entity, std::move(sensor))); + + gzdbg << "OpticalFlowSystem PreUpdate" << std::endl; + + return true; + }); +} + +void OpticalFlowSystem::PostUpdate(const gz::sim::UpdateInfo &_info, const gz::sim::EntityComponentManager &_ecm) +{ + if (!_info.paused) { + for (auto &[entity, sensor] : this->entitySensorMap) { + sensor->Update(_info.simTime); + } + } + + this->RemoveSensorEntities(_ecm); +} + +void OpticalFlowSystem::RemoveSensorEntities(const gz::sim::EntityComponentManager &_ecm) +{ + _ecm.EachRemoved( + [&](const gz::sim::Entity & _entity, + const gz::sim::components::CustomSensor *)->bool { + if (this->entitySensorMap.erase(_entity) == 0) + { + gzerr << "Internal error, missing optical flow sensor for entity [" + << _entity << "]" << std::endl; + } + return true; + }); +} + +GZ_ADD_PLUGIN(OpticalFlowSystem, gz::sim::System, + OpticalFlowSystem::ISystemPreUpdate, + OpticalFlowSystem::ISystemPostUpdate + ) + +GZ_ADD_PLUGIN_ALIAS(OpticalFlowSystem, "custom::OpticalFlowSystem") diff --git a/src/modules/simulation/gz_plugins/OpticalFlowSystem.hpp b/src/modules/simulation/gz_plugins/OpticalFlowSystem.hpp new file mode 100644 index 0000000000..eecaaa2f6f --- /dev/null +++ b/src/modules/simulation/gz_plugins/OpticalFlowSystem.hpp @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include + +namespace custom +{ +class OpticalFlowSystem: + public gz::sim::System, + public gz::sim::ISystemPreUpdate, + public gz::sim::ISystemPostUpdate +{ +public: + void PreUpdate(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) final; + + void PostUpdate(const gz::sim::UpdateInfo &_info, + const gz::sim::EntityComponentManager &_ecm) final; + +private: + void RemoveSensorEntities(const gz::sim::EntityComponentManager &_ecm); + + std::unordered_map> entitySensorMap; +}; +} // end namespace custom diff --git a/src/modules/simulation/gz_plugins/optical_flow.cmake b/src/modules/simulation/gz_plugins/optical_flow.cmake new file mode 100644 index 0000000000..8ac1c1f649 --- /dev/null +++ b/src/modules/simulation/gz_plugins/optical_flow.cmake @@ -0,0 +1,53 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +include(ExternalProject) +find_package(OpenCV REQUIRED) + +if(NOT TARGET OpticalFlow) + ExternalProject_Add(OpticalFlow + GIT_REPOSITORY https://github.com/PX4/PX4-OpticalFlow.git + GIT_TAG master + PREFIX ${CMAKE_BINARY_DIR}/OpticalFlow + INSTALL_DIR ${CMAKE_BINARY_DIR}/OpticalFlow/install + CMAKE_ARGS -DCMAKE_INSTALL_PREFIX= + BUILD_BYPRODUCTS ${CMAKE_BINARY_DIR}/OpticalFlow/install/lib/libOpticalFlow.so + UPDATE_DISCONNECTED ON + BUILD_ALWAYS OFF + STEP_TARGETS build + ) + + ExternalProject_Get_Property(OpticalFlow install_dir) + set(OpticalFlow_INCLUDE_DIRS ${install_dir}/include CACHE INTERNAL "") + set(OpticalFlow_LIBS ${install_dir}/lib/libOpticalFlow.so CACHE INTERNAL "") +endif() From 9198125ec599869e6ceb986ec08ea09e55e0a9b9 Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Mon, 3 Mar 2025 21:33:02 +0000 Subject: [PATCH 79/91] Remove reboot_required from IMU_GYRO_* parameters (#24435) * fix: IMU_GYRO_* parameters do not requires reboot Signed-off-by: Beniamino Pozzan * restore IMU_GYRO_RATEMAX reboot_required to true --------- Signed-off-by: Beniamino Pozzan --- .../vehicle_angular_velocity/imu_gyro_parameters.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c b/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c index cb5f320bc3..cfd6fb8cf6 100644 --- a/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c +++ b/src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c @@ -45,7 +45,7 @@ * @min 0 * @max 1000 * @unit Hz -* @reboot_required true +* @reboot_required false * @group Sensors */ PARAM_DEFINE_FLOAT(IMU_GYRO_NF0_FRQ, 0.0f); @@ -60,7 +60,7 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_NF0_FRQ, 0.0f); * @min 0 * @max 100 * @unit Hz -* @reboot_required true +* @reboot_required false * @group Sensors */ PARAM_DEFINE_FLOAT(IMU_GYRO_NF0_BW, 20.0f); @@ -79,7 +79,7 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_NF0_BW, 20.0f); * @min 0 * @max 1000 * @unit Hz -* @reboot_required true +* @reboot_required false * @group Sensors */ PARAM_DEFINE_FLOAT(IMU_GYRO_NF1_FRQ, 0.0f); @@ -94,7 +94,7 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_NF1_FRQ, 0.0f); * @min 0 * @max 100 * @unit Hz -* @reboot_required true +* @reboot_required false * @group Sensors */ PARAM_DEFINE_FLOAT(IMU_GYRO_NF1_BW, 20.0f); @@ -112,7 +112,7 @@ PARAM_DEFINE_FLOAT(IMU_GYRO_NF1_BW, 20.0f); * @min 0 * @max 1000 * @unit Hz -* @reboot_required true +* @reboot_required false * @group Sensors */ PARAM_DEFINE_FLOAT(IMU_GYRO_CUTOFF, 40.0f); @@ -154,7 +154,7 @@ PARAM_DEFINE_INT32(IMU_GYRO_RATEMAX, 400); * @min 0 * @max 1000 * @unit Hz -* @reboot_required true +* @reboot_required false * @group Sensors */ PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 30.0f); From 38a794260caf25dc75488acf9486c6c121466524 Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Mon, 3 Mar 2025 15:11:28 -0800 Subject: [PATCH 80/91] voxl2-slpi: Updated ghst_parse call in RC driver to match the new function signature --- boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.cpp b/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.cpp index e213179922..9ad45c1389 100644 --- a/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.cpp +++ b/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.cpp @@ -207,15 +207,15 @@ void GhstRc::Run() if (new_bytes > 0) { _bytes_rx += new_bytes; - int8_t ghst_rssi = -1; - bool rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], new_bytes, &_raw_rc_values[0], &ghst_rssi, + ghstLinkStatistics_t link_stats = { .rssi_pct = -1, .rssi_dbm = NAN, .link_quality = 0 }; + bool rc_updated = ghst_parse(cycle_timestamp, &_rcs_buf[0], new_bytes, &_raw_rc_values[0], &link_stats, &_raw_rc_count, GHST_MAX_NUM_CHANNELS); if (rc_updated) { _last_packet_seen = time_now_us; // we have a new GHST frame. Publish it. _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST; - fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0, ghst_rssi); + fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0, link_stats.rssi_pct); // ghst telemetry works on fmu-v5 // on other Pixhawk (-related) boards we cannot write to the RC UART From 71d514d359d80d3406d53bb93c5fe1dcd591383f Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 3 Mar 2025 14:47:06 +0100 Subject: [PATCH 81/91] replay: avoid recursion when adding subscriptions readAndAddSubscription uses nextDataMessage to find the first corresponding data and nextDataMessage calls readAndAddSubscription when it finds a new message definition. --- src/modules/replay/Replay.cpp | 30 +++++++++++++++++------------- 1 file changed, 17 insertions(+), 13 deletions(-) diff --git a/src/modules/replay/Replay.cpp b/src/modules/replay/Replay.cpp index a49c005bb5..8329477649 100644 --- a/src/modules/replay/Replay.cpp +++ b/src/modules/replay/Replay.cpp @@ -721,10 +721,6 @@ Replay::nextDataMessage(std::ifstream &file, Subscription &subscription, int msg } switch (message_header.msg_type) { - case (int)ULogMessageType::ADD_LOGGED_MSG: - readAndAddSubscription(file, message_header.msg_size); - break; - case (int)ULogMessageType::DATA: file.read((char *)&file_msg_id, sizeof(file_msg_id)); @@ -751,6 +747,7 @@ Replay::nextDataMessage(std::ifstream &file, Subscription &subscription, int msg break; case (int)ULogMessageType::REMOVE_LOGGED_MSG: //skip these + case (int)ULogMessageType::ADD_LOGGED_MSG: case (int)ULogMessageType::PARAMETER: case (int)ULogMessageType::DROPOUT: case (int)ULogMessageType::INFO: @@ -907,19 +904,26 @@ Replay::run() ulog_message_header_s message_header; replay_file.seekg(_data_section_start); - //we know the next message must be an ADD_LOGGED_MSG - ReadAndAndAddSubResult res; - - do { + while (true) { + //we are in the Definition & Data Section Message Header section replay_file.read((char *)&message_header, ULOG_MSG_HEADER_LEN); - res = readAndAddSubscription(replay_file, message_header.msg_size); - if (res == ReadAndAndAddSubResult::kFailure) { - PX4_ERR("Failed to read subscription"); - return; + if (!replay_file) { + break; } - } while (res != ReadAndAndAddSubResult::kSuccess); + if (message_header.msg_type == (int)ULogMessageType::ADD_LOGGED_MSG) { + readAndAddSubscription(replay_file, message_header.msg_size); + + } else if (message_header.msg_type == (int)ULogMessageType::DATA) { + // End of Definition & Data Section Message Header section + break; + + } else { + // Not important for now, skip + replay_file.seekg(message_header.msg_size, ios::cur); + } + } const uint64_t timestamp_offset = getTimestampOffset(); uint32_t nr_published_messages = 0; From 5c7143a33bf8059d3c1f953aeabc9c7dd4d235f1 Mon Sep 17 00:00:00 2001 From: Alex Klimaj Date: Tue, 4 Mar 2025 05:05:54 -0500 Subject: [PATCH 82/91] uavcannode rangefinder: add tolerance to reading too close check (#24415) --- src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp b/src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp index 2ad053d9a4..134caea21a 100644 --- a/src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp +++ b/src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp @@ -100,10 +100,12 @@ public: } // reading_type + const float tolerance = 1e-6; + if (dist.current_distance > dist.max_distance) { range_sensor.reading_type = uavcan::equipment::range_sensor::Measurement::READING_TYPE_TOO_FAR; - } else if (dist.current_distance < dist.min_distance) { + } else if (dist.current_distance < dist.min_distance - tolerance) { range_sensor.reading_type = uavcan::equipment::range_sensor::Measurement::READING_TYPE_TOO_CLOSE; } else if (dist.signal_quality != 0) { From 49a84f38a27250fe62a8c6b51f2be4e7e4b3e8a4 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 19 Feb 2025 13:42:42 +0100 Subject: [PATCH 83/91] VehicleStatus.msg: remove unused VEHICLE_TYPE_AIRSHIP Signed-off-by: Silvan Fuhrer --- msg/versioned/VehicleStatus.msg | 1 - 1 file changed, 1 deletion(-) diff --git a/msg/versioned/VehicleStatus.msg b/msg/versioned/VehicleStatus.msg index b9a3edd91f..cf395d8501 100644 --- a/msg/versioned/VehicleStatus.msg +++ b/msg/versioned/VehicleStatus.msg @@ -93,7 +93,6 @@ uint8 VEHICLE_TYPE_UNKNOWN = 0 uint8 VEHICLE_TYPE_ROTARY_WING = 1 uint8 VEHICLE_TYPE_FIXED_WING = 2 uint8 VEHICLE_TYPE_ROVER = 3 -uint8 VEHICLE_TYPE_AIRSHIP = 4 uint8 FAILSAFE_DEFER_STATE_DISABLED = 0 uint8 FAILSAFE_DEFER_STATE_ENABLED = 1 From 7cb6464cfb0207d8f1dafb99df16c22c7793d3e9 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 19 Feb 2025 14:04:53 +0100 Subject: [PATCH 84/91] VehicleStatus.msg: remove VEHICLE_TYPE_UNKNOWN Signed-off-by: Silvan Fuhrer --- msg/versioned/VehicleStatus.msg | 7 +++---- .../lightware_laser_i2c/lightware_laser_i2c.cpp | 2 +- src/lib/rtl/rtl_time_estimator.h | 2 +- src/modules/commander/Commander.cpp | 2 +- 4 files changed, 6 insertions(+), 7 deletions(-) diff --git a/msg/versioned/VehicleStatus.msg b/msg/versioned/VehicleStatus.msg index cf395d8501..8a17a60cd7 100644 --- a/msg/versioned/VehicleStatus.msg +++ b/msg/versioned/VehicleStatus.msg @@ -89,10 +89,9 @@ uint8 HIL_STATE_ON = 1 # If it's a VTOL, then the value will be VEHICLE_TYPE_ROTARY_WING while flying as a multicopter, and VEHICLE_TYPE_FIXED_WING when flying as a fixed-wing uint8 vehicle_type -uint8 VEHICLE_TYPE_UNKNOWN = 0 -uint8 VEHICLE_TYPE_ROTARY_WING = 1 -uint8 VEHICLE_TYPE_FIXED_WING = 2 -uint8 VEHICLE_TYPE_ROVER = 3 +uint8 VEHICLE_TYPE_ROTARY_WING = 0 +uint8 VEHICLE_TYPE_FIXED_WING = 1 +uint8 VEHICLE_TYPE_ROVER = 2 uint8 FAILSAFE_DEFER_STATE_DISABLED = 0 uint8 FAILSAFE_DEFER_STATE_ENABLED = 1 diff --git a/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp b/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp index 614d408a5e..825cbdf97f 100644 --- a/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp +++ b/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp @@ -143,7 +143,7 @@ private: ) uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - typeof(px4::msg::VehicleStatus::vehicle_type) _vehicle_type{px4::msg::VehicleStatus::VEHICLE_TYPE_UNKNOWN}; + typeof(px4::msg::VehicleStatus::vehicle_type) _vehicle_type{px4::msg::VehicleStatus::VEHICLE_TYPE_ROTARY_WING}; uORB::Subscription _dist_sense_mode_change_sub{ORB_ID(distance_sensor_mode_change_request)}; typeof(px4::msg::DistanceSensorModeChangeRequest::request_on_off) _req_mode{px4::msg::DistanceSensorModeChangeRequest::REQUEST_OFF}; bool _restriction{false}; diff --git a/src/lib/rtl/rtl_time_estimator.h b/src/lib/rtl/rtl_time_estimator.h index 8c8519cd9b..7ca79fba1f 100644 --- a/src/lib/rtl/rtl_time_estimator.h +++ b/src/lib/rtl/rtl_time_estimator.h @@ -116,7 +116,7 @@ private: float _time_estimate{0.f}; /**< Accumulated time estimate [s] */ bool _is_valid{false}; /**< Checks if time estimate is valid */ - uint8_t _vehicle_type{vehicle_status_s::VEHICLE_TYPE_UNKNOWN}; /**< the defined vehicle type to use for the calculation*/ + uint8_t _vehicle_type{vehicle_status_s::VEHICLE_TYPE_ROTARY_WING}; /**< the defined vehicle type to use for the calculation*/ DEFINE_PARAMETERS( (ParamFloat) _param_rtl_time_factor, /**< Safety factory for safe time estimate */ diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 2025d0a26c..ff13f277a4 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -688,7 +688,7 @@ Commander::Commander() : _vehicle_status.system_id = 1; _vehicle_status.component_id = 1; _vehicle_status.system_type = 0; - _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNKNOWN; + _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; _vehicle_status.nav_state = _user_mode_intention.get(); _vehicle_status.nav_state_user_intention = _user_mode_intention.get(); _vehicle_status.nav_state_timestamp = hrt_absolute_time(); From d857a278ff4e8e702245f1c693ebe9ef36111a06 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 19 Feb 2025 14:06:07 +0100 Subject: [PATCH 85/91] Commander: use is_ground_vehicle() consistently instead of checking vehicle_type Signed-off-by: Silvan Fuhrer --- src/modules/commander/Commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index ff13f277a4..cebc08ae94 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -574,7 +574,7 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_ if (!_vehicle_control_mode.flag_control_climb_rate_enabled && !_failsafe_flags.manual_control_signal_lost && !_is_throttle_low - && _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROVER) { + && !is_ground_vehicle(_vehicle_status)) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied: high throttle\t"); events::send(events::ID("commander_arm_denied_throttle_high"), {events::Log::Critical, events::LogInternal::Info}, From a127a8293abb1982fd46eda3ee82033930b8f20a Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 19 Feb 2025 14:06:48 +0100 Subject: [PATCH 86/91] VehicleStatus.msg: make clear that vehicle_status should refer to current locomotion method Signed-off-by: Silvan Fuhrer --- msg/versioned/VehicleStatus.msg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/msg/versioned/VehicleStatus.msg b/msg/versioned/VehicleStatus.msg index 8a17a60cd7..81bb3ba1c0 100644 --- a/msg/versioned/VehicleStatus.msg +++ b/msg/versioned/VehicleStatus.msg @@ -87,7 +87,7 @@ uint8 hil_state uint8 HIL_STATE_OFF = 0 uint8 HIL_STATE_ON = 1 -# If it's a VTOL, then the value will be VEHICLE_TYPE_ROTARY_WING while flying as a multicopter, and VEHICLE_TYPE_FIXED_WING when flying as a fixed-wing +# Current vehicle locomotion method. A vehicle can have different methods (e.g. VTOL transitions from RW to FW method) uint8 vehicle_type uint8 VEHICLE_TYPE_ROTARY_WING = 0 uint8 VEHICLE_TYPE_FIXED_WING = 1 From d1e41988643c5ae8b5026015a39f6d2c2f72e675 Mon Sep 17 00:00:00 2001 From: chfriedrich98 <125505139+chfriedrich98@users.noreply.github.com> Date: Tue, 4 Mar 2025 17:28:09 +0100 Subject: [PATCH 87/91] rover_control: migrate params from .yaml to .c file (#24445) * rover_control: migrate params from .yaml to .c file * Update src/lib/rover_control/rovercontrol_params.c --------- Co-authored-by: Silvan Fuhrer --- src/lib/rover_control/module.yaml | 207 ---------------- src/lib/rover_control/rovercontrol_params.c | 255 ++++++++++++++++++++ src/modules/rover_ackermann/CMakeLists.txt | 2 - 3 files changed, 255 insertions(+), 209 deletions(-) delete mode 100644 src/lib/rover_control/module.yaml create mode 100644 src/lib/rover_control/rovercontrol_params.c diff --git a/src/lib/rover_control/module.yaml b/src/lib/rover_control/module.yaml deleted file mode 100644 index a786a32c4b..0000000000 --- a/src/lib/rover_control/module.yaml +++ /dev/null @@ -1,207 +0,0 @@ -module_name: Rover Control - -parameters: - - group: Rover Control - definitions: - RO_MAX_THR_SPEED: - description: - short: Speed the rover drives at maximum throttle - long: Used to linearly map speeds [m/s] to throttle values [-1. 1]. - type: float - unit: m/s - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 0 - - RO_ACCEL_LIM: - description: - short: Acceleration limit - long: | - Set to -1 to disable. - For mecanum rovers this limit is used for longitudinal and lateral acceleration. - type: float - unit: m/s^2 - min: -1 - max: 100 - increment: 0.01 - decimal: 2 - default: -1 - - RO_DECEL_LIM: - description: - short: Deceleration limit - long: | - Set to -1 to disable. - Note that if it is disabled the rover will not slow down when approaching waypoints in auto modes. - For mecanum rovers this limit is used for longitudinal and lateral deceleration. - type: float - unit: m/s^2 - min: -1 - max: 100 - increment: 0.01 - decimal: 2 - default: -1 - - RO_JERK_LIM: - description: - short: Jerk limit - long: | - Set to -1 to disable. - Note that if it is disabled the rover will not slow down when approaching waypoints in auto modes. - For mecanum rovers this limit is used for longitudinal and lateral jerk. - type: float - unit: m/s^3 - min: -1 - max: 100 - increment: 0.01 - decimal: 2 - default: -1 - - RO_YAW_RATE_TH: - description: - short: Yaw rate measurement threshold - long: The minimum threshold for the yaw rate measurement not to be interpreted as zero. - type: float - unit: deg/s - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 3 - - RO_SPEED_TH: - description: - short: Speed measurement threshold - long: The minimum threshold for the speed measurement not to be interpreted as zero. - type: float - unit: m/s - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 0.1 - - RO_YAW_STICK_DZ: - description: - short: Yaw stick deadzone - long: Percentage of stick input range that will be interpreted as zero around the stick centered value. - type: float - min: 0 - max: 1 - increment: 0.01 - decimal: 2 - default: 0.1 - - - group: Rover Rate Control - definitions: - RO_YAW_RATE_P: - description: - short: Proportional gain for closed loop yaw rate controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 0 - - RO_YAW_RATE_I: - description: - short: Integral gain for closed loop yaw rate controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 0 - - RO_YAW_RATE_LIM: - description: - short: Yaw rate limit - long: | - Used to cap yaw rate setpoints and map controller inputs to yaw rate setpoints - in Acro, Stabilized and Position mode. - type: float - unit: deg/s - min: 0 - max: 10000 - increment: 0.01 - decimal: 2 - default: 0 - - RO_YAW_ACCEL_LIM: - description: - short: Yaw acceleration limit - long: | - Used to cap how quickly the magnitude of yaw rate setpoints can increase. - Set to -1 to disable. - type: float - unit: deg/s^2 - min: -1 - max: 10000 - increment: 0.01 - decimal: 2 - default: -1 - - RO_YAW_DECEL_LIM: - description: - short: Yaw deceleration limit - long: | - Used to cap how quickly the magnitude of yaw rate setpoints can decrease. - Set to -1 to disable. - type: float - unit: deg/s^2 - min: -1 - max: 10000 - increment: 0.01 - decimal: 2 - default: -1 - - - group: Rover Attitude Control - definitions: - RO_YAW_P: - description: - short: Proportional gain for closed loop yaw controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 3 - default: 0 - - - group: Rover Velocity Control - definitions: - RO_SPEED_P: - description: - short: Proportional gain for ground speed controller - type: float - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 0 - - RO_SPEED_I: - description: - short: Integral gain for ground speed controller - type: float - min: 0 - max: 100 - increment: 0.001 - decimal: 3 - default: 0 - - RO_SPEED_LIM: - description: - short: Speed limit - long: | - Used to cap speed setpoints and map controller inputs to speed setpoints - in Position mode. - type: float - unit: m/s - min: -1 - max: 100 - increment: 0.01 - decimal: 2 - default: -1 diff --git a/src/lib/rover_control/rovercontrol_params.c b/src/lib/rover_control/rovercontrol_params.c new file mode 100644 index 0000000000..adee1a69d5 --- /dev/null +++ b/src/lib/rover_control/rovercontrol_params.c @@ -0,0 +1,255 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file rovercontrol_params.c + * + * Parameters defined by the rover control lib. + */ + +/** + * Yaw stick deadzone + * + * Percentage of stick input range that will be interpreted as zero around the stick centered value. + * + * @min 0 + * @max 1 + * @increment 0.01 + * @decimal 2 + * @group Rover Rate Control + */ +PARAM_DEFINE_FLOAT(RO_YAW_STICK_DZ, 0.1f); + +/** + * Yaw rate measurement threshold + * + * The minimum threshold for the yaw rate measurement not to be interpreted as zero. + * + * @unit deg/s + * @min 0 + * @max 100 + * @increment 0.01 + * @decimal 2 + * @group Rover Rate Control + */ +PARAM_DEFINE_FLOAT(RO_YAW_RATE_TH, 3.f); + +/** + * Proportional gain for closed loop yaw rate controller + * + * @min 0 + * @max 100 + * @increment 0.01 + * @decimal 3 + * @group Rover Rate Control + */ +PARAM_DEFINE_FLOAT(RO_YAW_RATE_P, 0.f); + +/** + * Integral gain for closed loop yaw rate controller + * + * @min 0 + * @max 100 + * @increment 0.01 + * @decimal 3 + * @group Rover Rate Control + */ +PARAM_DEFINE_FLOAT(RO_YAW_RATE_I, 0.f); + +/** + * Yaw rate limit + * + * Used to cap yaw rate setpoints and map controller inputs to yaw rate setpoints + * in Acro, Stabilized and Position mode. + * + * @unit deg/s + * @min 0 + * @max 10000 + * @increment 0.01 + * @decimal 2 + * @group Rover Rate Control + */ +PARAM_DEFINE_FLOAT(RO_YAW_RATE_LIM, 0.f); + +/** + * Yaw acceleration limit + * + * Used to cap how quickly the magnitude of yaw rate setpoints can increase. + * Set to -1 to disable. + * + * @unit deg/s^2 + * @min -1 + * @max 10000 + * @increment 0.01 + * @decimal 2 + * @group Rover Rate Control + */ +PARAM_DEFINE_FLOAT(RO_YAW_ACCEL_LIM, -1.f); + +/** + * Yaw deceleration limit + * + * Used to cap how quickly the magnitude of yaw rate setpoints can decrease. + * Set to -1 to disable. + * + * @unit deg/s^2 + * @min -1 + * @max 10000 + * @increment 0.01 + * @decimal 2 + * @group Rover Rate Control + */ +PARAM_DEFINE_FLOAT(RO_YAW_DECEL_LIM, -1.f); + +/** + * Proportional gain for closed loop yaw controller + * + * @min 0 + * @max 100 + * @increment 0.01 + * @decimal 3 + * @group Rover Attitude Control + */ +PARAM_DEFINE_FLOAT(RO_YAW_P, 0.f); + +/** + * Speed the rover drives at maximum throttle + * + * Used to linearly map speeds [m/s] to throttle values [-1. 1]. + * + * @min 0 + * @max 100 + * @unit m/s + * @increment 0.01 + * @decimal 2 + * @group Rover Velocity Control + */ +PARAM_DEFINE_FLOAT(RO_MAX_THR_SPEED, 0.f); + +/** + * Proportional gain for ground speed controller + * + * @min 0 + * @max 100 + * @increment 0.01 + * @decimal 2 + * @group Rover Velocity Control + */ +PARAM_DEFINE_FLOAT(RO_SPEED_P, 0.f); + +/** + * Integral gain for ground speed controller + * + * @min 0 + * @max 100 + * @increment 0.001 + * @decimal 3 + * @group Rover Velocity Control + */ +PARAM_DEFINE_FLOAT(RO_SPEED_I, 0.f); + +/** + * Speed limit + * + * Used to cap speed setpoints and map controller inputs to speed setpoints in Position mode. + * + * @unit m/s + * @min -1 + * @max 100 + * @increment 0.01 + * @decimal 2 + * @group Rover Velocity Control + */ +PARAM_DEFINE_FLOAT(RO_SPEED_LIM, -1.f); + +/** + * Acceleration limit + * + * Set to -1 to disable. + * For mecanum rovers this limit is used for longitudinal and lateral acceleration. + * + * @unit m/s^2 + * @min -1 + * @max 100 + * @increment 0.01 + * @decimal 2 + * @group Rover Velocity Control + */ +PARAM_DEFINE_FLOAT(RO_ACCEL_LIM, -1.f); + +/** + * Deceleration limit + * + * Set to -1 to disable. + * Note that if it is disabled the rover will not slow down when approaching waypoints in auto modes. + * For mecanum rovers this limit is used for longitudinal and lateral deceleration. + * + * @unit m/s^2 + * @min -1 + * @max 100 + * @increment 0.01 + * @decimal 2 + * @group Rover Velocity Control + */ +PARAM_DEFINE_FLOAT(RO_DECEL_LIM, -1.f); + +/** + * Jerk limit + * + * Set to -1 to disable. + * Note that if it is disabled the rover will not slow down when approaching waypoints in auto modes. + * For mecanum rovers this limit is used for longitudinal and lateral jerk. + * + * @unit m/s^3 + * @min -1 + * @max 100 + * @increment 0.01 + * @decimal 2 + * @group Rover Velocity Control + */ +PARAM_DEFINE_FLOAT(RO_JERK_LIM, -1.f); + +/** + * Speed measurement threshold + * + * Set to -1 to disable. + * The minimum threshold for the speed measurement not to be interpreted as zero. + * + * @unit m/s + * @min 0 + * @max 100 + * @increment 0.01 + * @decimal 2 + * @group Rover Velocity Control + */ +PARAM_DEFINE_FLOAT(RO_SPEED_TH, 0.1f); diff --git a/src/modules/rover_ackermann/CMakeLists.txt b/src/modules/rover_ackermann/CMakeLists.txt index 93269c29b9..9ad139d6e5 100644 --- a/src/modules/rover_ackermann/CMakeLists.txt +++ b/src/modules/rover_ackermann/CMakeLists.txt @@ -50,5 +50,3 @@ px4_add_module( MODULE_CONFIG module.yaml ) - -set_property(GLOBAL APPEND PROPERTY PX4_MODULE_CONFIG_FILES ${CMAKE_CURRENT_SOURCE_DIR}/../../lib/rover_control/module.yaml) From 0ee592f67c4f52cf8e7a0d9939e00e5f1a3348f6 Mon Sep 17 00:00:00 2001 From: jmackay2 <1.732mackay@gmail.com> Date: Tue, 4 Mar 2025 22:58:33 -0500 Subject: [PATCH 88/91] cleanup gz_msgs CMakeLists (#24450) * cleanup gz_sim CMakeLists * Check if protobuf is found * ignore old protobuf float warning --------- Co-authored-by: jmackay2 --- src/modules/simulation/gz_msgs/CMakeLists.txt | 32 +++---------------- 1 file changed, 5 insertions(+), 27 deletions(-) diff --git a/src/modules/simulation/gz_msgs/CMakeLists.txt b/src/modules/simulation/gz_msgs/CMakeLists.txt index c74c010f91..2501811e5e 100644 --- a/src/modules/simulation/gz_msgs/CMakeLists.txt +++ b/src/modules/simulation/gz_msgs/CMakeLists.txt @@ -4,35 +4,10 @@ # ############################################################################ -# message(FATAL_ERROR "JAKE JAKE JAKE JAKE JAKE JAKE JAKE JAKE JAKE JAKE JAKE JAKE JAKE JAKE JAKE JAKE JAKE JAKE ") - -# Check for Gazebo first -if(NOT DEFINED ENV{GZ_DISTRO}) - set(GZ_DISTRO "harmonic" CACHE STRING "Gazebo distribution to use") -else() - set(GZ_DISTRO $ENV{GZ_DISTRO}) -endif() - -# Set versions based on distribution -if(GZ_DISTRO STREQUAL "harmonic") - set(GZ_CMAKE_VERSION "3") - set(GZ_MSGS_VERSION "10") - set(GZ_TRANSPORT_VERSION "13") -elseif(GZ_DISTRO STREQUAL "ionic") - set(GZ_CMAKE_VERSION "4") - set(GZ_MSGS_VERSION "11") - set(GZ_TRANSPORT_VERSION "14") -else() - message(FATAL_ERROR "Unknown Gazebo distribution: ${GZ_DISTRO}") -endif() - # Find required packages -find_package(gz-transport${GZ_TRANSPORT_VERSION}) -if(gz-transport${GZ_TRANSPORT_VERSION}_FOUND) - find_package(gz-cmake${GZ_CMAKE_VERSION} REQUIRED) - find_package(gz-msgs${GZ_MSGS_VERSION} REQUIRED) - find_package(Protobuf REQUIRED) +find_package(Protobuf) +if (Protobuf_FOUND) # Generate protobuf messages file(GLOB MSGS_PROTOS "${CMAKE_CURRENT_SOURCE_DIR}/*.proto") PROTOBUF_GENERATE_CPP(PROTO_SRCS PROTO_HDRS ${MSGS_PROTOS}) @@ -49,6 +24,9 @@ if(gz-transport${GZ_TRANSPORT_VERSION}_FOUND) ${Protobuf_INCLUDE_DIRS} ) target_link_libraries(px4_gz_msgs PUBLIC ${PROTOBUF_LIBRARIES}) + if (Protobuf_VERSION VERSION_LESS "3.8") + target_compile_options(px4_gz_msgs PRIVATE -Wno-error=float-equal) + endif() # Export the binary dir for other modules set(PX4_GZ_MSGS_BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR} CACHE INTERNAL "") From 2aecdfe116519fbd915714947c09898bd8b82b68 Mon Sep 17 00:00:00 2001 From: Niklas Hauser <121870655+niklaut@users.noreply.github.com> Date: Wed, 5 Mar 2025 10:36:39 +0100 Subject: [PATCH 89/91] [adc] Refactor ADS1115 driver (#24428) --- src/drivers/adc/ads1115/ADS1115.cpp | 166 +++++------------------ src/drivers/adc/ads1115/ADS1115.h | 39 +++--- src/drivers/adc/ads1115/ads1115_main.cpp | 88 ++++++------ 3 files changed, 101 insertions(+), 192 deletions(-) diff --git a/src/drivers/adc/ads1115/ADS1115.cpp b/src/drivers/adc/ads1115/ADS1115.cpp index 5ce4ae3504..25c8e92956 100644 --- a/src/drivers/adc/ads1115/ADS1115.cpp +++ b/src/drivers/adc/ads1115/ADS1115.cpp @@ -42,18 +42,7 @@ int ADS1115::init() return ret; } - uint8_t config[2] = {}; - config[0] = CONFIG_HIGH_OS_NOACT | CONFIG_HIGH_MUX_P0NG | CONFIG_HIGH_PGA_6144 | CONFIG_HIGH_MODE_SS; - config[1] = CONFIG_LOW_DR_250SPS | CONFIG_LOW_COMP_MODE_TRADITIONAL | CONFIG_LOW_COMP_POL_RESET | - CONFIG_LOW_COMP_LAT_NONE | CONFIG_LOW_COMP_QU_DISABLE; - ret = writeReg(ADDRESSPOINTER_REG_CONFIG, config, 2); - - if (ret != PX4_OK) { - PX4_ERR("writeReg failed (%i)", ret); - return ret; - } - - setChannel(ADS1115::A0); // prepare for the first measure. + readChannel(Channel::A0); // prepare for the first measure. ScheduleOnInterval(SAMPLE_INTERVAL / 4, SAMPLE_INTERVAL / 4); @@ -62,152 +51,69 @@ int ADS1115::init() int ADS1115::probe() { - uint8_t buf[2] = {}; - int ret = readReg(ADDRESSPOINTER_REG_CONFIG, buf, 2); + // The ADS1115 has no ID register, so we read out the threshold registers + // and check their default values. We cannot use the config register, as + // this is changed by this driver. Note the default value is in BE. + static constexpr uint32_t DEFAULT{0xFF7F0080}; + union { + struct { + uint8_t low[2]; + uint8_t high[2]; + } parts; + uint32_t threshold{}; + }; + int ret = readReg(ADDRESSPOINTER_REG_LO_THRESH, parts.low, 2); if (ret != PX4_OK) { - DEVICE_DEBUG("readReg failed (%i)", ret); + DEVICE_DEBUG("lo_thresh read failed (%i)", ret); return ret; } - if (buf[0] != CONFIG_RESET_VALUE_HIGH || buf[1] != CONFIG_RESET_VALUE_LOW) { - DEVICE_DEBUG("ADS1115 not found"); - return PX4_ERROR; + ret = readReg(ADDRESSPOINTER_REG_HI_THRESH, parts.high, 2); + + if (ret != PX4_OK) { + DEVICE_DEBUG("hi_thresh read failed (%i)", ret); + return ret; } - return PX4_OK; + if (threshold == DEFAULT) { + return PX4_OK; + } + + DEVICE_DEBUG("ADS1115 not found"); + return PX4_ERROR; } -int ADS1115::setChannel(ADS1115::ChannelSelection ch) +int ADS1115::readChannel(ADS1115::Channel ch) { - uint8_t buf[2] = {}; - uint8_t next_mux_reg = CONFIG_HIGH_MUX_P0NG; - - switch (ch) { - case A0: - next_mux_reg = CONFIG_HIGH_MUX_P0NG; - break; - - case A1: - next_mux_reg = CONFIG_HIGH_MUX_P1NG; - break; - - case A2: - next_mux_reg = CONFIG_HIGH_MUX_P2NG; - break; - - case A3: - next_mux_reg = CONFIG_HIGH_MUX_P3NG; - break; - - default: - assert(false); - break; - } - - buf[0] = CONFIG_HIGH_OS_START_SINGLE | next_mux_reg | CONFIG_HIGH_PGA_6144 | CONFIG_HIGH_MODE_SS; + uint8_t buf[2]; + buf[0] = CONFIG_HIGH_OS_START_SINGLE | uint8_t(ch) | CONFIG_HIGH_PGA_6144 | CONFIG_HIGH_MODE_SS; buf[1] = CONFIG_LOW_DR_250SPS | CONFIG_LOW_COMP_MODE_TRADITIONAL | CONFIG_LOW_COMP_POL_RESET | CONFIG_LOW_COMP_LAT_NONE | CONFIG_LOW_COMP_QU_DISABLE; return writeReg(ADDRESSPOINTER_REG_CONFIG, buf, 2); // must write whole register to take effect } -bool ADS1115::isSampleReady() +int ADS1115::isSampleReady() { uint8_t buf[1] = {0x00}; - if (readReg(ADDRESSPOINTER_REG_CONFIG, buf, 1) != 0) { return false; } // Pull config register + if (readReg(ADDRESSPOINTER_REG_CONFIG, buf, 1) != PX4_OK) { return -1; } // Pull config register - return (buf[0] & (uint8_t) 0x80); + return (buf[0] & (uint8_t) 0x80) ? 1 : 0; } -ADS1115::ChannelSelection ADS1115::getMeasurement(int16_t *value) +ADS1115::Channel ADS1115::getMeasurement(int16_t *value) { uint8_t buf[2] = {0x00}; - readReg(ADDRESSPOINTER_REG_CONFIG, buf, 1); // Pull config register - ChannelSelection channel; - switch ((buf[0] & (uint8_t) 0x70) >> 4) { - case 0x04: - channel = A0; - break; + if (readReg(ADDRESSPOINTER_REG_CONFIG, buf, 1) != PX4_OK) { return Channel::Invalid; } - case 0x05: - channel = A1; - break; + const auto channel{Channel(buf[0] & CONFIG_HIGH_MUX_P3NG)}; - case 0x06: - channel = A2; - break; + if (readReg(ADDRESSPOINTER_REG_CONVERSATION, buf, 2) != PX4_OK) { return Channel::Invalid; } - case 0x07: - channel = A3; - break; + *value = int16_t((buf[0] << 8) | buf[1]); - default: - return Invalid; - } - - readReg(ADDRESSPOINTER_REG_CONVERSATION, buf, 2); - uint16_t raw_adc_val = buf[0] * 256 + buf[1]; - - if (raw_adc_val & (uint16_t) 0x8000) { // Negetive value - raw_adc_val = ~raw_adc_val + 1; // 2's complement - *value = -raw_adc_val; - - } else { - *value = raw_adc_val; - } - - return channel; -} - -ADS1115::ChannelSelection ADS1115::cycleMeasure(int16_t *value) -{ - uint8_t buf[2] = {0x00}; - readReg(ADDRESSPOINTER_REG_CONFIG, buf, 1); // Pull config register - ChannelSelection channel; - uint8_t next_mux_reg = CONFIG_HIGH_MUX_P0NG; - - switch ((buf[0] & (uint8_t) 0x70) >> 4) { - case 0x04: - channel = A0; - next_mux_reg = CONFIG_HIGH_MUX_P1NG; - break; - - case 0x05: - channel = A1; - next_mux_reg = CONFIG_HIGH_MUX_P2NG; - break; - - case 0x06: - channel = A2; - next_mux_reg = CONFIG_HIGH_MUX_P3NG; - break; - - case 0x07: - channel = A3; - next_mux_reg = CONFIG_HIGH_MUX_P0NG; - break; - - default: - return Invalid; - } - - readReg(ADDRESSPOINTER_REG_CONVERSATION, buf, 2); - uint16_t raw_adc_val = buf[0] * 256 + buf[1]; - - if (raw_adc_val & (uint16_t) 0x8000) { // Negetive value - raw_adc_val = ~raw_adc_val + 1; // 2's complement - *value = -raw_adc_val; - - } else { - *value = raw_adc_val; - } - - buf[0] = CONFIG_HIGH_OS_START_SINGLE | next_mux_reg | CONFIG_HIGH_PGA_6144 | CONFIG_HIGH_MODE_SS; - buf[1] = CONFIG_LOW_DR_250SPS | CONFIG_LOW_COMP_MODE_TRADITIONAL | CONFIG_LOW_COMP_POL_RESET | - CONFIG_LOW_COMP_LAT_NONE | CONFIG_LOW_COMP_QU_DISABLE; - writeReg(ADDRESSPOINTER_REG_CONFIG, buf, 2); // must write whole register to take effect return channel; } diff --git a/src/drivers/adc/ads1115/ADS1115.h b/src/drivers/adc/ads1115/ADS1115.h index 79a31198c9..634a659df8 100644 --- a/src/drivers/adc/ads1115/ADS1115.h +++ b/src/drivers/adc/ads1115/ADS1115.h @@ -93,9 +93,6 @@ #define CONFIG_LOW_COMP_QU_AFTER4 0x02 #define CONFIG_LOW_COMP_QU_DISABLE 0x03 -#define CONFIG_RESET_VALUE_HIGH 0x85 -#define CONFIG_RESET_VALUE_LOW 0x83 - using namespace time_literals; /* @@ -127,35 +124,39 @@ protected: private: - uORB::PublicationMulti _to_adc_report{ORB_ID(adc_report)}; + uORB::PublicationMulti _to_adc_report{ORB_ID(adc_report)}; - static const hrt_abstime SAMPLE_INTERVAL{50_ms}; + static const hrt_abstime SAMPLE_INTERVAL{50_ms}; adc_report_s _adc_report{}; - perf_counter_t _cycle_perf; + perf_counter_t _cycle_perf; + perf_counter_t _comms_errors; - int _channel_cycle_count{0}; + uint8_t _channel_cycle_mask{0}; - bool _reported_ready_last_cycle{false}; + static constexpr uint8_t MAX_READY_COUNTER{20}; + uint8_t _ready_counter{MAX_READY_COUNTER}; // ADS1115 logic part - enum ChannelSelection { - Invalid = -1, A0 = 0, A1, A2, A3 + enum class Channel : uint8_t { + A0 = CONFIG_HIGH_MUX_P0NG, + A1 = CONFIG_HIGH_MUX_P1NG, + A2 = CONFIG_HIGH_MUX_P2NG, + A3 = CONFIG_HIGH_MUX_P3NG, + Invalid = 0xff, }; - /* set multiplexer to specific channel */ - int setChannel(ChannelSelection ch); - /* return true if sample result is valid */ - bool isSampleReady(); + constexpr unsigned ch2u(Channel ch) { return (unsigned(ch) >> 4) & 0b11u; } + constexpr Channel u2ch(unsigned ch) { return Channel((ch << 4) | CONFIG_HIGH_MUX_P0NG); } + // Set the channel and start a conversion + int readChannel(Channel ch); + // return 1 if sample result is valid else 0 or -1 if I2C transaction failed + int isSampleReady(); /* * get adc sample. return the channel being measured. * Invalid indicates sample failure. */ - ChannelSelection getMeasurement(int16_t *value); - /* - * get adc sample and automatically switch to next channel and start another measurement - */ - ChannelSelection cycleMeasure(int16_t *value); + Channel getMeasurement(int16_t *value); int readReg(uint8_t addr, uint8_t *buf, size_t len); diff --git a/src/drivers/adc/ads1115/ads1115_main.cpp b/src/drivers/adc/ads1115/ads1115_main.cpp index 2132082c66..1967b58263 100644 --- a/src/drivers/adc/ads1115/ads1115_main.cpp +++ b/src/drivers/adc/ads1115/ads1115_main.cpp @@ -45,7 +45,8 @@ ADS1115::ADS1115(const I2CSPIDriverConfig &config) : I2C(config), I2CSPIDriver(config), - _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": single-sample")) + _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": single-sample")), + _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms errors")) { _adc_report.device_id = this->get_device_id(); _adc_report.resolution = 32768; @@ -61,6 +62,7 @@ ADS1115::~ADS1115() { ScheduleClear(); perf_free(_cycle_perf); + perf_free(_comms_errors); } void ADS1115::exit_and_cleanup() @@ -77,56 +79,55 @@ void ADS1115::RunImpl() perf_begin(_cycle_perf); - _adc_report.timestamp = hrt_absolute_time(); + const int ready = isSampleReady(); - if (isSampleReady()) { // whether ADS1115 is ready to be read or not - if (!_reported_ready_last_cycle) { - PX4_INFO("ADS1115: reported ready"); - _reported_ready_last_cycle = true; + if (ready == 1) { + // I2C transaction success and status register reported conversion as finished + if (_ready_counter == 0) { PX4_INFO("ADS1115: reported ready"); } + + if (_ready_counter < MAX_READY_COUNTER) { _ready_counter++; } + + int16_t value; + Channel ch = getMeasurement(&value); + + if (ch != Channel::Invalid) { + // Store current readings and mark channel as read + const unsigned index{ch2u(ch)}; + _adc_report.channel_id[index] = index; + _adc_report.raw_data[index] = value; + _channel_cycle_mask |= 1u << index; + + } else { + // we will retry the same channel again + perf_count(_comms_errors); } - int16_t buf; - ADS1115::ChannelSelection ch = cycleMeasure(&buf); - ++_channel_cycle_count; + // Find the next unread channel in the bitmask + uint8_t next_index{0}; - switch (ch) { - case ADS1115::A0: - _adc_report.channel_id[0] = 0; - _adc_report.raw_data[0] = buf; - break; + for (; next_index < 4 && (_channel_cycle_mask & (1u << next_index)); next_index++) {} - case ADS1115::A1: - _adc_report.channel_id[1] = 1; - _adc_report.raw_data[1] = buf; - break; + readChannel(u2ch(next_index)); - case ADS1115::A2: - _adc_report.channel_id[2] = 2; - _adc_report.raw_data[2] = buf; - break; - - case ADS1115::A3: - _adc_report.channel_id[3] = 3; - _adc_report.raw_data[3] = buf; - break; - - default: - PX4_DEBUG("ADS1115: undefined behaviour"); - setChannel(ADS1115::A0); - --_channel_cycle_count; - break; - } - - if (_channel_cycle_count == 4) { // ADS1115 has 4 channels - _channel_cycle_count = 0; + if (_channel_cycle_mask == 0b1111) { + _channel_cycle_mask = 0; _to_adc_report.publish(_adc_report); } - } else { - if (_reported_ready_last_cycle) { - _reported_ready_last_cycle = false; - PX4_ERR("ADS1115: not ready. Device lost?"); - } + } else if (ready == 0) { + // I2C transaction success but status register reported conversion still in progress + perf_count(_comms_errors); + // Reset the channel to unstick the device + readChannel(Channel::A0); + + } else if (ready == -1) { + if (_ready_counter == 1) { PX4_ERR("ADS1115: device lost"); } + + if (_ready_counter > 0) { _ready_counter--; } + + perf_count(_comms_errors); + // Reset the channel to unstick the device + readChannel(Channel::A0); } perf_end(_cycle_perf); @@ -151,7 +152,7 @@ parameter, and is disabled by default. If enabled, internal ADCs are not used. )DESCR_STR"); - + PRINT_MODULE_USAGE_NAME("ads1115", "driver"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); @@ -163,6 +164,7 @@ void ADS1115::print_status() { I2CSPIDriverBase::print_status(); perf_print_counter(_cycle_perf); + perf_print_counter(_comms_errors); } extern "C" int ads1115_main(int argc, char *argv[]) From 0ab3e45c13b603eaf867b7a341ab0eeafc4c92aa Mon Sep 17 00:00:00 2001 From: Mathieu Bresciani Date: Wed, 5 Mar 2025 16:37:26 +0100 Subject: [PATCH 90/91] MC auto: improve behavior of RC help during landing Letting the autopilot set the heading during landing while the pilot is nudging the vehile leads to a weird UX as the vehicle would make a turn instead of translating. With this modification, the initial land heading is immediately overridden when the pilot begins to adjust the drone's position, providing the sensation of full control. --- .../flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 556ac6c72e..5cb75222a0 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -253,9 +253,15 @@ void FlightTaskAuto::_prepareLandSetpoints() // Stick full up -1 -> stop, stick full down 1 -> double the speed vertical_speed *= (1 - _sticks.getThrottleZeroCenteredExpo()); + Vector2f sticks_xy = _sticks.getPitchRollExpo(); + + if (sticks_xy.longerThan(FLT_EPSILON)) { + // Ensure no unintended yawing when nudging horizontally during initial heading alignment + _land_heading = _yaw_sp_prev; + } + rcHelpModifyYaw(_land_heading); - Vector2f sticks_xy = _sticks.getPitchRollExpo(); Vector2f sticks_ne = sticks_xy; Sticks::rotateIntoHeadingFrameXY(sticks_ne, _yaw, _land_heading); From ea8bcd9cef1554d93a9229b6a8796e1e3585cd75 Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Wed, 5 Mar 2025 15:37:16 -0900 Subject: [PATCH 91/91] gz: use server config file for loading world plugins (#24441) * gz: use server config file for loading world plugins * submodule * use server.config in tree * newlines * format * gzbridge: rename function * format * gzbridge: add magnetometer callback * change gz_find_package to find_package * fix up directory structure and cmake to allow multiple plugins * newlines * add comment block explaining gz_env.sh * remove dupe readme * remove SENS_EN_MAGSIM from all gz airframe files except spacecraft * update gz submodule --- .../init.d-posix/airframes/4001_gz_x500 | 2 - .../init.d-posix/airframes/4003_gz_rc_cessna | 3 - .../airframes/4004_gz_standard_vtol | 1 - .../init.d-posix/airframes/4006_gz_px4vision | 2 - .../airframes/4008_gz_advanced_plane | 1 - .../init.d-posix/airframes/4009_gz_r1_rover | 3 - .../init.d-posix/airframes/4011_gz_lawnmower | 2 - .../airframes/4012_gz_rover_ackermann | 3 - .../airframes/4018_gz_quadtailsitter | 2 - .../init.d-posix/airframes/4020_gz_tiltrotor | 2 - .../init.d-posix/airframes/8011_gz_omnicopter | 1 - Tools/simulation/gz | 2 +- .../simulation/gz_bridge/CMakeLists.txt | 4 +- src/modules/simulation/gz_bridge/GZBridge.cpp | 75 ++++++++++++----- src/modules/simulation/gz_bridge/GZBridge.hpp | 10 ++- src/modules/simulation/gz_bridge/gz_env.sh.in | 13 +++ .../simulation/gz_bridge/server.config | 19 +++++ .../simulation/gz_plugins/CMakeLists.txt | 84 +++++++++++-------- .../gz_plugins/optical_flow/CMakeLists.txt | 65 ++++++++++++++ .../{ => optical_flow}/OpticalFlowSensor.cpp | 0 .../{ => optical_flow}/OpticalFlowSensor.hpp | 0 .../{ => optical_flow}/OpticalFlowSystem.cpp | 0 .../{ => optical_flow}/OpticalFlowSystem.hpp | 0 .../{ => optical_flow}/optical_flow.cmake | 0 .../gz_plugins/template_plugin/CMakeLists.txt | 68 +++++++++++++++ .../gz_plugins/template_plugin/README.md | 30 +++++++ .../template_plugin/TemplateSystem.cpp | 58 +++++++++++++ .../template_plugin/TemplateSystem.hpp | 57 +++++++++++++ .../sensor_mag_sim/SensorMagSim.hpp | 2 +- 29 files changed, 425 insertions(+), 84 deletions(-) create mode 100644 src/modules/simulation/gz_bridge/server.config create mode 100644 src/modules/simulation/gz_plugins/optical_flow/CMakeLists.txt rename src/modules/simulation/gz_plugins/{ => optical_flow}/OpticalFlowSensor.cpp (100%) rename src/modules/simulation/gz_plugins/{ => optical_flow}/OpticalFlowSensor.hpp (100%) rename src/modules/simulation/gz_plugins/{ => optical_flow}/OpticalFlowSystem.cpp (100%) rename src/modules/simulation/gz_plugins/{ => optical_flow}/OpticalFlowSystem.hpp (100%) rename src/modules/simulation/gz_plugins/{ => optical_flow}/optical_flow.cmake (100%) create mode 100644 src/modules/simulation/gz_plugins/template_plugin/CMakeLists.txt create mode 100644 src/modules/simulation/gz_plugins/template_plugin/README.md create mode 100644 src/modules/simulation/gz_plugins/template_plugin/TemplateSystem.cpp create mode 100644 src/modules/simulation/gz_plugins/template_plugin/TemplateSystem.hpp diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 b/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 index 3923b44cb4..2add7a5cb4 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 @@ -13,8 +13,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500} param set-default SIM_GZ_EN 1 -param set-default SENS_EN_MAGSIM 1 - param set-default CA_AIRFRAME 0 param set-default CA_ROTOR_COUNT 4 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna b/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna index ea7b3a2f03..96dd5fa151 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna @@ -12,11 +12,8 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=rc_cessna} param set-default SIM_GZ_EN 1 -param set-default SENS_EN_MAGSIM 1 param set-default SENS_EN_ARSPDSIM 1 - - param set-default FW_LND_ANG 8 param set-default NPFG_PERIOD 12 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol index 69b0cec559..5bd432abde 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol @@ -13,7 +13,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=standard_vtol} param set-default SIM_GZ_EN 1 -param set-default SENS_EN_MAGSIM 1 param set-default SENS_EN_ARSPDSIM 1 # TODO: Enable motor failure detection when the diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision b/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision index e988f1f9b0..4370a677ed 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision @@ -14,8 +14,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=px4vision} param set-default SIM_GZ_EN 1 -param set-default SENS_EN_MAGSIM 1 - # Commander Parameters param set-default COM_DISARM_LAND 0.5 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane b/ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane index 8479f2e38c..8ac407bac4 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane @@ -11,7 +11,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=advanced_plane} param set-default SIM_GZ_EN 1 -param set-default SENS_EN_MAGSIM 1 param set-default SENS_EN_ARSPDSIM 1 param set-default FW_LND_ANG 8 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover index 5ff39b3f96..2d36aed7ce 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover @@ -45,9 +45,6 @@ param set-default PP_LOOKAHD_GAIN 1 param set-default PP_LOOKAHD_MAX 10 param set-default PP_LOOKAHD_MIN 1 -# Simulated sensors -param set-default SENS_EN_MAGSIM 1 - # Actuator mapping param set-default SIM_GZ_WH_FUNC1 101 # right wheel param set-default SIM_GZ_WH_MIN1 70 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower index a56cf93005..83f387b56e 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower @@ -11,8 +11,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=lawnmower} param set-default SIM_GZ_EN 1 # Gazebo bridge -# Simulated sensors -param set-default SENS_EN_MAGSIM 1 # We can arm and drive in manual mode when it slides and GPS check fails: param set-default COM_ARM_WO_GPS 1 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann index 5867b4d3e3..9c24991fcf 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann @@ -44,9 +44,6 @@ param set-default PP_LOOKAHD_GAIN 1 param set-default PP_LOOKAHD_MAX 10 param set-default PP_LOOKAHD_MIN 1 -# Simulated sensors -param set-default SENS_EN_MAGSIM 1 - # Wheels param set-default SIM_GZ_WH_FUNC1 101 param set-default SIM_GZ_WH_MIN1 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter b/ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter index 1af6e721bc..bced062c1e 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter @@ -13,8 +13,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=quadtailsitter} param set-default SIM_GZ_EN 1 # Gazebo bridge -param set-default SENS_EN_MAGSIM 1 - param set-default MAV_TYPE 20 param set-default CA_AIRFRAME 4 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor b/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor index 8325f67a2c..ff7805cb77 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor @@ -13,8 +13,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=tiltrotor} param set-default SIM_GZ_EN 1 # Gazebo bridge -param set-default SENS_EN_MAGSIM 1 - param set-default MAV_TYPE 21 param set-default CA_AIRFRAME 3 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter b/ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter index 0f6748edaf..be7a32431b 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter @@ -83,7 +83,6 @@ param set-default CA_ROTOR7_AY -0.211325 param set-default CA_ROTOR7_AZ -0.57735 param set-default SIM_GZ_EN 1 -param set-default SENS_EN_MAGSIM 1 param set-default SIM_GZ_EC_FUNC1 101 param set-default SIM_GZ_EC_FUNC2 102 diff --git a/Tools/simulation/gz b/Tools/simulation/gz index 5bbae38b4f..6c18846a4c 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 5bbae38b4f942521b4f3288c298083571ea5718c +Subproject commit 6c18846a4c7f9fe786840a29bf4e3237f908611b diff --git a/src/modules/simulation/gz_bridge/CMakeLists.txt b/src/modules/simulation/gz_bridge/CMakeLists.txt index 784c5a5ca8..5fdc455cc8 100644 --- a/src/modules/simulation/gz_bridge/CMakeLists.txt +++ b/src/modules/simulation/gz_bridge/CMakeLists.txt @@ -132,14 +132,14 @@ if (gz-transport${GZ_TRANSPORT_VERSION}_FOUND) COMMAND ${CMAKE_COMMAND} -E env PX4_SIM_MODEL=gz_${model_name} $ WORKING_DIRECTORY ${SITL_WORKING_DIR} USES_TERMINAL - DEPENDS px4 OpticalFlowSystem + DEPENDS px4 px4_gz_plugins ) else() add_custom_target(gz_${model_name}_${world_name} COMMAND ${CMAKE_COMMAND} -E env PX4_SIM_MODEL=gz_${model_name} PX4_GZ_WORLD=${world_name} $ WORKING_DIRECTORY ${SITL_WORKING_DIR} USES_TERMINAL - DEPENDS px4 OpticalFlowSystem + DEPENDS px4 px4_gz_plugins ) endif() endforeach() diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index c890ad8d5c..6cf1734da5 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -85,7 +85,16 @@ int GZBridge::init() return PX4_ERROR; } - // IMU: /world/$WORLD/model/$MODEL/link/base_link/sensor/imu_sensor/imu + // mag: /world/$WORLD/model/$MODEL/link/base_link/sensor/magnetometer_sensor/magnetometer + std::string mag_topic = "/world/" + _world_name + "/model/" + _model_name + + "/link/base_link/sensor/magnetometer_sensor/magnetometer"; + + if (!_node.Subscribe(mag_topic, &GZBridge::magnetometerCallback, this)) { + PX4_ERR("failed to subscribe to %s", mag_topic.c_str()); + return PX4_ERROR; + } + + // odom: /world/$WORLD/model/$MODEL/link/base_link/odometry_with_covariance std::string odometry_topic = "/model/" + _model_name + "/odometry_with_covariance"; if (!_node.Subscribe(odometry_topic, &GZBridge::odometryCallback, this)) { @@ -176,16 +185,16 @@ void GZBridge::clockCallback(const gz::msgs::Clock &msg) px4_clock_settime(CLOCK_MONOTONIC, &ts); } -void GZBridge::opticalFlowCallback(const px4::msgs::OpticalFlow &flow) +void GZBridge::opticalFlowCallback(const px4::msgs::OpticalFlow &msg) { - sensor_optical_flow_s msg = {}; + sensor_optical_flow_s report = {}; - msg.timestamp = hrt_absolute_time(); - msg.timestamp_sample = flow.time_usec(); - msg.pixel_flow[0] = flow.integrated_x(); - msg.pixel_flow[1] = flow.integrated_y(); - msg.quality = flow.quality(); - msg.integration_timespan_us = flow.integration_time_us(); + report.timestamp = hrt_absolute_time(); + report.timestamp_sample = msg.time_usec(); + report.pixel_flow[0] = msg.integrated_x(); + report.pixel_flow[1] = msg.integrated_y(); + report.quality = msg.quality(); + report.integration_timespan_us = msg.integration_time_us(); // Static data device::Device::DeviceId id; @@ -193,21 +202,47 @@ void GZBridge::opticalFlowCallback(const px4::msgs::OpticalFlow &flow) id.devid_s.bus = 0; id.devid_s.address = 0; id.devid_s.devtype = DRV_FLOW_DEVTYPE_SIM; - msg.device_id = id.devid; + report.device_id = id.devid; // values taken from PAW3902 - msg.mode = sensor_optical_flow_s::MODE_LOWLIGHT; - msg.max_flow_rate = 7.4f; - msg.min_ground_distance = 0.f; - msg.max_ground_distance = 30.f; - msg.error_count = 0; + report.mode = sensor_optical_flow_s::MODE_LOWLIGHT; + report.max_flow_rate = 7.4f; + report.min_ground_distance = 0.f; + report.max_ground_distance = 30.f; + report.error_count = 0; // No delta angle // No distance // This means that delta angle will come from vehicle gyro // Distance will come from vehicle distance sensor - _optical_flow_pub.publish(msg); + _optical_flow_pub.publish(report); +} + +void GZBridge::magnetometerCallback(const gz::msgs::Magnetometer &msg) +{ + const uint64_t timestamp = hrt_absolute_time(); + + device::Device::DeviceId id{}; + id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; + id.devid_s.devtype = DRV_MAG_DEVTYPE_MAGSIM; + id.devid_s.bus = 1; + id.devid_s.address = 3; // TODO: any value other than 3 causes Commander to not use the mag.... wtf + + sensor_mag_s report{}; + report.timestamp = timestamp; + report.timestamp_sample = timestamp; + report.device_id = id.devid; + report.temperature = this->_temperature; + + // FIMEX: once we're on jetty or later + // The magnetometer plugin publishes in units of gauss and in a weird left handed coordinate system + // https://github.com/gazebosim/gz-sim/pull/2460 + report.x = -msg.field_tesla().y(); + report.y = -msg.field_tesla().x(); + report.z = msg.field_tesla().z(); + + _sensor_mag_pub.publish(report); } void GZBridge::barometerCallback(const gz::msgs::FluidPressure &msg) @@ -253,7 +288,6 @@ void GZBridge::airspeedCallback(const gz::msgs::AirSpeed &msg) void GZBridge::imuCallback(const gz::msgs::IMU &msg) { - const uint64_t timestamp = hrt_absolute_time(); // FLU -> FRD @@ -284,7 +318,6 @@ void GZBridge::imuCallback(const gz::msgs::IMU &msg) accel.samples = 1; _sensor_accel_pub.publish(accel); - gz::math::Vector3d gyro_b = q_FLU_to_FRD.RotateVector(gz::math::Vector3d( msg.angular_velocity().x(), msg.angular_velocity().y(), @@ -484,8 +517,8 @@ static float generate_wgn() return X; } -void GZBridge::addRealisticGpsNoise(double &latitude, double &longitude, double &altitude, - float &vel_north, float &vel_east, float &vel_down) +void GZBridge::addGpsNoise(double &latitude, double &longitude, double &altitude, + float &vel_north, float &vel_east, float &vel_down) { _gps_pos_noise_n = _pos_markov_time * _gps_pos_noise_n + _pos_random_walk * generate_wgn() * _pos_noise_amplitude - @@ -546,7 +579,7 @@ void GZBridge::navSatCallback(const gz::msgs::NavSat &msg) _gpos_ground_truth_pub.publish(gps_truth); // Apply noise model (based on ublox F9P) - addRealisticGpsNoise(latitude, longitude, altitude, vel_north, vel_east, vel_down); + addGpsNoise(latitude, longitude, altitude, vel_north, vel_east, vel_down); // Device ID device::Device::DeviceId id{}; diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 6128e8c175..ac096744c2 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -57,6 +57,7 @@ #include #include #include +#include #include #include #include @@ -116,12 +117,13 @@ private: void navSatCallback(const gz::msgs::NavSat &msg); void laserScantoLidarSensorCallback(const gz::msgs::LaserScan &msg); void laserScanCallback(const gz::msgs::LaserScan &msg); - void opticalFlowCallback(const px4::msgs::OpticalFlow &image_msg); + void opticalFlowCallback(const px4::msgs::OpticalFlow &msg); + void magnetometerCallback(const gz::msgs::Magnetometer &msg); static void rotateQuaternion(gz::math::Quaterniond &q_FRD_to_NED, const gz::math::Quaterniond q_FLU_to_ENU); - void addRealisticGpsNoise(double &latitude, double &longitude, double &altitude, - float &vel_north, float &vel_east, float &vel_down); + void addGpsNoise(double &latitude, double &longitude, double &altitude, + float &vel_north, float &vel_east, float &vel_down); uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -136,9 +138,11 @@ private: uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; uORB::PublicationMulti _sensor_accel_pub{ORB_ID(sensor_accel)}; uORB::PublicationMulti _sensor_gyro_pub{ORB_ID(sensor_gyro)}; + uORB::PublicationMulti _sensor_mag_pub{ORB_ID(sensor_mag)}; uORB::PublicationMulti _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; uORB::PublicationMulti _optical_flow_pub{ORB_ID(sensor_optical_flow)}; + GZMixingInterfaceESC _mixing_interface_esc{_node}; GZMixingInterfaceServo _mixing_interface_servo{_node}; GZMixingInterfaceWheel _mixing_interface_wheel{_node}; diff --git a/src/modules/simulation/gz_bridge/gz_env.sh.in b/src/modules/simulation/gz_bridge/gz_env.sh.in index 3267318bfc..bff7180b03 100644 --- a/src/modules/simulation/gz_bridge/gz_env.sh.in +++ b/src/modules/simulation/gz_bridge/gz_env.sh.in @@ -1,8 +1,21 @@ #!/usr/bin/env bash +# ----------------------------------------------------------------------- +# Gazebo Environment Configuration +# ----------------------------------------------------------------------- +# GZ_SIM_RESOURCE_PATH: Where Gazebo looks for models and worlds +# GZ_SIM_SYSTEM_PLUGIN_PATH: Where Gazebo looks for plugin libraries +# GZ_SIM_SERVER_CONFIG_PATH: Custom Gazebo server configuration file +# +# See Gazebo docs +# https://gazebosim.org/api/sim/8/resources.html +# https://gazebosim.org/api/sim/8/server_config.html +# ----------------------------------------------------------------------- export PX4_GZ_MODELS=@PX4_SOURCE_DIR@/Tools/simulation/gz/models export PX4_GZ_WORLDS=@PX4_SOURCE_DIR@/Tools/simulation/gz/worlds export PX4_GZ_PLUGINS=@PX4_BINARY_DIR@/src/modules/simulation/gz_plugins +export PX4_GZ_SERVER_CONFIG=@PX4_SOURCE_DIR@/src/modules/simulation/gz_bridge/server.config export GZ_SIM_RESOURCE_PATH=$GZ_SIM_RESOURCE_PATH:$PX4_GZ_MODELS:$PX4_GZ_WORLDS export GZ_SIM_SYSTEM_PLUGIN_PATH=$GZ_SIM_SYSTEM_PLUGIN_PATH:$PX4_GZ_PLUGINS +export GZ_SIM_SERVER_CONFIG_PATH=$PX4_GZ_SERVER_CONFIG diff --git a/src/modules/simulation/gz_bridge/server.config b/src/modules/simulation/gz_bridge/server.config new file mode 100644 index 0000000000..93bff3f442 --- /dev/null +++ b/src/modules/simulation/gz_bridge/server.config @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + ogre2 + + + + + diff --git a/src/modules/simulation/gz_plugins/CMakeLists.txt b/src/modules/simulation/gz_plugins/CMakeLists.txt index c4400b0204..4bb3307108 100644 --- a/src/modules/simulation/gz_plugins/CMakeLists.txt +++ b/src/modules/simulation/gz_plugins/CMakeLists.txt @@ -1,4 +1,37 @@ -project(OpticalFlowSystem) +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +project(px4_gz_plugins) if(NOT DEFINED ENV{GZ_DISTRO}) set(GZ_DISTRO "harmonic" CACHE STRING "Gazebo distribution to use") @@ -28,43 +61,26 @@ else() message(FATAL_ERROR "Unknown Gazebo distribution: ${GZ_DISTRO}. Valid options are: harmonic or ionic") endif() -# Use gz-transport as litmus test for prescence of gz +# Use gz-transport as litmus test for presence of gz find_package(gz-transport${GZ_TRANSPORT_VERSION}) if (gz-transport${GZ_TRANSPORT_VERSION}_FOUND) + find_package(gz-cmake${GZ_CMAKE_VERSION} REQUIRED) + find_package(gz-msgs${GZ_MSGS_VERSION} REQUIRED) + find_package(Protobuf REQUIRED) + find_package(gz-plugin${GZ_PLUGIN_VERSION} REQUIRED COMPONENTS register) + find_package(gz-sim${GZ_SIM_VERSION} REQUIRED) + find_package(gz-sensors${GZ_SENSORS_VERSION} REQUIRED) - gz_find_package(gz-cmake${GZ_CMAKE_VERSION} REQUIRED) - gz_find_package(gz-msgs${GZ_MSGS_VERSION} REQUIRED) - gz_find_package(Protobuf REQUIRED) - gz_find_package(gz-plugin${GZ_PLUGIN_VERSION} REQUIRED COMPONENTS register) - gz_find_package(gz-sim${GZ_SIM_VERSION} REQUIRED) - gz_find_package(gz-sensors${GZ_SENSORS_VERSION} REQUIRED) + # Create a flat output directory for all plugin libraries + set(PX4_GZ_PLUGIN_OUTPUT_DIR "${CMAKE_CURRENT_BINARY_DIR}" CACHE PATH "Directory for all Gazebo plugin libraries") + file(MAKE_DIRECTORY ${PX4_GZ_PLUGIN_OUTPUT_DIR}) + set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PX4_GZ_PLUGIN_OUTPUT_DIR}) - include(${CMAKE_CURRENT_SOURCE_DIR}/optical_flow.cmake) - - add_library(${PROJECT_NAME} SHARED - OpticalFlowSensor.cpp - OpticalFlowSystem.cpp - ) - - target_link_libraries(${PROJECT_NAME} - PUBLIC px4_gz_msgs - PUBLIC gz-sensors${GZ_SENSORS_VERSION}::gz-sensors${GZ_SENSORS_VERSION} - PUBLIC gz-plugin${GZ_PLUGIN_VERSION}::gz-plugin${GZ_PLUGIN_VERSION} - PUBLIC gz-sim${GZ_SIM_VERSION}::gz-sim${GZ_SIM_VERSION} - PUBLIC gz-transport${GZ_TRANSPORT_VERSION}::gz-transport${GZ_TRANSPORT_VERSION} - PUBLIC ${OpenCV_LIBS} - PUBLIC ${OpticalFlow_LIBS} - ) - - target_include_directories(${PROJECT_NAME} - PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} - PUBLIC ${CMAKE_CURRENT_BINARY_DIR} - PUBLIC ${OpenCV_INCLUDE_DIRS} - PUBLIC ${OpticalFlow_INCLUDE_DIRS} - PUBLIC px4_gz_msgs - ) - - add_dependencies(${PROJECT_NAME} OpticalFlow) + # Add our plugins as subdirectories + add_subdirectory(optical_flow) + add_subdirectory(template_plugin) + # Add an alias target for each plugin + add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem TemplatePlugin) endif() diff --git a/src/modules/simulation/gz_plugins/optical_flow/CMakeLists.txt b/src/modules/simulation/gz_plugins/optical_flow/CMakeLists.txt new file mode 100644 index 0000000000..2a57ba3c25 --- /dev/null +++ b/src/modules/simulation/gz_plugins/optical_flow/CMakeLists.txt @@ -0,0 +1,65 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +project(OpticalFlowSystem) + +# Include the OpticalFlow external dependency +include(${CMAKE_CURRENT_SOURCE_DIR}/optical_flow.cmake) + +# Find OpenCV +find_package(OpenCV REQUIRED) + +add_library(${PROJECT_NAME} SHARED + OpticalFlowSensor.cpp + OpticalFlowSystem.cpp +) + +target_link_libraries(${PROJECT_NAME} + PUBLIC px4_gz_msgs + PUBLIC gz-sensors${GZ_SENSORS_VERSION}::gz-sensors${GZ_SENSORS_VERSION} + PUBLIC gz-plugin${GZ_PLUGIN_VERSION}::gz-plugin${GZ_PLUGIN_VERSION} + PUBLIC gz-sim${GZ_SIM_VERSION}::gz-sim${GZ_SIM_VERSION} + PUBLIC gz-transport${GZ_TRANSPORT_VERSION}::gz-transport${GZ_TRANSPORT_VERSION} + PUBLIC ${OpenCV_LIBS} + PUBLIC ${OpticalFlow_LIBS} +) + +target_include_directories(${PROJECT_NAME} + PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} + PUBLIC ${CMAKE_CURRENT_BINARY_DIR} + PUBLIC ${OpenCV_INCLUDE_DIRS} + PUBLIC ${OpticalFlow_INCLUDE_DIRS} + PUBLIC px4_gz_msgs +) + +add_dependencies(${PROJECT_NAME} OpticalFlow) diff --git a/src/modules/simulation/gz_plugins/OpticalFlowSensor.cpp b/src/modules/simulation/gz_plugins/optical_flow/OpticalFlowSensor.cpp similarity index 100% rename from src/modules/simulation/gz_plugins/OpticalFlowSensor.cpp rename to src/modules/simulation/gz_plugins/optical_flow/OpticalFlowSensor.cpp diff --git a/src/modules/simulation/gz_plugins/OpticalFlowSensor.hpp b/src/modules/simulation/gz_plugins/optical_flow/OpticalFlowSensor.hpp similarity index 100% rename from src/modules/simulation/gz_plugins/OpticalFlowSensor.hpp rename to src/modules/simulation/gz_plugins/optical_flow/OpticalFlowSensor.hpp diff --git a/src/modules/simulation/gz_plugins/OpticalFlowSystem.cpp b/src/modules/simulation/gz_plugins/optical_flow/OpticalFlowSystem.cpp similarity index 100% rename from src/modules/simulation/gz_plugins/OpticalFlowSystem.cpp rename to src/modules/simulation/gz_plugins/optical_flow/OpticalFlowSystem.cpp diff --git a/src/modules/simulation/gz_plugins/OpticalFlowSystem.hpp b/src/modules/simulation/gz_plugins/optical_flow/OpticalFlowSystem.hpp similarity index 100% rename from src/modules/simulation/gz_plugins/OpticalFlowSystem.hpp rename to src/modules/simulation/gz_plugins/optical_flow/OpticalFlowSystem.hpp diff --git a/src/modules/simulation/gz_plugins/optical_flow.cmake b/src/modules/simulation/gz_plugins/optical_flow/optical_flow.cmake similarity index 100% rename from src/modules/simulation/gz_plugins/optical_flow.cmake rename to src/modules/simulation/gz_plugins/optical_flow/optical_flow.cmake diff --git a/src/modules/simulation/gz_plugins/template_plugin/CMakeLists.txt b/src/modules/simulation/gz_plugins/template_plugin/CMakeLists.txt new file mode 100644 index 0000000000..f5e2935ef6 --- /dev/null +++ b/src/modules/simulation/gz_plugins/template_plugin/CMakeLists.txt @@ -0,0 +1,68 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# Template for a new plugin project +# Replace TemplatePlugin with your plugin name +project(TemplatePlugin) + +# Add external dependencies if needed +# include(${CMAKE_CURRENT_SOURCE_DIR}/dependency.cmake) + +# Find required packages +# find_package(PackageName REQUIRED) + +add_library(${PROJECT_NAME} SHARED + # Add your source files here + TemplateSystem.cpp +) + +target_link_libraries(${PROJECT_NAME} + PUBLIC px4_gz_msgs + PUBLIC gz-sensors${GZ_SENSORS_VERSION}::gz-sensors${GZ_SENSORS_VERSION} + PUBLIC gz-plugin${GZ_PLUGIN_VERSION}::gz-plugin${GZ_PLUGIN_VERSION} + PUBLIC gz-sim${GZ_SIM_VERSION}::gz-sim${GZ_SIM_VERSION} + PUBLIC gz-transport${GZ_TRANSPORT_VERSION}::gz-transport${GZ_TRANSPORT_VERSION} + # Add other dependencies as needed + # PUBLIC ${OtherLib_LIBS} +) + +target_include_directories(${PROJECT_NAME} + PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} + PUBLIC ${CMAKE_CURRENT_BINARY_DIR} + PUBLIC px4_gz_msgs + # Add other include directories as needed + # PUBLIC ${OtherLib_INCLUDE_DIRS} +) + +# Add dependencies if needed +# add_dependencies(${PROJECT_NAME} ExternalDependency) diff --git a/src/modules/simulation/gz_plugins/template_plugin/README.md b/src/modules/simulation/gz_plugins/template_plugin/README.md new file mode 100644 index 0000000000..dda5fbcbf5 --- /dev/null +++ b/src/modules/simulation/gz_plugins/template_plugin/README.md @@ -0,0 +1,30 @@ +# Template Gazebo Plugin + +This is a template for creating new Gazebo plugins for PX4. Follow these steps to create your own plugin: + +1. Copy this directory and rename it to your plugin name +2. Update the project name in CMakeLists.txt +3. Rename and implement the TemplateSystem.hpp/cpp files +4. Add your plugin to the top-level CMakeLists.txt in the gz_plugins directory: + ```cmake + add_subdirectory(your_plugin_directory) + ``` +5. Add your plugin's target to the `px4_gz_plugins` target dependencies in the top-level CMakeLists.txt: + ```cmake + add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem YourPluginSystem) + ``` +6. Update the server.config file to load your plugin: + ```xml + + ``` + +## Plugin Structure + +This template follows the standard Gazebo plugin structure: + +- `TemplateSystem.hpp/cpp`: The main plugin system class that is loaded by Gazebo +- CMakeLists.txt: Build configuration for this plugin + +## Testing Your Plugin + +After building, you can test your plugin by adding it to the server.config file and running a simulation. diff --git a/src/modules/simulation/gz_plugins/template_plugin/TemplateSystem.cpp b/src/modules/simulation/gz_plugins/template_plugin/TemplateSystem.cpp new file mode 100644 index 0000000000..ce07329224 --- /dev/null +++ b/src/modules/simulation/gz_plugins/template_plugin/TemplateSystem.cpp @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "TemplateSystem.hpp" + +#include + +using namespace custom; + +// Register the plugin +GZ_ADD_PLUGIN( + TemplateSystem, + gz::sim::System, + TemplateSystem::ISystemPreUpdate, + TemplateSystem::ISystemPostUpdate +) + +void TemplateSystem::PreUpdate(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) +{ + // Implement pre-update logic here +} + +void TemplateSystem::PostUpdate(const gz::sim::UpdateInfo &_info, + const gz::sim::EntityComponentManager &_ecm) +{ + // Implement post-update logic here +} diff --git a/src/modules/simulation/gz_plugins/template_plugin/TemplateSystem.hpp b/src/modules/simulation/gz_plugins/template_plugin/TemplateSystem.hpp new file mode 100644 index 0000000000..840dec1c33 --- /dev/null +++ b/src/modules/simulation/gz_plugins/template_plugin/TemplateSystem.hpp @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +namespace custom +{ +class TemplateSystem: + public gz::sim::System, + public gz::sim::ISystemPreUpdate, + public gz::sim::ISystemPostUpdate +{ +public: + void PreUpdate(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) final; + + void PostUpdate(const gz::sim::UpdateInfo &_info, + const gz::sim::EntityComponentManager &_ecm) final; + +private: + // Add your private member variables and methods here + gz::transport::Node _node; +}; +} // end namespace custom diff --git a/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp b/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp index 7ee92ad750..18c4adbf1d 100644 --- a/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp +++ b/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp @@ -78,7 +78,7 @@ private: uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude_groundtruth)}; uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)}; - PX4Magnetometer _px4_mag{197388, ROTATION_NONE}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + PX4Magnetometer _px4_mag{197388, ROTATION_NONE}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 1, ADDR: 3, TYPE: SIMULATION bool _mag_earth_available{false};