From abc629c2bbc30a4f1d93ca9e2c1b7245574452fc Mon Sep 17 00:00:00 2001 From: Alexis Paques <6976744+AlexisTM@users.noreply.github.com> Date: Fri, 2 Aug 2024 17:48:55 +0200 Subject: [PATCH 01/90] zenoh: update zenoh-pico from 0.7.0 to 1.0.0 (#23462) * Update Zenoh-pico from 0.7.0 to 1.0.0 * Update the zenoh-pico version to use PX4/dev/1.0.0-px4 * Remove the rostopic and rt/ prefix * Unlike zenoh-bridge-dds we were using, zenoh-bridge-ros2dds is now adding the rt/ prefix automagically. --- .gitmodules | 2 +- boards/px4/fmu-v6x/nuttx-config/nsh/defconfig | 1 + src/modules/zenoh/CMakeLists.txt | 3 +- src/modules/zenoh/Kconfig.topics | 5 +++ .../zenoh/publishers/uorb_publisher.hpp | 2 +- .../zenoh/publishers/zenoh_publisher.cpp | 33 +++++++-------- .../zenoh/publishers/zenoh_publisher.hpp | 10 +---- .../zenoh/subscribers/uorb_subscriber.hpp | 9 ++-- .../zenoh/subscribers/zenoh_subscriber.cpp | 39 ++++++++--------- .../zenoh/subscribers/zenoh_subscriber.hpp | 12 ++---- src/modules/zenoh/zenoh-pico | 2 +- src/modules/zenoh/zenoh.cpp | 42 ++++++++++++------- 12 files changed, 81 insertions(+), 79 deletions(-) diff --git a/.gitmodules b/.gitmodules index e443da19a290..a783b1693843 100644 --- a/.gitmodules +++ b/.gitmodules @@ -71,7 +71,7 @@ [submodule "src/modules/zenoh/zenoh-pico"] path = src/modules/zenoh/zenoh-pico url = https://github.com/px4/zenoh-pico - branch = pr-zubf-werror-fix + branch = dev/1.0.0-px4 [submodule "src/lib/heatshrink/heatshrink"] path = src/lib/heatshrink/heatshrink url = https://github.com/PX4/heatshrink.git diff --git a/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig b/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig index 333609282e00..ec6b1c7e2b0e 100644 --- a/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig @@ -93,6 +93,7 @@ CONFIG_DEFAULT_SMALL=y CONFIG_DEV_FIFO_SIZE=0 CONFIG_DEV_PIPE_MAXSIZE=1024 CONFIG_DEV_PIPE_SIZE=70 +CONFIG_DEV_URANDOM=y CONFIG_ETH0_PHY_LAN8742A=y CONFIG_EXPERIMENTAL=y CONFIG_FAT_DMAMEMORY=y diff --git a/src/modules/zenoh/CMakeLists.txt b/src/modules/zenoh/CMakeLists.txt index 889a1994a427..e3a8166dff73 100644 --- a/src/modules/zenoh/CMakeLists.txt +++ b/src/modules/zenoh/CMakeLists.txt @@ -57,6 +57,7 @@ target_compile_options(zenohpico PUBLIC -Wno-cast-align -DZ_BATCH_SIZE_TX=512 -DZ_FRAG_MAX_SIZE=1024) +target_compile_options(zenohpico PRIVATE -Wno-missing-prototypes) if(CONFIG_PLATFORM_NUTTX) target_compile_options(zenohpico PRIVATE -DUNIX_NO_MULTICAST_IF) endif() @@ -72,7 +73,6 @@ px4_add_module( SRCS zenoh.cpp zenoh_config.cpp - zenoh.h publishers/zenoh_publisher.cpp subscribers/zenoh_subscriber.cpp MODULE_CONFIG @@ -96,6 +96,5 @@ px4_add_module( -Wno-double-promotion -Wno-unused -DZENOH_LINUX - -DZENOH_NO_STDATOMIC -D_Bool=int8_t ) diff --git a/src/modules/zenoh/Kconfig.topics b/src/modules/zenoh/Kconfig.topics index e1c2ea4ada5b..ee9fe7ce7362 100644 --- a/src/modules/zenoh/Kconfig.topics +++ b/src/modules/zenoh/Kconfig.topics @@ -573,6 +573,10 @@ menu "Zenoh publishers/subscribers" bool "rover_ackermann_guidance_status" default n + config ZENOH_PUBSUB_ROVER_ACKERMANN_STATUS + bool "rover_ackermann_status" + default n + config ZENOH_PUBSUB_RPM bool "rpm" default n @@ -1009,6 +1013,7 @@ config ZENOH_PUBSUB_ALL_SELECTION select ZENOH_PUBSUB_REGISTER_EXT_COMPONENT_REPLY select ZENOH_PUBSUB_REGISTER_EXT_COMPONENT_REQUEST select ZENOH_PUBSUB_ROVER_ACKERMANN_GUIDANCE_STATUS + select ZENOH_PUBSUB_ROVER_ACKERMANN_STATUS select ZENOH_PUBSUB_RPM select ZENOH_PUBSUB_RTL_STATUS select ZENOH_PUBSUB_RTL_TIME_ESTIMATE diff --git a/src/modules/zenoh/publishers/uorb_publisher.hpp b/src/modules/zenoh/publishers/uorb_publisher.hpp index b541bfd1dd7c..fd41e4f2e88c 100644 --- a/src/modules/zenoh/publishers/uorb_publisher.hpp +++ b/src/modules/zenoh/publishers/uorb_publisher.hpp @@ -51,7 +51,7 @@ class uORB_Zenoh_Publisher : public Zenoh_Publisher { public: uORB_Zenoh_Publisher(const orb_metadata *meta, const uint32_t *ops) : - Zenoh_Publisher(true), + Zenoh_Publisher(), _uorb_meta{meta}, _cdr_ops(ops) { diff --git a/src/modules/zenoh/publishers/zenoh_publisher.cpp b/src/modules/zenoh/publishers/zenoh_publisher.cpp index 1e312ffea502..8ce688d2b787 100644 --- a/src/modules/zenoh/publishers/zenoh_publisher.cpp +++ b/src/modules/zenoh/publishers/zenoh_publisher.cpp @@ -42,9 +42,8 @@ #include "zenoh_publisher.hpp" -Zenoh_Publisher::Zenoh_Publisher(bool rostopic) +Zenoh_Publisher::Zenoh_Publisher() { - this->_rostopic = rostopic; this->_topic[0] = 0x0; } @@ -59,24 +58,18 @@ int Zenoh_Publisher::undeclare_publisher() return 0; } -int Zenoh_Publisher::declare_publisher(z_session_t s, const char *keyexpr) +int Zenoh_Publisher::declare_publisher(z_owned_session_t s, const char *keyexpr) { - if (_rostopic) { - strncpy(this->_topic, (char *)_rt_prefix, _rt_prefix_offset); + strncpy(this->_topic, keyexpr, sizeof(this->_topic)); - if (keyexpr[0] == '/') { - strncpy(this->_topic + _rt_prefix_offset, keyexpr + 1, sizeof(this->_topic) - _rt_prefix_offset); + z_view_keyexpr_t ke; + z_view_keyexpr_from_str(&ke, this->_topic); - } else { - strncpy(this->_topic + _rt_prefix_offset, keyexpr, sizeof(this->_topic) - _rt_prefix_offset); - } - - } else { - strncpy(this->_topic, keyexpr, sizeof(this->_topic)); + if (z_declare_publisher(&_pub, z_loan(s), z_loan(ke), NULL) < 0) { + printf("Unable to declare publisher for key expression!\n"); + return -1; } - _pub = z_declare_publisher(s, z_keyexpr(this->_topic), NULL); - if (!z_publisher_check(&_pub)) { printf("Unable to declare publisher for key expression!\n"); return -1; @@ -87,9 +80,13 @@ int Zenoh_Publisher::declare_publisher(z_session_t s, const char *keyexpr) int8_t Zenoh_Publisher::publish(const uint8_t *buf, int size) { - z_publisher_put_options_t options = z_publisher_put_options_default(); - options.encoding = z_encoding(Z_ENCODING_PREFIX_APP_CUSTOM, NULL); - return z_publisher_put(z_publisher_loan(&_pub), buf, size, &options); + z_publisher_put_options_t options; + z_publisher_put_options_default(&options); + options.encoding = NULL; + + z_owned_bytes_t payload; + z_bytes_serialize_from_slice(&payload, buf, size); + return z_publisher_put(z_loan(_pub), z_move(payload), &options); } void Zenoh_Publisher::print() diff --git a/src/modules/zenoh/publishers/zenoh_publisher.hpp b/src/modules/zenoh/publishers/zenoh_publisher.hpp index 1d88a453e2f7..a12849c78682 100644 --- a/src/modules/zenoh/publishers/zenoh_publisher.hpp +++ b/src/modules/zenoh/publishers/zenoh_publisher.hpp @@ -51,10 +51,10 @@ class Zenoh_Publisher : public ListNode { public: - Zenoh_Publisher(bool rostopic = true); + Zenoh_Publisher(); virtual ~Zenoh_Publisher(); - virtual int declare_publisher(z_session_t s, const char *keyexpr); + virtual int declare_publisher(z_owned_session_t s, const char *keyexpr); virtual int undeclare_publisher(); @@ -66,11 +66,5 @@ class Zenoh_Publisher : public ListNode int8_t publish(const uint8_t *, int size); z_owned_publisher_t _pub; - char _topic[60]; // The Topic name is somewhere is the Zenoh stack as well but no good api to fetch it. - - // Indicates ROS2 Topic namespace - bool _rostopic; - const char *_rt_prefix = "rt/"; - const size_t _rt_prefix_offset = 3; // "rt/" are 3 chars }; diff --git a/src/modules/zenoh/subscribers/uorb_subscriber.hpp b/src/modules/zenoh/subscribers/uorb_subscriber.hpp index 550c1a3f4584..bd59adb84606 100644 --- a/src/modules/zenoh/subscribers/uorb_subscriber.hpp +++ b/src/modules/zenoh/subscribers/uorb_subscriber.hpp @@ -50,7 +50,7 @@ class uORB_Zenoh_Subscriber : public Zenoh_Subscriber { public: uORB_Zenoh_Subscriber(const orb_metadata *meta, const uint32_t *ops) : - Zenoh_Subscriber(true), + Zenoh_Subscriber(), _uorb_meta{meta}, _cdr_ops(ops) { @@ -61,11 +61,14 @@ class uORB_Zenoh_Subscriber : public Zenoh_Subscriber ~uORB_Zenoh_Subscriber() override = default; // Update the uORB Subscription and broadcast a Zenoh ROS2 message - void data_handler(const z_sample_t *sample) + void data_handler(const z_loaned_sample_t *sample) { char data[_uorb_meta->o_size]; - dds_istream_t is = {.m_buffer = (unsigned char *)(sample->payload.start), .m_size = static_cast(sample->payload.len), + const z_loaned_bytes_t *payload = z_sample_payload(sample); + size_t len = z_bytes_len(payload); + + dds_istream_t is = {.m_buffer = (unsigned char *)(payload), .m_size = static_cast(len), .m_index = 4, .m_xcdr_version = DDSI_RTPS_CDR_ENC_VERSION_2 }; dds_stream_read(&is, data, &dds_allocator, _cdr_ops); diff --git a/src/modules/zenoh/subscribers/zenoh_subscriber.cpp b/src/modules/zenoh/subscribers/zenoh_subscriber.cpp index aab66ee5bdba..79f37d89812a 100644 --- a/src/modules/zenoh/subscribers/zenoh_subscriber.cpp +++ b/src/modules/zenoh/subscribers/zenoh_subscriber.cpp @@ -41,22 +41,23 @@ #include "zenoh_subscriber.hpp" -static void data_handler_cb(const z_sample_t *sample, void *arg) +static void data_handler_cb(const z_loaned_sample_t *sample, void *arg) { static_cast(arg)->data_handler(sample); } -void Zenoh_Subscriber::data_handler(const z_sample_t *sample) +void Zenoh_Subscriber::data_handler(const z_loaned_sample_t *sample) { - z_owned_str_t keystr = z_keyexpr_to_string(sample->keyexpr); - printf(">> [Subscriber] Received ('%s' size '%d')\n", z_str_loan(&keystr), (int)sample->payload.len); - z_str_drop(z_str_move(&keystr)); + z_view_string_t keystr; + z_keyexpr_as_view_string(z_sample_keyexpr(sample), &keystr); + z_owned_slice_t value; + z_bytes_deserialize_into_slice(z_sample_payload(sample), &value); + printf(">> [Subscriber] Received ('%s' size '%d')\n", z_string_data(z_loan(keystr)), (int)z_slice_len(z_loan(value))); } -Zenoh_Subscriber::Zenoh_Subscriber(bool rostopic) +Zenoh_Subscriber::Zenoh_Subscriber() { - this->_rostopic = rostopic; this->_topic[0] = 0x0; } @@ -71,27 +72,21 @@ int Zenoh_Subscriber::undeclare_subscriber() return 0; } -int Zenoh_Subscriber::declare_subscriber(z_session_t s, const char *keyexpr) +int Zenoh_Subscriber::declare_subscriber(z_owned_session_t s, const char *keyexpr) { - z_owned_closure_sample_t callback = z_closure_sample(data_handler_cb, NULL, this); + z_owned_closure_sample_t callback; + z_closure_sample(&callback, data_handler_cb, NULL, this); - if (_rostopic) { - strncpy(this->_topic, (char *)_rt_prefix, _rt_prefix_offset); + strncpy(this->_topic, keyexpr, sizeof(this->_topic)); - if (keyexpr[0] == '/') { - strncpy(this->_topic + _rt_prefix_offset, keyexpr + 1, sizeof(this->_topic) - _rt_prefix_offset); + z_view_keyexpr_t ke; + z_view_keyexpr_from_str(&ke, this->_topic); - } else { - strncpy(this->_topic + _rt_prefix_offset, keyexpr, sizeof(this->_topic) - _rt_prefix_offset); - } - - } else { - strncpy(this->_topic, (char *)keyexpr, sizeof(this->_topic)); + if (z_declare_subscriber(&_sub, z_loan(s), z_loan(ke), z_closure_sample_move(&callback), NULL) < 0) { + printf("Unable to declare subscriber.\n"); + exit(-1); } - _sub = z_declare_subscriber(s, z_keyexpr(this->_topic), z_closure_sample_move(&callback), NULL); - - if (!z_subscriber_check(&_sub)) { printf("Unable to declare subscriber for key expression!\n %s\n", keyexpr); return -1; diff --git a/src/modules/zenoh/subscribers/zenoh_subscriber.hpp b/src/modules/zenoh/subscribers/zenoh_subscriber.hpp index e3f1200e9245..1b6d10922045 100644 --- a/src/modules/zenoh/subscribers/zenoh_subscriber.hpp +++ b/src/modules/zenoh/subscribers/zenoh_subscriber.hpp @@ -55,14 +55,14 @@ class Zenoh_Subscriber : public ListNode { public: - Zenoh_Subscriber(bool rostopic = true); + Zenoh_Subscriber(); virtual ~Zenoh_Subscriber(); - virtual int declare_subscriber(z_session_t s, const char *keyexpr); + virtual int declare_subscriber(z_owned_session_t s, const char *keyexpr); virtual int undeclare_subscriber(); - virtual void data_handler(const z_sample_t *sample); + virtual void data_handler(const z_loaned_sample_t *sample); virtual void print(); @@ -71,10 +71,4 @@ class Zenoh_Subscriber : public ListNode z_owned_subscriber_t _sub; char _topic[60]; // The Topic name is somewhere is the Zenoh stack as well but no good api to fetch it. - - - // Indicates ROS2 Topic namespace - bool _rostopic; - const char *_rt_prefix = "rt/"; - const size_t _rt_prefix_offset = 3; // "rt/" are 3 chars }; diff --git a/src/modules/zenoh/zenoh-pico b/src/modules/zenoh/zenoh-pico index 22bbfc215092..fff1a8c168e1 160000 --- a/src/modules/zenoh/zenoh-pico +++ b/src/modules/zenoh/zenoh-pico @@ -1 +1 @@ -Subproject commit 22bbfc215092a051341c234fdcf68f5baa267dec +Subproject commit fff1a8c168e10d4ed9cbf2aa7d67ede07f239c52 diff --git a/src/modules/zenoh/zenoh.cpp b/src/modules/zenoh/zenoh.cpp index 0caf2c5eaa5f..6411734a2918 100644 --- a/src/modules/zenoh/zenoh.cpp +++ b/src/modules/zenoh/zenoh.cpp @@ -32,6 +32,8 @@ ****************************************************************************/ #include "zenoh.h" +#include "zenoh-pico/api/macros.h" +#include "zenoh-pico/api/primitives.h" #include #include #include @@ -78,27 +80,39 @@ void ZENOH::run() z_config.getNetworkConfig(mode, locator); - z_owned_config_t config = z_config_default(); - zp_config_insert(z_config_loan(&config), Z_CONFIG_MODE_KEY, z_string_make(mode)); + z_owned_config_t config; + z_config_default(&config); + zp_config_insert(z_loan_mut(config), Z_CONFIG_MODE_KEY, mode); if (locator[0] != 0) { - zp_config_insert(z_config_loan(&config), Z_CONFIG_PEER_KEY, z_string_make(locator)); + zp_config_insert(z_loan_mut(config), Z_CONFIG_CONNECT_KEY, locator); } else if (strcmp(Z_CONFIG_MODE_PEER, mode) == 0) { - zp_config_insert(z_config_loan(&config), Z_CONFIG_PEER_KEY, z_string_make(Z_CONFIG_MULTICAST_LOCATOR_DEFAULT)); + zp_config_insert(z_loan_mut(config), Z_CONFIG_CONNECT_KEY, Z_CONFIG_MULTICAST_LOCATOR_DEFAULT); } PX4_INFO("Opening session..."); - z_owned_session_t s = z_open(z_config_move(&config)); + z_owned_session_t s; + ret = z_open(&s, z_move(config)); + + if (ret < 0) { + PX4_ERR("Unable to open session, ret: %d", ret); + return; + } + + PX4_INFO("Checking session..."); if (!z_session_check(&s)) { - PX4_ERR("Unable to open session!"); + PX4_ERR("Unable to check session!"); return; } + PX4_INFO("Starting reading/writing tasks..."); + // Start read and lease tasks for zenoh-pico - if (zp_start_read_task(z_session_loan(&s), NULL) < 0 || zp_start_lease_task(z_session_loan(&s), NULL) < 0) { + if (zp_start_read_task(z_loan_mut(s), NULL) < 0 || zp_start_lease_task(z_loan_mut(s), NULL) < 0) { PX4_ERR("Unable to start read and lease tasks"); + z_close(z_move(s)); return; } @@ -114,7 +128,7 @@ void ZENOH::run() _zenoh_subscribers[i] = genSubscriber(type); if (_zenoh_subscribers[i] != 0) { - _zenoh_subscribers[i]->declare_subscriber(z_session_loan(&s), topic); + _zenoh_subscribers[i]->declare_subscriber(s, topic); } @@ -141,7 +155,7 @@ void ZENOH::run() _zenoh_publishers[i] = genPublisher(type); if (_zenoh_publishers[i] != 0) { - _zenoh_publishers[i]->declare_publisher(z_session_loan(&s), topic); + _zenoh_publishers[i]->declare_publisher(s, topic); _zenoh_publishers[i]->setPollFD(&pfds[i]); } } @@ -154,7 +168,7 @@ void ZENOH::run() if (_pub_count == 0) { // Nothing to publish but we don't want to stop this thread while (!should_exit()) { - sleep(2); + usleep(1000); } } @@ -194,8 +208,8 @@ void ZENOH::run() free(_zenoh_publishers); // Stop read and lease tasks for zenoh-pico - zp_stop_read_task(z_session_loan(&s)); - zp_stop_lease_task(z_session_loan(&s)); + zp_stop_read_task(z_session_loan_mut(&s)); + zp_stop_lease_task(z_session_loan_mut(&s)); z_close(z_session_move(&s)); exit_and_cleanup(); @@ -234,8 +248,8 @@ Zenoh demo bridge PX4_INFO_RAW(" addsubscriber Publish Zenoh topic to uORB\n"); PX4_INFO_RAW(" net Zenoh network mode\n"); PX4_INFO_RAW(" values: client|peer \n"); - PX4_INFO_RAW(" client: locator address for router\n"); - PX4_INFO_RAW(" peer: multicast address e.g. udp/224.0.0.225:7447#iface=eth0\n"); + PX4_INFO_RAW(" client: locator address e.g. tcp/10.41.10.1:7447#iface=eth0\n"); + PX4_INFO_RAW(" peer: multicast address e.g. udp/224.0.0.224:7446#iface=eth0\n"); return 0; } From d4d60a5181b83e51d63ee5af6befaf33d873df5c Mon Sep 17 00:00:00 2001 From: Alexis Paques Date: Fri, 2 Aug 2024 15:14:30 +0200 Subject: [PATCH 02/90] Add missing rc.sysinit file in the ROMFS --- ROMFS/px4fmu_common/init.d/CMakeLists.txt | 1 + ROMFS/px4fmu_common/init.d/rc.sysinit | 4 ++++ 2 files changed, 5 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/rc.sysinit diff --git a/ROMFS/px4fmu_common/init.d/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/CMakeLists.txt index 90b14f812f02..24dea3fcda93 100644 --- a/ROMFS/px4fmu_common/init.d/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/CMakeLists.txt @@ -44,6 +44,7 @@ px4_add_romfs_files( # TODO rc.balloon_apps rc.balloon_defaults + rc.sysinit ) if(CONFIG_MODULES_AIRSHIP_ATT_CONTROL) diff --git a/ROMFS/px4fmu_common/init.d/rc.sysinit b/ROMFS/px4fmu_common/init.d/rc.sysinit new file mode 100644 index 000000000000..9f878a1387bb --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.sysinit @@ -0,0 +1,4 @@ +#!/bin/sh +# +# Standard system init script +# From 4883f2128a02286df603d27104ed8a5110ee6d4c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 30 Jul 2024 11:52:18 +0200 Subject: [PATCH 03/90] commander: allow external modes more time for initial response We've come accross a case where a ROS node would consistently take something over 800 ms until the first arming check request subscription callback was triggered. After the first sample, the callback always triggered within the expected timeframe. Therefore this patch allows for more time right after registration until timing out. --- .../HealthAndArmingChecks/checks/externalChecks.cpp | 7 ++++++- .../HealthAndArmingChecks/checks/externalChecks.hpp | 4 ++++ 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp index c60a9a4af05e..8ef3c5cfa966 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp @@ -64,6 +64,7 @@ int ExternalChecks::addRegistration(int8_t nav_mode_id, int8_t replaces_nav_stat _active_registrations_mask |= 1 << free_registration_index; _registrations[free_registration_index].nav_mode_id = nav_mode_id; _registrations[free_registration_index].replaces_nav_state = replaces_nav_state; + _registrations[free_registration_index].waiting_for_first_response = true; _registrations[free_registration_index].num_no_response = 0; _registrations[free_registration_index].unresponsive = false; _registrations[free_registration_index].total_num_unresponsive = 0; @@ -230,6 +231,7 @@ void ExternalChecks::update() && _current_request_id == reply.request_id) { _reply_received_mask |= 1u << reply.registration_id; _registrations[reply.registration_id].num_no_response = 0; + _registrations[reply.registration_id].waiting_for_first_response = false; // Prevent toggling between unresponsive & responsive state if (_registrations[reply.registration_id].total_num_unresponsive <= 3) { @@ -253,7 +255,10 @@ void ExternalChecks::update() for (int i = 0; i < MAX_NUM_REGISTRATIONS; ++i) { if ((1u << i) & no_reply) { - if (!_registrations[i].unresponsive && ++_registrations[i].num_no_response >= NUM_NO_REPLY_UNTIL_UNRESPONSIVE) { + const int max_num_no_reply = + _registrations[i].waiting_for_first_response ? NUM_NO_REPLY_UNTIL_UNRESPONSIVE_INIT : NUM_NO_REPLY_UNTIL_UNRESPONSIVE; + + if (!_registrations[i].unresponsive && ++_registrations[i].num_no_response > max_num_no_reply) { // Clear immediately if not a mode if (_registrations[i].nav_mode_id == -1) { removeRegistration(i, -1); diff --git a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp index b4ee24cba6d5..7129e4620361 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp @@ -72,6 +72,9 @@ class ExternalChecks : public HealthAndArmingCheckBase static constexpr hrt_abstime UPDATE_INTERVAL = 300_ms; static_assert(REQUEST_TIMEOUT < UPDATE_INTERVAL, "keep timeout < update interval"); static constexpr int NUM_NO_REPLY_UNTIL_UNRESPONSIVE = 3; ///< Mode timeout = this value * UPDATE_INTERVAL + /// Timeout directly after registering (in some cases ROS can take a while until the subscription gets the first + /// sample, around 800ms was observed) + static constexpr int NUM_NO_REPLY_UNTIL_UNRESPONSIVE_INIT = 10; void checkNonRegisteredModes(const Context &context, Report &reporter) const; @@ -83,6 +86,7 @@ class ExternalChecks : public HealthAndArmingCheckBase int8_t nav_mode_id{-1}; ///< associated mode, -1 if none int8_t replaces_nav_state{-1}; + bool waiting_for_first_response{true}; uint8_t num_no_response{0}; bool unresponsive{false}; uint8_t total_num_unresponsive{0}; From 326e2a9f5cf6dceeb6ef7c9e2bdf5eec89edf411 Mon Sep 17 00:00:00 2001 From: sbtjagu <145494402+sbtjagu@users.noreply.github.com> Date: Mon, 5 Aug 2024 13:02:12 +0200 Subject: [PATCH 04/90] ackermann: add protection against float precision problem in acceptance radius update (#23478) * ackermann: add protection against float precision problem in acceptance radius update * ackermann: protect against divide-by-zero --------- Co-authored-by: Mathieu Bresciani --- .../RoverAckermannGuidance/RoverAckermannGuidance.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp index db0495b67a3e..ade89f6c7e18 100644 --- a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp +++ b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp @@ -268,8 +268,9 @@ float RoverAckermannGuidance::updateAcceptanceRadius(const Vector2f &curr_wp_ned // Calculate acceptance radius s.t. the rover cuts the corner tangential to the current and next line segment if (curr_to_next_wp_ned.norm() > FLT_EPSILON && curr_to_prev_wp_ned.norm() > FLT_EPSILON) { - const float theta = acosf((curr_to_prev_wp_ned * curr_to_next_wp_ned) / (curr_to_prev_wp_ned.norm() * - curr_to_next_wp_ned.norm())) / 2.f; + 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 + const float theta = acosf(cosin) / 2.f; const float min_turning_radius = wheel_base / sinf(max_steer_angle); const float acceptance_radius_temp = min_turning_radius / tanf(theta); const float acceptance_radius_temp_scaled = acceptance_radius_gain * From 84d4ee0e60550646007370926400597913e3e4e1 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 5 Aug 2024 14:32:20 +0200 Subject: [PATCH 05/90] zenoh-pico: update to correct dev/1.0.0 branch which is up to date containing "Use SO_REUSEPORT only if it exists" and is advertised by GitHub because the commit is on a branch --- src/modules/zenoh/zenoh-pico | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/zenoh/zenoh-pico b/src/modules/zenoh/zenoh-pico index fff1a8c168e1..f093aa7955bb 160000 --- a/src/modules/zenoh/zenoh-pico +++ b/src/modules/zenoh/zenoh-pico @@ -1 +1 @@ -Subproject commit fff1a8c168e10d4ed9cbf2aa7d67ede07f239c52 +Subproject commit f093aa7955bb24dfa3e626de084583711e3bac5c From 8ed3489bd1af260b01e4932b93ddc03bc3f81a19 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 6 Aug 2024 10:48:12 +0200 Subject: [PATCH 06/90] hardfault_log: revert to explicit path to not trip the module documentation parser - the module documentation parser can only resolve defines from the same file - also it cannot deal with defines embeded in strings - what board should it add for the general documentation anyways? As a result of these issues I suggest to stay with the original hardcoded /fs/microsd for the documentation. It's still the most common path as far as I can see. --- src/systemcmds/hardfault_log/hardfault_log.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/hardfault_log/hardfault_log.c b/src/systemcmds/hardfault_log/hardfault_log.c index bad376b25edc..f8ff269ec6b8 100644 --- a/src/systemcmds/hardfault_log/hardfault_log.c +++ b/src/systemcmds/hardfault_log/hardfault_log.c @@ -1289,7 +1289,7 @@ static void print_usage(void) "Hardfault type: 0=divide by 0, 1=Assertion, 2=jump to 0x0, 3=write to 0x0 (default=0)", true); PRINT_MODULE_USAGE_COMMAND_DESCR("commit", - "Write uncommitted hardfault to " CONFIG_BOARD_ROOT_PATH "/fault_%i.txt (and rearm, but don't reset)"); + "Write uncommitted hardfault to /fs/microsd/fault_%i.txt (and rearm, but don't reset)"); PRINT_MODULE_USAGE_COMMAND_DESCR("count", "Read the reboot counter, counts the number of reboots of an uncommitted hardfault (returned as the exit code of the program)"); PRINT_MODULE_USAGE_COMMAND_DESCR("reset", "Reset the reboot counter"); From d2478d00cf612c8869f6a90ef3617f1f6c095499 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 5 Aug 2024 11:28:49 +0200 Subject: [PATCH 07/90] ekf2: only allow ref sensor to reset height --- .../barometer/baro_height_control.cpp | 9 +- .../external_vision/ev_height_control.cpp | 2 +- .../aid_sources/gnss/gnss_height_control.cpp | 4 +- .../range_finder/range_height_control.cpp | 2 +- src/modules/ekf2/test/sensor_simulator/gps.h | 1 + .../ekf2/test/test_EKF_height_fusion.cpp | 87 +++++++++++++++++++ 6 files changed, 98 insertions(+), 7 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp index 264c826738f2..dc6d36bcb5c7 100644 --- a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp @@ -126,7 +126,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max); - if (isHeightResetRequired()) { + if (isHeightResetRequired() && (_height_sensor_ref == HeightSensor::BARO)) { // All height sources are failing ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); @@ -142,10 +142,13 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) aid_src.time_last_fuse = imu_sample.time_us; } else if (is_fusion_failing) { - // Some other height source is still working ECL_WARN("stopping %s height fusion, fusion failing", HGT_SRC_NAME); stopBaroHgtFusion(); - _baro_hgt_faulty = true; + + if (isRecent(_time_last_hgt_fuse, _params.hgt_fusion_timeout_max)) { + // Some other height source is still working + _baro_hgt_faulty = true; + } } } else { diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp index e7b6f13f1181..440c0fe7acf6 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp @@ -142,7 +142,7 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max); - if (isHeightResetRequired() && quality_sufficient) { + if (isHeightResetRequired() && quality_sufficient && (_height_sensor_ref == HeightSensor::EV)) { // All height sources are failing ECL_WARN("%s fusion reset required, all height sources failing", AID_SRC_NAME); _information_events.flags.reset_hgt_to_ev = true; diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp index c141d415eec5..91a8685cebae 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp @@ -100,12 +100,12 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample) const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max); - if (isHeightResetRequired()) { + if (isHeightResetRequired() && (_height_sensor_ref == HeightSensor::GNSS)) { // All height sources are failing ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); _information_events.flags.reset_hgt_to_gps = true; - resetVerticalPositionTo(-(measurement - bias_est.getBias()), measurement_var); + resetVerticalPositionTo(aid_src.observation, measurement_var); bias_est.setBias(_state.pos(2) + measurement); aid_src.time_last_fuse = _time_delayed_us; diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp index 6c33d4db639a..0aafe8a1b00d 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp @@ -173,7 +173,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max); - if (isHeightResetRequired() && _control_status.flags.rng_hgt) { + if (isHeightResetRequired() && _control_status.flags.rng_hgt && (_height_sensor_ref == HeightSensor::RANGE)) { // All height sources are failing ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); diff --git a/src/modules/ekf2/test/sensor_simulator/gps.h b/src/modules/ekf2/test/sensor_simulator/gps.h index f962cc9bced7..e8ff206916f7 100644 --- a/src/modules/ekf2/test/sensor_simulator/gps.h +++ b/src/modules/ekf2/test/sensor_simulator/gps.h @@ -66,6 +66,7 @@ class Gps: public Sensor void setPdop(const float pdop); gnssSample getDefaultGpsData(); + const gnssSample &getData() const { return _gps_data; } private: void send(uint64_t time) override; diff --git a/src/modules/ekf2/test/test_EKF_height_fusion.cpp b/src/modules/ekf2/test/test_EKF_height_fusion.cpp index c80939f1ba6e..b322a2818245 100644 --- a/src/modules/ekf2/test/test_EKF_height_fusion.cpp +++ b/src/modules/ekf2/test/test_EKF_height_fusion.cpp @@ -261,6 +261,93 @@ TEST_F(EkfHeightFusionTest, gpsRefFailOver) EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::UNKNOWN); } +TEST_F(EkfHeightFusionTest, gpsRefAllHgtFailReset) +{ + // GIVEN: EKF that fuses GNSS (reference) and baro + _sensor_simulator.startBaro(); + _sensor_simulator.startGps(); + _ekf_wrapper.setGpsHeightRef(); + _ekf_wrapper.enableBaroHeightFusion(); + _ekf_wrapper.enableGpsHeightFusion(); + + _sensor_simulator.runSeconds(11); + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); + EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::GNSS); + + const Vector3f previous_position = _ekf->getPosition(); + + ResetLoggingChecker reset_logging_checker(_ekf); + reset_logging_checker.capturePreResetState(); + + // WHEN: + const float gnss_height_step = 10.f; + _sensor_simulator._gps.stepHeightByMeters(gnss_height_step); + + const float baro_height_step = 5.f; + _sensor_simulator._baro.setData(_sensor_simulator._baro.getData() + baro_height_step); + _sensor_simulator.runSeconds(15); + + // THEN: then the fusion of both sensors starts to fail and the height is reset to the + // reference sensor (GNSS) + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); + + const Vector3f new_position = _ekf->getPosition(); + EXPECT_NEAR(new_position(2), previous_position(2) - gnss_height_step, 0.2f); + + // Also check the reset counters to make sure the reset logic triggered + reset_logging_checker.capturePostResetState(); + EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(1)); + EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1)); +} + +TEST_F(EkfHeightFusionTest, baroRefAllHgtFailReset) +{ + // GIVEN: EKF that fuses GNSS and baro (reference) + _sensor_simulator.startBaro(); + _sensor_simulator.startGps(); + _ekf_wrapper.setBaroHeightRef(); + _ekf_wrapper.enableBaroHeightFusion(); + _ekf_wrapper.enableGpsHeightFusion(); + + _sensor_simulator.runSeconds(11); + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); + EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::BARO); + + const Vector3f previous_position = _ekf->getPosition(); + + ResetLoggingChecker reset_logging_checker(_ekf); + reset_logging_checker.capturePreResetState(); + + // WHEN: + const float gnss_height_step = 10.f; + _sensor_simulator._gps.stepHeightByMeters(gnss_height_step); + + const float baro_height_step = 5.f; + _sensor_simulator._baro.setData(_sensor_simulator._baro.getData() + baro_height_step); + _sensor_simulator.runSeconds(20); + + // THEN: then the fusion of both sensors starts to fail and the height is reset to the + // reference sensor (baro) + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); + + const Vector3f new_position = _ekf->getPosition(); + EXPECT_NEAR(new_position(2), previous_position(2) - baro_height_step, 0.2f); + + // Also check the reset counters to make sure the reset logic triggered + reset_logging_checker.capturePostResetState(); + + // The velocity does not reset as baro only provides height measurement + EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(0)); + + // The height resets twice in a row as the baro innovation is not corrected after a height + // reset and triggers a new reset at the next iteration + EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(2)); +} + TEST_F(EkfHeightFusionTest, changeEkfOriginAlt) { _sensor_simulator.startBaro(); From 3157a4e1712e4e0c402e345b4315d93fad5799e1 Mon Sep 17 00:00:00 2001 From: Thomas Frans <160142177+flyingthingsintothings@users.noreply.github.com> Date: Wed, 7 Aug 2024 00:46:18 +0200 Subject: [PATCH 08/90] gnss: update supported baud rates (#23415) * gnss: update supported baud rates The Septentrio GNSS driver requires certain baud rates to test all the supported baud rates of the receiver. Without these changes, certain "non-standard" ones would print an error to the MAVLink console when the driver was started through the console. * platforms: add missing baudrate defines --------- Co-authored-by: Thomas Frans Co-authored-by: Julian Oes --- platforms/nuttx/src/px4/common/SerialImpl.cpp | 24 +++++++++++++++++++ platforms/posix/src/px4/common/SerialImpl.cpp | 24 +++++++++++++++++++ 2 files changed, 48 insertions(+) diff --git a/platforms/nuttx/src/px4/common/SerialImpl.cpp b/platforms/nuttx/src/px4/common/SerialImpl.cpp index 7fc5ea7520a2..4264976f4471 100644 --- a/platforms/nuttx/src/px4/common/SerialImpl.cpp +++ b/platforms/nuttx/src/px4/common/SerialImpl.cpp @@ -97,12 +97,36 @@ bool SerialImpl::configure() case 460800: speed = B460800; break; +#ifndef B500000 +#define B500000 500000 +#endif + + case 500000: speed = B500000; break; + +#ifndef B576000 +#define B576000 576000 +#endif + + case 576000: speed = B576000; break; + #ifndef B921600 #define B921600 921600 #endif case 921600: speed = B921600; break; +#ifndef B1000000 +#define B1000000 1000000 +#endif + + case 1000000: speed = B1000000; break; + +#ifndef B1500000 +#define B1500000 1500000 +#endif + + case 1500000: speed = B1500000; break; + default: speed = _baudrate; PX4_WARN("Using non-standard baudrate: %lu", _baudrate); diff --git a/platforms/posix/src/px4/common/SerialImpl.cpp b/platforms/posix/src/px4/common/SerialImpl.cpp index 822ed4255ec0..2298c3829263 100644 --- a/platforms/posix/src/px4/common/SerialImpl.cpp +++ b/platforms/posix/src/px4/common/SerialImpl.cpp @@ -90,12 +90,36 @@ bool SerialImpl::configure() case 460800: speed = B460800; break; +#ifndef B500000 +#define B500000 500000 +#endif + + case 500000: speed = B500000; break; + +#ifndef B576000 +#define B576000 576000 +#endif + + case 576000: speed = B576000; break; + #ifndef B921600 #define B921600 921600 #endif case 921600: speed = B921600; break; +#ifndef B1000000 +#define B1000000 1000000 +#endif + + case 1000000: speed = B1000000; break; + +#ifndef B1500000 +#define B1500000 1500000 +#endif + + case 1500000: speed = B1500000; break; + default: speed = _baudrate; PX4_WARN("Using non-standard baudrate: %u", _baudrate); From bfcd4564a6caf1549fcb681b162546f965181fc2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 7 Aug 2024 08:17:03 +0200 Subject: [PATCH 09/90] fix metadata.cmake: add missing paths to json & xml parameter outputs (#23464) --- cmake/metadata.cmake | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cmake/metadata.cmake b/cmake/metadata.cmake index 59ae6bd82990..ef946c703159 100644 --- a/cmake/metadata.cmake +++ b/cmake/metadata.cmake @@ -74,13 +74,13 @@ add_custom_target(metadata_parameters --markdown ${PX4_BINARY_DIR}/docs/parameters.md COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/src/lib/parameters/px_process_params.py - --src-path `find ${PX4_SOURCE_DIR}/src -maxdepth 4 -type d` + --src-path `find ${PX4_SOURCE_DIR}/src -maxdepth 4 -type d` ${generated_params_dir} --inject-xml ${PX4_SOURCE_DIR}/src/lib/parameters/parameters_injected.xml --json ${PX4_BINARY_DIR}/docs/parameters.json --compress COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/src/lib/parameters/px_process_params.py - --src-path `find ${PX4_SOURCE_DIR}/src -maxdepth 4 -type d` + --src-path `find ${PX4_SOURCE_DIR}/src -maxdepth 4 -type d` ${generated_params_dir} --inject-xml ${PX4_SOURCE_DIR}/src/lib/parameters/parameters_injected.xml --xml ${PX4_BINARY_DIR}/docs/parameters.xml From 33d99a13e835743943b15fe8a75c00acd2fe01b9 Mon Sep 17 00:00:00 2001 From: chfriedrich98 <125505139+chfriedrich98@users.noreply.github.com> Date: Wed, 7 Aug 2024 09:53:37 +0200 Subject: [PATCH 10/90] differential: restructure and update module (#23430) * differential: rename module * differential: restructure and update module --- .../init.d-posix/airframes/4009_gz_r1_rover | 24 +- .../init.d-posix/airframes/4011_gz_lawnmower | 12 +- ROMFS/px4fmu_common/init.d/CMakeLists.txt | 2 +- .../init.d/airframes/CMakeLists.txt | 2 +- .../init.d/rc.rover_differential_apps | 2 +- .../init.d/rc.rover_differential_defaults | 11 +- boards/px4/fmu-v5/rover.px4board | 3 +- boards/px4/fmu-v5x/default.px4board | 1 - boards/px4/fmu-v5x/rover.px4board | 2 + boards/px4/fmu-v6c/rover.px4board | 3 +- boards/px4/fmu-v6u/rover.px4board | 3 +- boards/px4/fmu-v6x/rover.px4board | 3 +- boards/px4/fmu-v6xrt/rover.px4board | 3 +- boards/px4/sitl/default.px4board | 2 +- msg/CMakeLists.txt | 3 +- msg/DifferentialDriveSetpoint.msg | 8 - msg/RoverDifferentialGuidanceStatus.msg | 10 + msg/RoverDifferentialStatus.msg | 8 + .../control_allocator/ControlAllocator.cpp | 2 +- .../differential_drive/DifferentialDrive.cpp | 191 ------------- .../DifferentialDriveControl/CMakeLists.txt | 39 --- .../DifferentialDriveControl.cpp | 109 -------- .../DifferentialDriveControl.hpp | 91 ------- .../DifferentialDriveGuidance.cpp | 138 ---------- .../DifferentialDriveGuidance.hpp | 140 ---------- .../CMakeLists.txt | 40 --- .../DifferentialDriveKinematics.cpp | 97 ------- .../DifferentialDriveKinematics.hpp | 103 ------- .../DifferentialDriveKinematicsTest.cpp | 168 ------------ src/modules/differential_drive/Kconfig | 6 - src/modules/logger/logged_topics.cpp | 4 +- .../CMakeLists.txt | 17 +- src/modules/rover_differential/Kconfig | 6 + .../rover_differential/RoverDifferential.cpp | 256 ++++++++++++++++++ .../RoverDifferential.hpp} | 83 ++++-- .../RoverDifferentialGuidance}/CMakeLists.txt | 7 +- .../RoverDifferentialGuidance.cpp | 247 +++++++++++++++++ .../RoverDifferentialGuidance.hpp | 163 +++++++++++ .../module.yaml | 130 ++++++--- src/modules/uxrce_dds_client/dds_topics.yaml | 3 - 40 files changed, 902 insertions(+), 1240 deletions(-) delete mode 100644 msg/DifferentialDriveSetpoint.msg create mode 100644 msg/RoverDifferentialGuidanceStatus.msg create mode 100644 msg/RoverDifferentialStatus.msg delete mode 100644 src/modules/differential_drive/DifferentialDrive.cpp delete mode 100644 src/modules/differential_drive/DifferentialDriveControl/CMakeLists.txt delete mode 100644 src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.cpp delete mode 100644 src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.hpp delete mode 100644 src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp delete mode 100644 src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp delete mode 100644 src/modules/differential_drive/DifferentialDriveKinematics/CMakeLists.txt delete mode 100644 src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp delete mode 100644 src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp delete mode 100644 src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp delete mode 100644 src/modules/differential_drive/Kconfig rename src/modules/{differential_drive => rover_differential}/CMakeLists.txt (84%) create mode 100644 src/modules/rover_differential/Kconfig create mode 100644 src/modules/rover_differential/RoverDifferential.cpp rename src/modules/{differential_drive/DifferentialDrive.hpp => rover_differential/RoverDifferential.hpp} (53%) rename src/modules/{differential_drive/DifferentialDriveGuidance => rover_differential/RoverDifferentialGuidance}/CMakeLists.txt (89%) create mode 100644 src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp create mode 100644 src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp rename src/modules/{differential_drive => rover_differential}/module.yaml (55%) 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 80664cf661c0..7697e23db769 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 @@ -6,16 +6,36 @@ . ${R}etc/init.d/rc.rover_differential_defaults PX4_SIMULATOR=${PX4_SIMULATOR:=gz} -PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} +PX4_GZ_WORLD=${PX4_GZ_WORLD:=rover} PX4_SIM_MODEL=${PX4_SIM_MODEL:=r1_rover} param set-default SIM_GZ_EN 1 # Gazebo bridge +# Rover parameters +param set-default RD_WHEEL_TRACK 0.3 +param set-default RD_MAN_YAW_SCALE 0.1 +param set-default RD_YAW_RATE_I 0.1 +param set-default RD_YAW_RATE_P 5 +param set-default RD_MAX_ACCEL 6 +param set-default RD_MAX_JERK 30 +param set-default RD_MAX_SPEED 7 +param set-default RD_HEADING_P 5 +param set-default RD_HEADING_I 0.1 +param set-default RD_MAX_YAW_RATE 180 +param set-default RD_MISS_SPD_DEF 7 +param set-default RD_TRANS_DRV_TRN 0.349066 +param set-default RD_TRANS_TRN_DRV 0.174533 + +# Pure pursuit parameters +param set-default PP_LOOKAHD_MAX 30 +param set-default PP_LOOKAHD_MIN 2 +param set-default PP_LOOKAHD_GAIN 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 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 1dcb917a09ad..01fc515c4370 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower @@ -20,9 +20,9 @@ param set-default SENS_EN_ARSPDSIM 1 param set-default COM_ARM_WO_GPS 1 # Set Differential Drive Kinematics Library parameters: -param set RDD_WHEEL_BASE 0.9 -param set RDD_WHEEL_RADIUS 0.22 -param set RDD_WHEEL_SPEED 10.0 # Maximum wheel speed rad/s, approx 8 km/h +param set RD_WHEEL_BASE 0.9 +param set RD_WHEEL_RADIUS 0.22 +param set RD_WHEEL_SPEED 10.0 # Maximum wheel speed rad/s, approx 8 km/h # Actuator mapping - set SITL motors/servos output parameters: @@ -41,9 +41,9 @@ param set-default SIM_GZ_WH_FUNC2 102 # left wheel param set-default SIM_GZ_WH_REV 0 # no need to reverse any wheels -# Note: The servo configurations ( SIM_GZ_SV_FUNC*) outlined below are intended for educational purposes in this simulation. -# They do not have physical effects in the simulated environment, except for actuating the joints. Their definitions are meant to demonstrate -# how actuators could be mapped and configured in a real-world application, providing a foundation for understanding and implementing actuator +# Note: The servo configurations ( SIM_GZ_SV_FUNC*) outlined below are intended for educational purposes in this simulation. +# They do not have physical effects in the simulated environment, except for actuating the joints. Their definitions are meant to demonstrate +# how actuators could be mapped and configured in a real-world application, providing a foundation for understanding and implementing actuator # controls in practical scenarios. # Cutter deck blades clutch, PCA9685 servo channel 3, "RC FLAPS" (406) - leftmost switch, or "Servo 3" (203): diff --git a/ROMFS/px4fmu_common/init.d/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/CMakeLists.txt index 24dea3fcda93..3319779096c5 100644 --- a/ROMFS/px4fmu_common/init.d/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/CMakeLists.txt @@ -78,7 +78,7 @@ if(CONFIG_MODULES_ROVER_POS_CONTROL) ) endif() -if(CONFIG_MODULES_DIFFERENTIAL_DRIVE) +if(CONFIG_MODULES_ROVER_DIFFERENTIAL) px4_add_romfs_files( rc.rover_differential_apps rc.rover_differential_defaults diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt index f763a4376606..c6bae3566a1d 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt @@ -143,7 +143,7 @@ if(CONFIG_MODULES_ROVER_POS_CONTROL) ) endif() -if(CONFIG_MODULES_DIFFERENTIAL_DRIVE) +if(CONFIG_MODULES_ROVER_DIFFERENTIAL) px4_add_romfs_files( 50003_aion_robotics_r1_rover ) diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps b/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps index 275664af7e26..4acbe8301404 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps +++ b/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps @@ -2,7 +2,7 @@ # Standard apps for a differential drive rover. # Start rover differential drive controller. -differential_drive start +rover_differential start # Start Land Detector. land_detector start rover diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_differential_defaults b/ROMFS/px4fmu_common/init.d/rc.rover_differential_defaults index e0cace3a3238..4cddd44f89e3 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_differential_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.rover_differential_defaults @@ -3,9 +3,8 @@ set VEHICLE_TYPE rover_differential -param set-default MAV_TYPE 10 # MAV_TYPE_GROUND_ROVER - -param set-default CA_AIRFRAME 6 # Rover (Differential) -param set-default CA_R_REV 3 # Right and left motors reversible - -param set-default EKF2_MAG_TYPE 1 # make sure magnetometer is fused even when not flying +param set-default MAV_TYPE 10 # MAV_TYPE_GROUND_ROVER +param set-default CA_AIRFRAME 6 # Rover (Differential) +param set-default CA_R_REV 3 # Right and left motors reversible +param set-default NAV_ACC_RAD 0.5 # Waypoint acceptance radius +param set-default EKF2_MAG_TYPE 1 # Make sure magnetometer is fused even when not flying diff --git a/boards/px4/fmu-v5/rover.px4board b/boards/px4/fmu-v5/rover.px4board index 92c265280ab9..07bfbe094121 100644 --- a/boards/px4/fmu-v5/rover.px4board +++ b/boards/px4/fmu-v5/rover.px4board @@ -17,5 +17,6 @@ CONFIG_MODULES_UUV_POS_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set -CONFIG_MODULES_DIFFERENTIAL_DRIVE=y +CONFIG_MODULES_ROVER_DIFFERENTIAL=y CONFIG_MODULES_ROVER_ACKERMANN=y +CONFIG_DRIVERS_ROBOCLAW=y diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index 46d04d8cc2eb..a48b0956876b 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -54,7 +54,6 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y diff --git a/boards/px4/fmu-v5x/rover.px4board b/boards/px4/fmu-v5x/rover.px4board index 738e6d9b0d0a..4a4458c2e6bc 100644 --- a/boards/px4/fmu-v5x/rover.px4board +++ b/boards/px4/fmu-v5x/rover.px4board @@ -14,4 +14,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set +CONFIG_MODULES_ROVER_DIFFERENTIAL=y CONFIG_MODULES_ROVER_ACKERMANN=y +CONFIG_DRIVERS_ROBOCLAW=y diff --git a/boards/px4/fmu-v6c/rover.px4board b/boards/px4/fmu-v6c/rover.px4board index 553bc76a52c7..7ded4008d8af 100644 --- a/boards/px4/fmu-v6c/rover.px4board +++ b/boards/px4/fmu-v6c/rover.px4board @@ -13,5 +13,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set -CONFIG_MODULES_DIFFERENTIAL_DRIVE=y +CONFIG_MODULES_ROVER_DIFFERENTIAL=y CONFIG_MODULES_ROVER_ACKERMANN=y +CONFIG_DRIVERS_ROBOCLAW=y diff --git a/boards/px4/fmu-v6u/rover.px4board b/boards/px4/fmu-v6u/rover.px4board index 553bc76a52c7..7ded4008d8af 100644 --- a/boards/px4/fmu-v6u/rover.px4board +++ b/boards/px4/fmu-v6u/rover.px4board @@ -13,5 +13,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set -CONFIG_MODULES_DIFFERENTIAL_DRIVE=y +CONFIG_MODULES_ROVER_DIFFERENTIAL=y CONFIG_MODULES_ROVER_ACKERMANN=y +CONFIG_DRIVERS_ROBOCLAW=y diff --git a/boards/px4/fmu-v6x/rover.px4board b/boards/px4/fmu-v6x/rover.px4board index 553bc76a52c7..7ded4008d8af 100644 --- a/boards/px4/fmu-v6x/rover.px4board +++ b/boards/px4/fmu-v6x/rover.px4board @@ -13,5 +13,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set -CONFIG_MODULES_DIFFERENTIAL_DRIVE=y +CONFIG_MODULES_ROVER_DIFFERENTIAL=y CONFIG_MODULES_ROVER_ACKERMANN=y +CONFIG_DRIVERS_ROBOCLAW=y diff --git a/boards/px4/fmu-v6xrt/rover.px4board b/boards/px4/fmu-v6xrt/rover.px4board index 101555842444..95a01552267c 100644 --- a/boards/px4/fmu-v6xrt/rover.px4board +++ b/boards/px4/fmu-v6xrt/rover.px4board @@ -14,5 +14,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set -CONFIG_MODULES_DIFFERENTIAL_DRIVE=y +CONFIG_MODULES_ROVER_DIFFERENTIAL=y CONFIG_MODULES_ROVER_ACKERMANN=y +CONFIG_DRIVERS_ROBOCLAW=y diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index d46a7cc7e70f..784be31db67a 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -13,7 +13,6 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE=y CONFIG_MODULES_EKF2=y CONFIG_EKF2_VERBOSE_STATUS=y CONFIG_EKF2_AUX_GLOBAL_POSITION=y @@ -47,6 +46,7 @@ CONFIG_MODULES_PAYLOAD_DELIVERER=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_REPLAY=y CONFIG_MODULES_ROVER_ACKERMANN=y +CONFIG_MODULES_ROVER_DIFFERENTIAL=y CONFIG_MODULES_ROVER_POS_CONTROL=y CONFIG_MODULES_SENSORS=y CONFIG_COMMON_SIMULATION=y diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 65ec54d8b01e..f0f6d520c701 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -71,7 +71,6 @@ set(msg_files DebugKeyValue.msg DebugValue.msg DebugVect.msg - DifferentialDriveSetpoint.msg DifferentialPressure.msg DistanceSensor.msg Ekf2Timestamps.msg @@ -181,6 +180,8 @@ set(msg_files RegisterExtComponentRequest.msg RoverAckermannGuidanceStatus.msg RoverAckermannStatus.msg + RoverDifferentialGuidanceStatus.msg + RoverDifferentialStatus.msg Rpm.msg RtlStatus.msg RtlTimeEstimate.msg diff --git a/msg/DifferentialDriveSetpoint.msg b/msg/DifferentialDriveSetpoint.msg deleted file mode 100644 index f7e4c5840099..000000000000 --- a/msg/DifferentialDriveSetpoint.msg +++ /dev/null @@ -1,8 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 speed # [m/s] collective roll-off speed in body x-axis -bool closed_loop_speed_control # true if speed is controlled using estimator feedback, false if direct feed-forward -float32 yaw_rate # [rad/s] yaw rate -bool closed_loop_yaw_rate_control # true if yaw rate is controlled using gyroscope feedback, false if direct feed-forward - -# TOPICS differential_drive_setpoint differential_drive_control_output diff --git a/msg/RoverDifferentialGuidanceStatus.msg b/msg/RoverDifferentialGuidanceStatus.msg new file mode 100644 index 000000000000..836546e7ebb7 --- /dev/null +++ b/msg/RoverDifferentialGuidanceStatus.msg @@ -0,0 +1,10 @@ +uint64 timestamp # time since system start (microseconds) + +float32 desired_speed # [m/s] Desired forward speed for the rover +float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller +float32 heading_error_deg # [deg] Heading error of the pure pursuit controller +float32 pid_heading_integral # Integral of the PID for the desired yaw rate during missions +float32 pid_throttle_integral # Integral of the PID for the throttle during missions +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/RoverDifferentialStatus.msg b/msg/RoverDifferentialStatus.msg new file mode 100644 index 000000000000..31907ffa6477 --- /dev/null +++ b/msg/RoverDifferentialStatus.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) + +float32 actual_speed # [m/s] Actual forward speed of the rover +float32 desired_yaw_rate_deg_s # [deg/s] Desired yaw rate +float32 actual_yaw_rate_deg_s # [deg/s] Actual yaw rate of the rover +float32 pid_yaw_rate_integral # Integral of the PID for the desired yaw rate controller + +# TOPICS rover_differential_status diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 3399bd619016..48363eeb1e38 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -239,7 +239,7 @@ ControlAllocator::update_effectiveness_source() break; case EffectivenessSource::ROVER_DIFFERENTIAL: - // differential_drive_control does allocation and publishes directly to actuator_motors topic + // rover_differential_control does allocation and publishes directly to actuator_motors topic break; case EffectivenessSource::FIXED_WING: diff --git a/src/modules/differential_drive/DifferentialDrive.cpp b/src/modules/differential_drive/DifferentialDrive.cpp deleted file mode 100644 index a1d34e01e7d2..000000000000 --- a/src/modules/differential_drive/DifferentialDrive.cpp +++ /dev/null @@ -1,191 +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 "DifferentialDrive.hpp" - -DifferentialDrive::DifferentialDrive() : - ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) -{ - updateParams(); -} - -bool DifferentialDrive::init() -{ - ScheduleOnInterval(10_ms); // 100 Hz - return true; -} - -void DifferentialDrive::updateParams() -{ - ModuleParams::updateParams(); - - _differential_drive_kinematics.setWheelBase(_param_rdd_wheel_base.get()); - - _max_speed = _param_rdd_wheel_speed.get() * _param_rdd_wheel_radius.get(); - _differential_drive_guidance.setMaxSpeed(_max_speed); - _differential_drive_kinematics.setMaxSpeed(_max_speed); - - _max_angular_velocity = _max_speed / (_param_rdd_wheel_base.get() / 2.f); - _differential_drive_guidance.setMaxAngularVelocity(_max_angular_velocity); - _differential_drive_kinematics.setMaxAngularVelocity(_max_angular_velocity); -} - -void DifferentialDrive::Run() -{ - if (should_exit()) { - ScheduleClear(); - exit_and_cleanup(); - return; - } - - hrt_abstime now = hrt_absolute_time(); - const float dt = math::min((now - _time_stamp_last), 5000_ms) / 1e6f; - _time_stamp_last = now; - - if (_parameter_update_sub.updated()) { - parameter_update_s parameter_update; - _parameter_update_sub.copy(¶meter_update); - updateParams(); - } - - if (_vehicle_control_mode_sub.updated()) { - vehicle_control_mode_s vehicle_control_mode{}; - - if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) { - _manual_driving = vehicle_control_mode.flag_control_manual_enabled; - _mission_driving = vehicle_control_mode.flag_control_auto_enabled; - } - } - - if (_vehicle_status_sub.updated()) { - vehicle_status_s vehicle_status{}; - - if (_vehicle_status_sub.copy(&vehicle_status)) { - const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); - const bool spooled_up = armed && (hrt_elapsed_time(&vehicle_status.armed_time) > _param_com_spoolup_time.get() * 1_s); - _differential_drive_kinematics.setArmed(spooled_up); - _acro_driving = (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO); - } - } - - if (_manual_driving) { - // Manual mode - // directly produce setpoints from the manual control setpoint (joystick) - if (_manual_control_setpoint_sub.updated()) { - manual_control_setpoint_s manual_control_setpoint{}; - - if (_manual_control_setpoint_sub.copy(&manual_control_setpoint)) { - differential_drive_setpoint_s setpoint{}; - setpoint.speed = manual_control_setpoint.throttle * math::max(0.f, _param_rdd_speed_scale.get()) * _max_speed; - setpoint.yaw_rate = manual_control_setpoint.roll * _param_rdd_ang_velocity_scale.get() * _max_angular_velocity; - - // if acro mode, we activate the yaw rate control - if (_acro_driving) { - setpoint.closed_loop_speed_control = false; - setpoint.closed_loop_yaw_rate_control = true; - - } else { - setpoint.closed_loop_speed_control = false; - setpoint.closed_loop_yaw_rate_control = false; - } - - setpoint.timestamp = now; - _differential_drive_setpoint_pub.publish(setpoint); - } - } - - } else if (_mission_driving) { - // Mission mode - // directly receive setpoints from the guidance library - _differential_drive_guidance.computeGuidance( - _differential_drive_control.getVehicleYaw(), - _differential_drive_control.getVehicleBodyYawRate(), - dt - ); - } - - _differential_drive_control.control(dt); - _differential_drive_kinematics.allocate(); -} - -int DifferentialDrive::task_spawn(int argc, char *argv[]) -{ - DifferentialDrive *instance = new DifferentialDrive(); - - if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; - - if (instance->init()) { - return PX4_OK; - } - - } else { - PX4_ERR("alloc failed"); - } - - delete instance; - _object.store(nullptr); - _task_id = -1; - - return PX4_ERROR; -} - -int DifferentialDrive::custom_command(int argc, char *argv[]) -{ - return print_usage("unknown command"); -} - -int DifferentialDrive::print_usage(const char *reason) -{ - if (reason) { - PX4_ERR("%s\n", reason); - } - - PRINT_MODULE_DESCRIPTION( - R"DESCR_STR( -### Description -Rover Differential Drive controller. -)DESCR_STR"); - - PRINT_MODULE_USAGE_NAME("differential_drive", "controller"); - PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); - return 0; -} - -extern "C" __EXPORT int differential_drive_main(int argc, char *argv[]) -{ - return DifferentialDrive::main(argc, argv); -} diff --git a/src/modules/differential_drive/DifferentialDriveControl/CMakeLists.txt b/src/modules/differential_drive/DifferentialDriveControl/CMakeLists.txt deleted file mode 100644 index 4610fa6c3d4d..000000000000 --- a/src/modules/differential_drive/DifferentialDriveControl/CMakeLists.txt +++ /dev/null @@ -1,39 +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. -# -############################################################################ - -px4_add_library(DifferentialDriveControl - DifferentialDriveControl.cpp -) - -target_link_libraries(DifferentialDriveControl PUBLIC pid) -target_include_directories(DifferentialDriveControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.cpp b/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.cpp deleted file mode 100644 index 728c5e666a10..000000000000 --- a/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.cpp +++ /dev/null @@ -1,109 +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 "DifferentialDriveControl.hpp" - -using namespace matrix; - -DifferentialDriveControl::DifferentialDriveControl(ModuleParams *parent) : ModuleParams(parent) -{ - pid_init(&_pid_angular_velocity, PID_MODE_DERIVATIV_NONE, 0.001f); - pid_init(&_pid_speed, PID_MODE_DERIVATIV_NONE, 0.001f); -} - -void DifferentialDriveControl::updateParams() -{ - ModuleParams::updateParams(); - - pid_set_parameters(&_pid_angular_velocity, - _param_rdd_p_gain_angular_velocity.get(), // Proportional gain - _param_rdd_i_gain_angular_velocity.get(), // Integral gain - 0, // Derivative gain - 20, // Integral limit - 200); // Output limit - - pid_set_parameters(&_pid_speed, - _param_rdd_p_gain_speed.get(), // Proportional gain - _param_rdd_i_gain_speed.get(), // Integral gain - 0, // Derivative gain - 2, // Integral limit - 200); // Output limit -} - -void DifferentialDriveControl::control(float dt) -{ - if (_vehicle_angular_velocity_sub.updated()) { - vehicle_angular_velocity_s vehicle_angular_velocity{}; - - if (_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity)) { - _vehicle_body_yaw_rate = vehicle_angular_velocity.xyz[2]; - } - } - - if (_vehicle_attitude_sub.updated()) { - vehicle_attitude_s vehicle_attitude{}; - - if (_vehicle_attitude_sub.copy(&vehicle_attitude)) { - _vehicle_attitude_quaternion = 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{}; - - if (_vehicle_local_position_sub.copy(&vehicle_local_position)) { - 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 = velocity_in_body_frame(0); - } - } - - _differential_drive_setpoint_sub.update(&_differential_drive_setpoint); - - // PID to reach setpoint using control_output - differential_drive_setpoint_s differential_drive_control_output = _differential_drive_setpoint; - - if (_differential_drive_setpoint.closed_loop_speed_control) { - differential_drive_control_output.speed += - pid_calculate(&_pid_speed, _differential_drive_setpoint.speed, _vehicle_forward_speed, 0, dt); - } - - if (_differential_drive_setpoint.closed_loop_yaw_rate_control) { - differential_drive_control_output.yaw_rate += - pid_calculate(&_pid_angular_velocity, _differential_drive_setpoint.yaw_rate, _vehicle_body_yaw_rate, 0, dt); - } - - differential_drive_control_output.timestamp = hrt_absolute_time(); - _differential_drive_control_output_pub.publish(differential_drive_control_output); -} diff --git a/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.hpp b/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.hpp deleted file mode 100644 index 8d3b7e1c216c..000000000000 --- a/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.hpp +++ /dev/null @@ -1,91 +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. - * - ****************************************************************************/ - -/** - * @file DifferentialDriveControl.hpp - * - * Controller for heading rate and forward speed. - */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -class DifferentialDriveControl : public ModuleParams -{ -public: - DifferentialDriveControl(ModuleParams *parent); - ~DifferentialDriveControl() = default; - - void control(float dt); - float getVehicleBodyYawRate() const { return _vehicle_body_yaw_rate; } - float getVehicleYaw() const { return _vehicle_yaw; } - -protected: - void updateParams() override; - -private: - uORB::Subscription _differential_drive_setpoint_sub{ORB_ID(differential_drive_setpoint)}; - 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::Publication _differential_drive_control_output_pub{ORB_ID(differential_drive_control_output)}; - - differential_drive_setpoint_s _differential_drive_setpoint{}; - - matrix::Quatf _vehicle_attitude_quaternion{}; - float _vehicle_yaw{0.f}; - - // States - float _vehicle_body_yaw_rate{0.f}; - float _vehicle_forward_speed{0.f}; - - PID_t _pid_angular_velocity; ///< The PID controller for yaw rate. - PID_t _pid_speed; ///< The PID controller for velocity. - - DEFINE_PARAMETERS( - (ParamFloat) _param_rdd_p_gain_speed, - (ParamFloat) _param_rdd_i_gain_speed, - (ParamFloat) _param_rdd_p_gain_angular_velocity, - (ParamFloat) _param_rdd_i_gain_angular_velocity - ) -}; diff --git a/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp b/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp deleted file mode 100644 index df453ff8b5c0..000000000000 --- a/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp +++ /dev/null @@ -1,138 +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 "DifferentialDriveGuidance.hpp" - -#include - -using namespace matrix; - -DifferentialDriveGuidance::DifferentialDriveGuidance(ModuleParams *parent) : ModuleParams(parent) -{ - updateParams(); - - pid_init(&_heading_p_controller, PID_MODE_DERIVATIV_NONE, 0.001f); -} - -void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocity, float dt) -{ - if (_position_setpoint_triplet_sub.updated()) { - _position_setpoint_triplet_sub.copy(&_position_setpoint_triplet); - } - - if (_vehicle_global_position_sub.updated()) { - _vehicle_global_position_sub.copy(&_vehicle_global_position); - } - - matrix::Vector2d global_position(_vehicle_global_position.lat, _vehicle_global_position.lon); - matrix::Vector2d current_waypoint(_position_setpoint_triplet.current.lat, _position_setpoint_triplet.current.lon); - matrix::Vector2d next_waypoint(_position_setpoint_triplet.next.lat, _position_setpoint_triplet.next.lon); - - const float distance_to_next_wp = get_distance_to_next_waypoint(global_position(0), global_position(1), - current_waypoint(0), - current_waypoint(1)); - - float desired_heading = get_bearing_to_next_waypoint(global_position(0), global_position(1), current_waypoint(0), - current_waypoint(1)); - float heading_error = matrix::wrap_pi(desired_heading - yaw); - - if (_current_waypoint != current_waypoint) { - _currentState = GuidanceState::TURNING; - } - - // Make rover stop when it arrives at the last waypoint instead of loitering and driving around weirdly. - if ((current_waypoint == next_waypoint) && distance_to_next_wp <= _param_nav_acc_rad.get()) { - _currentState = GuidanceState::GOAL_REACHED; - } - - float desired_speed = 0.f; - - switch (_currentState) { - case GuidanceState::TURNING: - desired_speed = 0.f; - - if (fabsf(heading_error) < 0.05f) { - _currentState = GuidanceState::DRIVING; - } - - break; - - case GuidanceState::DRIVING: { - const float max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_rdd_max_jerk.get(), - _param_rdd_max_accel.get(), distance_to_next_wp, 0.0f); - _forwards_velocity_smoothing.updateDurations(max_velocity); - _forwards_velocity_smoothing.updateTraj(dt); - desired_speed = math::interpolate(abs(heading_error), 0.1f, 0.8f, - _forwards_velocity_smoothing.getCurrentVelocity(), 0.0f); - break; - } - - case GuidanceState::GOAL_REACHED: - // temporary till I find a better way to stop the vehicle - desired_speed = 0.f; - heading_error = 0.f; - angular_velocity = 0.f; - _desired_angular_velocity = 0.f; - break; - } - - // Compute the desired angular velocity relative to the heading error, to steer the vehicle towards the next waypoint. - float angular_velocity_pid = pid_calculate(&_heading_p_controller, heading_error, angular_velocity, 0, - dt) + heading_error; - - differential_drive_setpoint_s output{}; - output.speed = math::constrain(desired_speed, -_max_speed, _max_speed); - output.yaw_rate = math::constrain(angular_velocity_pid, -_max_angular_velocity, _max_angular_velocity); - output.closed_loop_speed_control = output.closed_loop_yaw_rate_control = true; - output.timestamp = hrt_absolute_time(); - - _differential_drive_setpoint_pub.publish(output); - - _current_waypoint = current_waypoint; -} - -void DifferentialDriveGuidance::updateParams() -{ - ModuleParams::updateParams(); - - pid_set_parameters(&_heading_p_controller, - _param_rdd_p_gain_heading.get(), // Proportional gain - 0, // Integral gain - 0, // Derivative gain - 0, // Integral limit - 200); // Output limit - - _forwards_velocity_smoothing.setMaxJerk(_param_rdd_max_jerk.get()); - _forwards_velocity_smoothing.setMaxAccel(_param_rdd_max_accel.get()); - _forwards_velocity_smoothing.setMaxVel(_max_speed); -} diff --git a/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp b/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp deleted file mode 100644 index 0a5a29bbca0b..000000000000 --- a/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp +++ /dev/null @@ -1,140 +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. - * - ****************************************************************************/ - -#pragma once - -#include - -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include - -#include - - -/** - * @brief Enum class for the different states of guidance. - */ -enum class GuidanceState { - TURNING, ///< The vehicle is currently turning. - DRIVING, ///< The vehicle is currently driving straight. - GOAL_REACHED ///< The vehicle has reached its goal. -}; - -/** - * @brief Class for differential drive guidance. - */ -class DifferentialDriveGuidance : public ModuleParams -{ -public: - /** - * @brief Constructor for DifferentialDriveGuidance. - * @param parent The parent ModuleParams object. - */ - DifferentialDriveGuidance(ModuleParams *parent); - ~DifferentialDriveGuidance() = default; - - /** - * @brief Compute guidance for the vehicle. - * @param global_pos The global position of the vehicle in degrees. - * @param current_waypoint The current waypoint the vehicle is heading towards in degrees. - * @param next_waypoint The next waypoint the vehicle will head towards after reaching the current waypoint in degrees. - * @param vehicle_yaw The yaw orientation of the vehicle in radians. - * @param body_velocity The velocity of the vehicle in m/s. - * @param angular_velocity The angular velocity of the vehicle in rad/s. - * @param dt The time step in seconds. - */ - void computeGuidance(float yaw, float angular_velocity, float dt); - - /** - * @brief Set the maximum speed for the vehicle. - * @param max_speed The maximum speed in m/s. - * @return The set maximum speed in m/s. - */ - float setMaxSpeed(float max_speed) { return _max_speed = max_speed; } - - - /** - * @brief Set the maximum angular velocity for the vehicle. - * @param max_angular_velocity The maximum angular velocity in rad/s. - * @return The set maximum angular velocity in rad/s. - */ - float setMaxAngularVelocity(float max_angular_velocity) { return _max_angular_velocity = max_angular_velocity; } - -protected: - /** - * @brief Update the parameters of the module. - */ - void updateParams() override; - -private: - uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; - uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; - - uORB::Publication _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)}; - - position_setpoint_triplet_s _position_setpoint_triplet{}; - vehicle_global_position_s _vehicle_global_position{}; - - GuidanceState _currentState; ///< The current state of guidance. - - float _desired_angular_velocity; ///< The desired angular velocity. - - float _max_speed; ///< The maximum speed. - float _max_angular_velocity; ///< The maximum angular velocity. - - matrix::Vector2d _current_waypoint; ///< The current waypoint. - - VelocitySmoothing _forwards_velocity_smoothing; ///< The velocity smoothing for forward motion. - PositionSmoothing _position_smoothing; ///< The position smoothing. - - PID_t _heading_p_controller; ///< The PID controller for yaw rate. - - DEFINE_PARAMETERS( - (ParamFloat) _param_rdd_p_gain_heading, - (ParamFloat) _param_nav_acc_rad, - (ParamFloat) _param_rdd_max_jerk, - (ParamFloat) _param_rdd_max_accel - ) -}; diff --git a/src/modules/differential_drive/DifferentialDriveKinematics/CMakeLists.txt b/src/modules/differential_drive/DifferentialDriveKinematics/CMakeLists.txt deleted file mode 100644 index c556db2b8f3f..000000000000 --- a/src/modules/differential_drive/DifferentialDriveKinematics/CMakeLists.txt +++ /dev/null @@ -1,40 +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. -# -############################################################################ - -px4_add_library(DifferentialDriveKinematics - DifferentialDriveKinematics.cpp -) - -target_include_directories(DifferentialDriveKinematics PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) - -px4_add_functional_gtest(SRC DifferentialDriveKinematicsTest.cpp LINKLIBS DifferentialDriveKinematics) diff --git a/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp b/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp deleted file mode 100644 index 42fcbea56b66..000000000000 --- a/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp +++ /dev/null @@ -1,97 +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 "DifferentialDriveKinematics.hpp" - -#include - -using namespace matrix; -using namespace time_literals; - -DifferentialDriveKinematics::DifferentialDriveKinematics(ModuleParams *parent) : ModuleParams(parent) -{} - -void DifferentialDriveKinematics::allocate() -{ - hrt_abstime now = hrt_absolute_time(); - - if (_differential_drive_control_output_sub.updated()) { - _differential_drive_control_output_sub.copy(&_differential_drive_control_output); - } - - const bool setpoint_timeout = (_differential_drive_control_output.timestamp + 100_ms) < now; - - Vector2f wheel_speeds = - computeInverseKinematics(_differential_drive_control_output.speed, _differential_drive_control_output.yaw_rate); - - if (!_armed || setpoint_timeout) { - wheel_speeds = {}; // stop - } - - wheel_speeds = matrix::constrain(wheel_speeds, -1.f, 1.f); - - actuator_motors_s actuator_motors{}; - actuator_motors.reversible_flags = _param_r_rev.get(); // should be 3 see rc.rover_differential_defaults - wheel_speeds.copyTo(actuator_motors.control); - actuator_motors.timestamp = now; - _actuator_motors_pub.publish(actuator_motors); -} - -matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float linear_velocity_x, float yaw_rate) const -{ - if (_max_speed < FLT_EPSILON) { - return Vector2f(); - } - - linear_velocity_x = math::constrain(linear_velocity_x, -_max_speed, _max_speed); - yaw_rate = math::constrain(yaw_rate, -_max_angular_velocity, _max_angular_velocity); - - const float rotational_velocity = (_wheel_base / 2.f) * yaw_rate; - float combined_velocity = fabsf(linear_velocity_x) + fabsf(rotational_velocity); - - // Compute an initial gain - float gain = 1.0f; - - if (combined_velocity > _max_speed) { - float excess_velocity = fabsf(combined_velocity - _max_speed); - const float adjusted_linear_velocity = fabsf(linear_velocity_x) - excess_velocity; - gain = adjusted_linear_velocity / fabsf(linear_velocity_x); - } - - // Apply the gain - linear_velocity_x *= gain; - - // Calculate the left and right wheel speeds - return Vector2f(linear_velocity_x - rotational_velocity, - linear_velocity_x + rotational_velocity) / _max_speed; -} diff --git a/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp b/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp deleted file mode 100644 index 524a5520a78f..000000000000 --- a/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp +++ /dev/null @@ -1,103 +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. - * - ****************************************************************************/ - -#pragma once - -#include -#include -#include -#include -#include -#include - -/** - * @brief Differential Drive Kinematics class for computing the kinematics of a differential drive robot. - * - * This class provides functions to set the wheel base and radius, and to compute the inverse kinematics - * given linear velocity and yaw rate. - */ -class DifferentialDriveKinematics : public ModuleParams -{ -public: - DifferentialDriveKinematics(ModuleParams *parent); - ~DifferentialDriveKinematics() = default; - - /** - * @brief Sets the wheel base of the robot. - * - * @param wheel_base The distance between the centers of the wheels. - */ - void setWheelBase(const float wheel_base) { _wheel_base = wheel_base; }; - - /** - * @brief Sets the maximum speed of the robot. - * - * @param max_speed The maximum speed of the robot. - */ - void setMaxSpeed(const float max_speed) { _max_speed = max_speed; }; - - /** - * @brief Sets the maximum angular speed of the robot. - * - * @param max_angular_speed The maximum angular speed of the robot. - */ - void setMaxAngularVelocity(const float max_angular_velocity) { _max_angular_velocity = max_angular_velocity; }; - - void setArmed(const bool armed) { _armed = armed; }; - - void allocate(); - - /** - * @brief Computes the inverse kinematics for differential drive. - * - * @param linear_velocity_x Linear velocity along the x-axis. - * @param yaw_rate Yaw rate of the robot. - * @return matrix::Vector2f Motor velocities for the right and left motors. - */ - matrix::Vector2f computeInverseKinematics(float linear_velocity_x, float yaw_rate) const; - -private: - uORB::Subscription _differential_drive_control_output_sub{ORB_ID(differential_drive_control_output)}; - uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; - - differential_drive_setpoint_s _differential_drive_control_output{}; - bool _armed = false; - - float _wheel_base{0.f}; - float _max_speed{0.f}; - float _max_angular_velocity{0.f}; - - DEFINE_PARAMETERS( - (ParamInt) _param_r_rev - ) -}; diff --git a/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp b/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp deleted file mode 100644 index b3c4935c1d9d..000000000000 --- a/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp +++ /dev/null @@ -1,168 +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 -#include "DifferentialDriveKinematics.hpp" -#include - -using namespace matrix; - -class DifferentialDriveKinematicsTest : public ::testing::Test -{ -public: - DifferentialDriveKinematics kinematics{nullptr}; -}; - -TEST_F(DifferentialDriveKinematicsTest, AllZeroInputCase) -{ - kinematics.setWheelBase(1.f); - kinematics.setMaxSpeed(10.f); - kinematics.setMaxAngularVelocity(10.f); - - // Test with zero linear velocity and zero yaw rate (stationary vehicle) - EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 0.f), Vector2f()); -} - - -TEST_F(DifferentialDriveKinematicsTest, InvalidParameterCase) -{ - kinematics.setWheelBase(0.f); - kinematics.setMaxSpeed(10.f); - kinematics.setMaxAngularVelocity(10.f); - - // Test with invalid parameters (zero wheel base and wheel radius) - EXPECT_EQ(kinematics.computeInverseKinematics(0.f, .1f), Vector2f()); -} - - -TEST_F(DifferentialDriveKinematicsTest, UnitCase) -{ - kinematics.setWheelBase(1.f); - kinematics.setMaxSpeed(10.f); - kinematics.setMaxAngularVelocity(10.f); - - // Test with unit values for linear velocity and yaw rate - EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 1.f), Vector2f(0.05f, 0.15f)); -} - - -TEST_F(DifferentialDriveKinematicsTest, UnitSaturationCase) -{ - kinematics.setWheelBase(1.f); - kinematics.setMaxSpeed(1.f); - kinematics.setMaxAngularVelocity(1.f); - - // Test with unit values for linear velocity and yaw rate, but with max speed that requires saturation - EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 1.f), Vector2f(0, 1)); -} - - -TEST_F(DifferentialDriveKinematicsTest, OppositeUnitSaturationCase) -{ - kinematics.setWheelBase(1.f); - kinematics.setMaxSpeed(1.f); - kinematics.setMaxAngularVelocity(1.f); - - // Negative linear velocity for backward motion and positive yaw rate for turning right - EXPECT_EQ(kinematics.computeInverseKinematics(-1.f, 1.f), Vector2f(-1, 0)); -} - -TEST_F(DifferentialDriveKinematicsTest, RandomCase) -{ - kinematics.setWheelBase(2.f); - kinematics.setMaxSpeed(1.f); - kinematics.setMaxAngularVelocity(1.f); - - // Negative linear velocity for backward motion and positive yaw rate for turning right - EXPECT_EQ(kinematics.computeInverseKinematics(0.5f, 0.7f), Vector2f(-0.4f, 1.0f)); -} - -TEST_F(DifferentialDriveKinematicsTest, RotateInPlaceCase) -{ - kinematics.setWheelBase(1.f); - kinematics.setMaxSpeed(1.f); - kinematics.setMaxAngularVelocity(1.f); - - // Test rotating in place (zero linear velocity, non-zero yaw rate) - EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 1.f), Vector2f(-0.5f, 0.5f)); -} - -TEST_F(DifferentialDriveKinematicsTest, StraightMovementCase) -{ - kinematics.setWheelBase(1.f); - kinematics.setMaxSpeed(1.f); - kinematics.setMaxAngularVelocity(1.f); - - // Test moving straight (non-zero linear velocity, zero yaw rate) - EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 0.f), Vector2f(1.f, 1.f)); -} - -TEST_F(DifferentialDriveKinematicsTest, MinInputValuesCase) -{ - kinematics.setWheelBase(FLT_MIN); - kinematics.setMaxSpeed(FLT_MIN); - kinematics.setMaxAngularVelocity(FLT_MIN); - - // Test with minimum possible input values - EXPECT_EQ(kinematics.computeInverseKinematics(FLT_MIN, FLT_MIN), Vector2f(0.f, 0.f)); -} - -TEST_F(DifferentialDriveKinematicsTest, MaxSpeedLimitCase) -{ - kinematics.setWheelBase(1.f); - kinematics.setMaxSpeed(1.f); - kinematics.setMaxAngularVelocity(1.f); - - // Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed - EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 10.f), Vector2f(0.f, 1.f)); -} - -TEST_F(DifferentialDriveKinematicsTest, MaxSpeedForwardsCase) -{ - kinematics.setWheelBase(1.f); - kinematics.setMaxSpeed(1.f); - kinematics.setMaxAngularVelocity(1.f); - - // Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed - EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 0.f), Vector2f(1.f, 1.f)); -} - -TEST_F(DifferentialDriveKinematicsTest, MaxAngularCase) -{ - kinematics.setWheelBase(2.f); - kinematics.setMaxSpeed(1.f); - kinematics.setMaxAngularVelocity(1.f); - - // Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed - EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 10.f), Vector2f(-1.f, 1.f)); -} diff --git a/src/modules/differential_drive/Kconfig b/src/modules/differential_drive/Kconfig deleted file mode 100644 index a95897e91f99..000000000000 --- a/src/modules/differential_drive/Kconfig +++ /dev/null @@ -1,6 +0,0 @@ -menuconfig MODULES_DIFFERENTIAL_DRIVE - bool "differential_drive" - default n - depends on MODULES_CONTROL_ALLOCATOR - ---help--- - Enable support for control of differential drive rovers diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index fc187a954d0e..24b5004ee9af 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -58,8 +58,6 @@ void LoggedTopics::add_default_topics() add_topic("commander_state"); add_topic("config_overrides"); add_topic("cpuload"); - add_optional_topic("differential_drive_control_output", 100); - add_optional_topic("differential_drive_setpoint", 100); add_optional_topic("external_ins_attitude"); add_optional_topic("external_ins_global_position"); add_optional_topic("external_ins_local_position"); @@ -105,6 +103,8 @@ void LoggedTopics::add_default_topics() add_topic("radio_status"); add_optional_topic("rover_ackermann_guidance_status", 100); add_optional_topic("rover_ackermann_status", 100); + add_optional_topic("rover_differential_guidance_status", 100); + add_optional_topic("rover_differential_status", 100); add_topic("rtl_time_estimate", 1000); add_topic("rtl_status", 2000); add_optional_topic("sensor_airflow", 100); diff --git a/src/modules/differential_drive/CMakeLists.txt b/src/modules/rover_differential/CMakeLists.txt similarity index 84% rename from src/modules/differential_drive/CMakeLists.txt rename to src/modules/rover_differential/CMakeLists.txt index 2e0e3583b9af..beaec32a8776 100644 --- a/src/modules/differential_drive/CMakeLists.txt +++ b/src/modules/rover_differential/CMakeLists.txt @@ -31,22 +31,19 @@ # ############################################################################ -add_subdirectory(DifferentialDriveControl) -add_subdirectory(DifferentialDriveGuidance) -add_subdirectory(DifferentialDriveKinematics) +add_subdirectory(RoverDifferentialGuidance) px4_add_module( - MODULE modules__differential_drive - MAIN differential_drive + MODULE modules__rover_differential + MAIN rover_differential SRCS - DifferentialDrive.cpp - DifferentialDrive.hpp + RoverDifferential.cpp + RoverDifferential.hpp DEPENDS - DifferentialDriveControl - DifferentialDriveGuidance - DifferentialDriveKinematics + RoverDifferentialGuidance px4_work_queue modules__control_allocator # for parameter CA_R_REV + pure_pursuit MODULE_CONFIG module.yaml ) diff --git a/src/modules/rover_differential/Kconfig b/src/modules/rover_differential/Kconfig new file mode 100644 index 000000000000..840e2cdbf98f --- /dev/null +++ b/src/modules/rover_differential/Kconfig @@ -0,0 +1,6 @@ +menuconfig MODULES_ROVER_DIFFERENTIAL + bool "rover_differential" + default n + depends on MODULES_CONTROL_ALLOCATOR + ---help--- + Enable support for control of differential rovers diff --git a/src/modules/rover_differential/RoverDifferential.cpp b/src/modules/rover_differential/RoverDifferential.cpp new file mode 100644 index 000000000000..ab89392a0ec8 --- /dev/null +++ b/src/modules/rover_differential/RoverDifferential.cpp @@ -0,0 +1,256 @@ +/**************************************************************************** + * + * 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 "RoverDifferential.hpp" +using namespace matrix; +using namespace time_literals; + +RoverDifferential::RoverDifferential() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) +{ + updateParams(); + _rover_differential_status_pub.advertise(); + pid_init(&_pid_yaw_rate, PID_MODE_DERIVATIV_NONE, 0.001f); +} + +bool RoverDifferential::init() +{ + ScheduleOnInterval(10_ms); // 100 Hz + return true; +} + +void RoverDifferential::updateParams() +{ + ModuleParams::updateParams(); + + _max_yaw_rate = _param_rd_max_yaw_rate.get() * M_DEG_TO_RAD_F; + + pid_set_parameters(&_pid_yaw_rate, + _param_rd_p_gain_yaw_rate.get(), // Proportional gain + _param_rd_i_gain_yaw_rate.get(), // Integral gain + 0.f, // Derivative gain + 1.f, // Integral limit + 1.f); // Output limit + +} + +void RoverDifferential::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + return; + } + + hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + // uORB subscriber updates + 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); + _nav_state = vehicle_status.nav_state; + } + + if (_vehicle_angular_velocity_sub.updated()) { + vehicle_angular_velocity_s vehicle_angular_velocity{}; + _vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity); + _vehicle_body_yaw_rate = vehicle_angular_velocity.xyz[2]; + } + + 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); + 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 = velocity_in_body_frame(0); + } + + // Navigation modes + 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)) { + _differential_setpoint.throttle = manual_control_setpoint.throttle; + _differential_setpoint.yaw_rate = manual_control_setpoint.roll * _param_rd_man_yaw_scale.get(); + + } + + _differential_setpoint.closed_loop_yaw_rate = false; + } break; + + case vehicle_status_s::NAVIGATION_STATE_ACRO: { + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + _differential_setpoint.throttle = manual_control_setpoint.throttle; + _differential_setpoint.yaw_rate = math::interpolate(manual_control_setpoint.roll, + -1.f, 1.f, + -_max_yaw_rate, _max_yaw_rate); + } + + _differential_setpoint.closed_loop_yaw_rate = true; + } break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: + _differential_setpoint = _rover_differential_guidance.computeGuidance(_vehicle_yaw, _vehicle_forward_speed, + _nav_state); + break; + + default: // Unimplemented nav states will stop the rover + _differential_setpoint.throttle = 0.f; + _differential_setpoint.yaw_rate = 0.f; + _differential_setpoint.closed_loop_yaw_rate = false; + break; + } + + float speed_diff_normalized = _differential_setpoint.yaw_rate; + + // Closed loop yaw rate control + if (_differential_setpoint.closed_loop_yaw_rate) { + if (fabsf(_differential_setpoint.yaw_rate - _vehicle_body_yaw_rate) < YAW_RATE_ERROR_THRESHOLD) { + speed_diff_normalized = 0.f; + pid_reset_integral(&_pid_yaw_rate); + + } else { + const float speed_diff = _differential_setpoint.yaw_rate * _param_rd_wheel_track.get(); // Feedforward + speed_diff_normalized = math::interpolate(speed_diff, -_param_rd_max_speed.get(), + _param_rd_max_speed.get(), -1.f, 1.f); + speed_diff_normalized = math::constrain(speed_diff_normalized + + pid_calculate(&_pid_yaw_rate, _differential_setpoint.yaw_rate, _vehicle_body_yaw_rate, 0, dt), + -1.f, 1.f); // Feedback + } + + } else { + pid_reset_integral(&_pid_yaw_rate); + } + + // Publish rover differential status (logging) + rover_differential_status_s rover_differential_status{}; + rover_differential_status.timestamp = _timestamp; + rover_differential_status.actual_speed = _vehicle_forward_speed; + rover_differential_status.desired_yaw_rate_deg_s = M_RAD_TO_DEG_F * _differential_setpoint.yaw_rate; + rover_differential_status.actual_yaw_rate_deg_s = M_RAD_TO_DEG_F * _vehicle_body_yaw_rate; + rover_differential_status.pid_yaw_rate_integral = _pid_yaw_rate.integral; + _rover_differential_status_pub.publish(rover_differential_status); + + // Publish to motors + actuator_motors_s actuator_motors{}; + actuator_motors.reversible_flags = _param_r_rev.get(); + computeMotorCommands(_differential_setpoint.throttle, speed_diff_normalized).copyTo(actuator_motors.control); + actuator_motors.timestamp = _timestamp; + _actuator_motors_pub.publish(actuator_motors); + +} + +matrix::Vector2f RoverDifferential::computeMotorCommands(float forward_speed, const float speed_diff) +{ + float combined_velocity = fabsf(forward_speed) + fabsf(speed_diff); + + if (combined_velocity > 1.0f) { // Prioritize yaw rate + float excess_velocity = fabsf(combined_velocity - 1.0f); + forward_speed -= sign(forward_speed) * excess_velocity; + } + + // Calculate the left and right wheel speeds + return Vector2f(forward_speed - speed_diff, + forward_speed + speed_diff); +} + +int RoverDifferential::task_spawn(int argc, char *argv[]) +{ + RoverDifferential *instance = new RoverDifferential(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int RoverDifferential::custom_command(int argc, char *argv[]) +{ + return print_usage("unk_timestampn command"); +} + +int RoverDifferential::print_usage(const char *reason) +{ + if (reason) { + PX4_ERR("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Rover Differential controller. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("rover_differential", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + return 0; +} + +extern "C" __EXPORT int rover_differential_main(int argc, char *argv[]) +{ + return RoverDifferential::main(argc, argv); +} diff --git a/src/modules/differential_drive/DifferentialDrive.hpp b/src/modules/rover_differential/RoverDifferential.hpp similarity index 53% rename from src/modules/differential_drive/DifferentialDrive.hpp rename to src/modules/rover_differential/RoverDifferential.hpp index 255bb0c52607..3dafe99b0566 100644 --- a/src/modules/differential_drive/DifferentialDrive.hpp +++ b/src/modules/rover_differential/RoverDifferential.hpp @@ -33,31 +33,41 @@ #pragma once +// PX4 includes #include #include #include #include #include + +// uORB includes #include +#include #include -#include #include #include -#include #include +#include +#include +#include +#include +#include + +// Standard libraries +#include +#include -#include "DifferentialDriveControl/DifferentialDriveControl.hpp" -#include "DifferentialDriveGuidance/DifferentialDriveGuidance.hpp" -#include "DifferentialDriveKinematics/DifferentialDriveKinematics.hpp" +// Local includes +#include "RoverDifferentialGuidance/RoverDifferentialGuidance.hpp" using namespace time_literals; -class DifferentialDrive : public ModuleBase, public ModuleParams, +class RoverDifferential : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: - DifferentialDrive(); - ~DifferentialDrive() override = default; + RoverDifferential(); + ~RoverDifferential() override = default; /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); @@ -70,36 +80,57 @@ class DifferentialDrive : public ModuleBase, public ModulePar bool init(); + /** + * @brief Computes motor commands for differential drive. + * + * @param forward_speed Linear velocity along the x-axis. + * @param speed_diff Speed difference between left and right wheels. + * @return matrix::Vector2f Motor velocities for the right and left motors. + */ + matrix::Vector2f computeMotorCommands(float forward_speed, const float speed_diff); + protected: void updateParams() override; private: void Run() override; + + // uORB Subscriptions uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; - uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; - + 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::Publication _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)}; - bool _manual_driving = false; - bool _mission_driving = false; - bool _acro_driving = false; - hrt_abstime _time_stamp_last{0}; /**< time stamp when task was last updated */ + // uORB Publications + uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; + uORB::Publication _rover_differential_status_pub{ORB_ID(rover_differential_status)}; + + // Instances + RoverDifferentialGuidance _rover_differential_guidance{this}; - DifferentialDriveGuidance _differential_drive_guidance{this}; - DifferentialDriveControl _differential_drive_control{this}; - DifferentialDriveKinematics _differential_drive_kinematics{this}; + // Variables + float _vehicle_body_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}; + matrix::Quatf _vehicle_attitude_quaternion{}; + hrt_abstime _timestamp{0}; + PID_t _pid_yaw_rate; // The PID controller for yaw rate + RoverDifferentialGuidance::differential_setpoint _differential_setpoint; - float _max_speed{0.f}; - float _max_angular_velocity{0.f}; + // Constants + static constexpr float YAW_RATE_ERROR_THRESHOLD = 0.1f; // [rad/s] Error threshold for the closed loop yaw rate control DEFINE_PARAMETERS( - (ParamFloat) _param_rdd_ang_velocity_scale, - (ParamFloat) _param_rdd_speed_scale, - (ParamFloat) _param_rdd_wheel_base, - (ParamFloat) _param_rdd_wheel_speed, - (ParamFloat) _param_rdd_wheel_radius, - (ParamFloat) _param_com_spoolup_time + (ParamFloat) _param_rd_man_yaw_scale, + (ParamFloat) _param_rd_wheel_track, + (ParamFloat) _param_rd_p_gain_yaw_rate, + (ParamFloat) _param_rd_i_gain_yaw_rate, + (ParamFloat) _param_rd_max_speed, + (ParamFloat) _param_rd_max_yaw_rate, + (ParamInt) _param_r_rev ) }; diff --git a/src/modules/differential_drive/DifferentialDriveGuidance/CMakeLists.txt b/src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt similarity index 89% rename from src/modules/differential_drive/DifferentialDriveGuidance/CMakeLists.txt rename to src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt index fa833c50ae42..0fd7b68c394e 100644 --- a/src/modules/differential_drive/DifferentialDriveGuidance/CMakeLists.txt +++ b/src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt @@ -31,8 +31,9 @@ # ############################################################################ -px4_add_library(DifferentialDriveGuidance - DifferentialDriveGuidance.cpp +px4_add_library(RoverDifferentialGuidance + RoverDifferentialGuidance.cpp ) -target_include_directories(DifferentialDriveGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(RoverDifferentialGuidance PUBLIC pid) +target_include_directories(RoverDifferentialGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp new file mode 100644 index 000000000000..efad52e7b874 --- /dev/null +++ b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp @@ -0,0 +1,247 @@ +/**************************************************************************** + * + * 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 "RoverDifferentialGuidance.hpp" + +#include + +using namespace matrix; +using namespace time_literals; + +RoverDifferentialGuidance::RoverDifferentialGuidance(ModuleParams *parent) : ModuleParams(parent) +{ + updateParams(); + _rover_differential_guidance_status_pub.advertise(); + pid_init(&_pid_heading, PID_MODE_DERIVATIV_NONE, 0.001f); + pid_init(&_pid_throttle, PID_MODE_DERIVATIV_NONE, 0.001f); + +} + +void RoverDifferentialGuidance::updateParams() +{ + ModuleParams::updateParams(); + + _max_yaw_rate = _param_rd_max_yaw_rate.get() * M_DEG_TO_RAD_F; + pid_set_parameters(&_pid_heading, + _param_rd_p_gain_heading.get(), // Proportional gain + _param_rd_i_gain_heading.get(), // Integral gain + 0.f, // Derivative gain + _max_yaw_rate, // Integral limit + _max_yaw_rate); // Output limit + pid_set_parameters(&_pid_throttle, + _param_rd_p_gain_speed.get(), // Proportional gain + _param_rd_i_gain_speed.get(), // Integral gain + 0.f, // Derivative gain + 1.f, // Integral limit + 1.f); // Output limit +} + +RoverDifferentialGuidance::differential_setpoint RoverDifferentialGuidance::computeGuidance(const float yaw, + const float actual_speed, const int nav_state) +{ + // Initializations + bool mission_finished{false}; + float desired_speed{0.f}; + float desired_yaw_rate{0.f}; + hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + // uORB subscriber updates + 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); + } + + const float desired_heading = _pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, + math::max(actual_speed, 0.f)); + + const float heading_error = matrix::wrap_pi(desired_heading - yaw); + + const float distance_to_next_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 + && distance_to_next_wp < _param_nav_acc_rad.get()) { // Return to launch + mission_finished = true; + } + + // State machine + if (!mission_finished && distance_to_next_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()) { + pid_reset_integral(&_pid_heading); + _currentState = GuidanceState::SPOT_TURNING; + + } else if (_currentState == GuidanceState::SPOT_TURNING && fabsf(heading_error) < _param_rd_trans_trn_drv.get()) { + pid_reset_integral(&_pid_heading); + _currentState = GuidanceState::DRIVING; + } + + } else { // Mission finished or delay command + _currentState = GuidanceState::STOPPED; + } + + // Guidance logic + switch (_currentState) { + case GuidanceState::DRIVING: { + desired_speed = _param_rd_miss_spd_def.get(); + + if (_param_rd_max_jerk.get() > FLT_EPSILON && _param_rd_max_accel.get() > FLT_EPSILON) { + desired_speed = math::trajectory::computeMaxSpeedFromDistance(_param_rd_max_jerk.get(), + _param_rd_max_accel.get(), distance_to_next_wp, 0.0f); + desired_speed = math::constrain(desired_speed, -_param_rd_max_speed.get(), _param_rd_max_speed.get()); + } + + desired_yaw_rate = pid_calculate(&_pid_heading, heading_error, 0.f, 0.f, dt); + } break; + + case GuidanceState::SPOT_TURNING: + if (actual_speed < TURN_MAX_VELOCITY) { // Wait for the rover to stop + desired_yaw_rate = pid_calculate(&_pid_heading, heading_error, 0.f, 0.f, dt); // Turn on the spot + } + + break; + + case GuidanceState::STOPPED: + default: + desired_speed = 0.f; + desired_yaw_rate = 0.f; + break; + + } + + // Closed loop speed control + float throttle{0.f}; + + if (fabsf(desired_speed) < FLT_EPSILON) { + pid_reset_integral(&_pid_throttle); + + } else { + throttle = pid_calculate(&_pid_throttle, desired_speed, actual_speed, 0, + dt); + + if (_param_rd_max_speed.get() > FLT_EPSILON) { // Feed-forward term + throttle += math::interpolate(desired_speed, + 0.f, _param_rd_max_speed.get(), + 0.f, 1.f); + } + } + + // Publish differential controller status (logging) + _rover_differential_guidance_status.timestamp = _timestamp; + _rover_differential_guidance_status.desired_speed = desired_speed; + _rover_differential_guidance_status.pid_throttle_integral = _pid_throttle.integral; + _rover_differential_guidance_status.lookahead_distance = _pure_pursuit.getLookaheadDistance(); + _rover_differential_guidance_status.pid_heading_integral = _pid_heading.integral; + _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); + + // Return setpoints + differential_setpoint differential_setpoint_temp; + differential_setpoint_temp.throttle = math::constrain(throttle, 0.f, 1.f); + differential_setpoint_temp.yaw_rate = math::constrain(desired_yaw_rate, -_max_yaw_rate, + _max_yaw_rate); + differential_setpoint_temp.closed_loop_yaw_rate = true; + return differential_setpoint_temp; +} + +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)); + +} diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp new file mode 100644 index 000000000000..29a131f8d8e2 --- /dev/null +++ b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp @@ -0,0 +1,163 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +// PX4 includes +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Standard libraries +#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 rover guidance. + */ +class RoverDifferentialGuidance : public ModuleParams +{ +public: + /** + * @brief Constructor for RoverDifferentialGuidance. + * @param parent The parent ModuleParams object. + */ + RoverDifferentialGuidance(ModuleParams *parent); + ~RoverDifferentialGuidance() = default; + + struct differential_setpoint { + float throttle{0.f}; + float yaw_rate{0.f}; + bool closed_loop_yaw_rate{false}; + }; + + /** + * @brief Compute guidance for the vehicle. + * @param yaw The yaw orientation of the vehicle in radians. + * @param actual_speed The velocity of the vehicle in m/s. + * @param dt The time step in seconds. + * @param nav_state Navigation state of the rover. + */ + RoverDifferentialGuidance::differential_setpoint computeGuidance(float yaw, float actual_speed, + int nav_state); + + /** + * @brief Update global/ned waypoint coordinates + */ + void updateWaypoints(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + // 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_guidance_status_pub{ORB_ID(rover_differential_guidance_status)}; + rover_differential_guidance_status_s _rover_differential_guidance_status{}; + + // Variables + MapProjection _global_ned_proj_ref{}; // Transform global to ned coordinates. + GuidanceState _currentState{GuidanceState::DRIVING}; // The current state of guidance. + PurePursuit _pure_pursuit{this}; // Pure pursuit library + hrt_abstime _timestamp{0}; + float _max_yaw_rate{0.f}; + + + // 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{}; + Vector2d _home_position{}; + + // Controllers + PID_t _pid_heading; // The PID controller for the heading + PID_t _pid_throttle; // The PID controller for velocity + + // Constants + static constexpr float TURN_MAX_VELOCITY = 0.2f; // Velocity threshhold for starting the spot turn [m/s] + + // Parameters + DEFINE_PARAMETERS( + (ParamFloat) _param_rd_p_gain_heading, + (ParamFloat) _param_rd_i_gain_heading, + (ParamFloat) _param_rd_p_gain_speed, + (ParamFloat) _param_rd_i_gain_speed, + (ParamFloat) _param_rd_max_speed, + (ParamFloat) _param_nav_acc_rad, + (ParamFloat) _param_rd_max_jerk, + (ParamFloat) _param_rd_max_accel, + (ParamFloat) _param_rd_miss_spd_def, + (ParamFloat) _param_rd_max_yaw_rate, + (ParamFloat) _param_rd_trans_trn_drv, + (ParamFloat) _param_rd_trans_drv_trn + + ) +}; diff --git a/src/modules/differential_drive/module.yaml b/src/modules/rover_differential/module.yaml similarity index 55% rename from src/modules/differential_drive/module.yaml rename to src/modules/rover_differential/module.yaml index 50ab9bb837ca..b6819305bc5f 100644 --- a/src/modules/differential_drive/module.yaml +++ b/src/modules/rover_differential/module.yaml @@ -1,11 +1,12 @@ -module_name: Differential Drive +module_name: Rover Differential parameters: - - group: Rover Differential Drive + - group: Rover Differential definitions: - RDD_WHEEL_BASE: + + RD_WHEEL_TRACK: description: - short: Wheel base + short: Wheel track long: Distance from the center of the right wheel to the center of the left wheel type: float unit: m @@ -14,55 +15,41 @@ parameters: increment: 0.001 decimal: 3 default: 0.5 - RDD_WHEEL_RADIUS: - description: - short: Wheel radius - long: Size of the wheel, half the diameter of the wheel - type: float - unit: m - min: 0.001 - max: 100 - increment: 0.001 - decimal: 3 - default: 0.1 - RDD_SPEED_SCALE: - description: - short: Manual speed scale - type: float - min: 0 - max: 1 - increment: 0.01 - decimal: 2 - default: 1 - RDD_ANG_SCALE: + + RD_MAN_YAW_SCALE: description: - short: Manual angular velocity scale + short: Manual yaw rate scale + long: | + In manual mode the setpoint for the yaw rate received from the rc remote + is scaled by this value. type: float - min: 0 + min: 0.01 max: 1 increment: 0.01 decimal: 2 default: 1 - RDD_WHEEL_SPEED: + + RD_HEADING_P: description: - short: Maximum wheel speed + short: Proportional gain for heading controller type: float - unit: rad/s min: 0 max: 100 increment: 0.01 decimal: 2 - default: 0.3 - RDD_P_HEADING: + default: 1 + + RD_HEADING_I: description: - short: Proportional gain for heading controller + short: Integral gain for heading controller type: float min: 0 max: 100 increment: 0.01 decimal: 2 - default: 1 - RDD_P_SPEED: + default: 0.1 + + RD_SPEED_P: description: short: Proportional gain for speed controller type: float @@ -71,7 +58,8 @@ parameters: increment: 0.01 decimal: 2 default: 1 - RDD_I_SPEED: + + RD_SPEED_I: description: short: Integral gain for ground speed controller type: float @@ -80,7 +68,8 @@ parameters: increment: 0.01 decimal: 2 default: 0 - RDD_P_ANG_VEL: + + RD_YAW_RATE_P: description: short: Proportional gain for angular velocity controller type: float @@ -89,7 +78,8 @@ parameters: increment: 0.01 decimal: 2 default: 1 - RDD_I_ANG_VEL: + + RD_YAW_RATE_I: description: short: Integral gain for angular velocity controller type: float @@ -98,7 +88,8 @@ parameters: increment: 0.01 decimal: 2 default: 0 - RDD_MAX_JERK: + + RD_MAX_JERK: description: short: Maximum jerk long: Limit for forwards acc/deceleration change. @@ -109,7 +100,8 @@ parameters: increment: 0.01 decimal: 2 default: 0.5 - RDD_MAX_ACCEL: + + RD_MAX_ACCEL: description: short: Maximum acceleration long: Maximum acceleration is used to limit the acceleration of the rover @@ -120,3 +112,61 @@ parameters: increment: 0.01 decimal: 2 default: 0.5 + + RD_MAX_SPEED: + description: + short: Maximum speed the rover can drive + long: This parameter is used to map desired speeds to normalized motor commands. + type: float + unit: m/s + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 7 + + 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 mode. + type: float + unit: deg/s + min: 0.01 + max: 1000 + increment: 0.01 + decimal: 2 + default: 90 + + RD_MISS_SPD_DEF: + description: + short: Default rover speed during a mission + type: float + unit: m/s + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 1 + + RD_TRANS_TRN_DRV: + description: + short: Heading error threshhold to switch from spot turning to driving + type: float + unit: rad + min: 0.001 + max: 180 + increment: 0.01 + decimal: 3 + default: 0.0872665 + + RD_TRANS_DRV_TRN: + description: + short: Heading error threshhold to switch from driving to spot turning + type: float + unit: rad + min: 0.001 + max: 180 + increment: 0.01 + decimal: 3 + default: 0.174533 diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml index 67b1e9456cee..e9e67cff25ed 100644 --- a/src/modules/uxrce_dds_client/dds_topics.yaml +++ b/src/modules/uxrce_dds_client/dds_topics.yaml @@ -130,9 +130,6 @@ subscriptions: - topic: /fmu/in/vehicle_rates_setpoint type: px4_msgs::msg::VehicleRatesSetpoint - - topic: /fmu/in/differential_drive_setpoint - type: px4_msgs::msg::DifferentialDriveSetpoint - - topic: /fmu/in/vehicle_visual_odometry type: px4_msgs::msg::VehicleOdometry From 28a0de63c5787c10b366aeb0371c5bfe710d0ca7 Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Wed, 7 Aug 2024 11:12:52 +0200 Subject: [PATCH 11/90] Orbit Yaw Vehicle Parameter (#23358) --- msg/OrbitStatus.msg | 1 + msg/VehicleCommand.msg | 8 ++++++++ .../tasks/Orbit/FlightTaskOrbit.cpp | 11 ++++++++++- .../tasks/Orbit/FlightTaskOrbit.hpp | 2 ++ .../tasks/Orbit/flight_task_orbit_params.c | 14 +++++++++++++- 5 files changed, 34 insertions(+), 2 deletions(-) diff --git a/msg/OrbitStatus.msg b/msg/OrbitStatus.msg index a04265db46c8..531fa4145306 100644 --- a/msg/OrbitStatus.msg +++ b/msg/OrbitStatus.msg @@ -4,6 +4,7 @@ uint8 ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING = 1 uint8 ORBIT_YAW_BEHAVIOUR_UNCONTROLLED = 2 uint8 ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE = 3 uint8 ORBIT_YAW_BEHAVIOUR_RC_CONTROLLED = 4 +uint8 ORBIT_YAW_BEHAVIOUR_UNCHANGED = 5 uint64 timestamp # time since system start (microseconds) float32 radius # Radius of the orbit circle. Positive values orbit clockwise, negative values orbit counter-clockwise. [m] diff --git a/msg/VehicleCommand.msg b/msg/VehicleCommand.msg index 0c1ce85d5fa8..8df0ca56b0ac 100644 --- a/msg/VehicleCommand.msg +++ b/msg/VehicleCommand.msg @@ -158,6 +158,14 @@ uint8 SPEED_TYPE_GROUNDSPEED = 1 uint8 SPEED_TYPE_CLIMB_SPEED = 2 uint8 SPEED_TYPE_DESCEND_SPEED = 3 +# used as param3 in CMD_DO_ORBIT +uint8 ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER = 0 +uint8 ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING = 1 +uint8 ORBIT_YAW_BEHAVIOUR_UNCONTROLLED = 2 +uint8 ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE = 3 +uint8 ORBIT_YAW_BEHAVIOUR_RC_CONTROLLED = 4 +uint8 ORBIT_YAW_BEHAVIOUR_UNCHANGED = 5 + # used as param1 in ARM_DISARM command int8 ARMING_ACTION_DISARM = 0 int8 ARMING_ACTION_ARM = 1 diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp index f639e492493d..18adf4d3eb80 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp @@ -90,7 +90,14 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command, b // commanded heading behaviour if (PX4_ISFINITE(command.param3)) { - _yaw_behaviour = command.param3; + if (static_cast(command.param3 + .5f) == vehicle_command_s::ORBIT_YAW_BEHAVIOUR_UNCHANGED) { + if (!_currently_orbiting) { // only change the yaw behaviour if we are not actively orbiting + _yaw_behaviour = _param_mc_orbit_yaw_mod.get(); + } + + } else { + _yaw_behaviour = command.param3; + } } // save current yaw estimate for ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING @@ -165,6 +172,7 @@ void FlightTaskOrbit::_sanitizeParams(float &radius, float &velocity) const bool FlightTaskOrbit::activate(const trajectory_setpoint_s &last_setpoint) { bool ret = FlightTaskManualAltitude::activate(last_setpoint); + _currently_orbiting = false; _orbit_radius = _radius_min; _orbit_velocity = 1.f; _center = _position; @@ -199,6 +207,7 @@ bool FlightTaskOrbit::activate(const trajectory_setpoint_s &last_setpoint) bool FlightTaskOrbit::update() { bool ret = true; + _currently_orbiting = true; _updateTrajectoryBoundaries(); _adjustParametersByStick(); diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp index 187a47de4119..2c55bbc15ba7 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp @@ -124,6 +124,7 @@ class FlightTaskOrbit : public FlightTaskManualAltitudeSmoothVel /** yaw behaviour during the orbit flight according to MAVLink's ORBIT_YAW_BEHAVIOUR enum */ int _yaw_behaviour = orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER; bool _started_clockwise{true}; + bool _currently_orbiting{false}; float _initial_heading = 0.f; /**< the heading of the drone when the orbit command was issued */ SlewRateYaw _slew_rate_yaw; @@ -132,6 +133,7 @@ class FlightTaskOrbit : public FlightTaskManualAltitudeSmoothVel DEFINE_PARAMETERS( (ParamFloat) _param_mc_orbit_rad_max, + (ParamInt) _param_mc_orbit_yaw_mod, (ParamFloat) _param_mpc_xy_cruise, /**< cruise speed for circle approach */ (ParamFloat) _param_mpc_yawrauto_max, (ParamFloat) _param_mpc_xy_traj_p, diff --git a/src/modules/flight_mode_manager/tasks/Orbit/flight_task_orbit_params.c b/src/modules/flight_mode_manager/tasks/Orbit/flight_task_orbit_params.c index 3e655c80e2ed..c8f66eb9f15d 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/flight_task_orbit_params.c +++ b/src/modules/flight_mode_manager/tasks/Orbit/flight_task_orbit_params.c @@ -39,6 +39,18 @@ * @max 10000.0 * @increment 0.5 * @decimal 1 - * @group FlightTaskOrbit + * @group Flight Task Orbit */ PARAM_DEFINE_FLOAT(MC_ORBIT_RAD_MAX, 1000.0f); + +/** + * Yaw behaviour during orbit flight. + * + * @value 0 Front to Circle Center + * @value 1 Hold Initial Heading + * @value 2 Uncontrolled + * @value 3 Hold Front Tangent to Circle + * @value 4 RC Controlled + * @group Flight Task Orbit + */ +PARAM_DEFINE_INT32(MC_ORBIT_YAW_MOD, 0); From c8ff5909b56224d24fb65e07f90d7ac0ab7ad4f7 Mon Sep 17 00:00:00 2001 From: chfriedrich98 <125505139+chfriedrich98@users.noreply.github.com> Date: Wed, 7 Aug 2024 15:16:41 +0200 Subject: [PATCH 12/90] rover: restructure airframes (#23506) --- .../50000_generic_rover_differential | 12 ++++++ ..._r1_rover => 50001_aion_robotics_r1_rover} | 3 -- ..._generic => 51000_generic_rover_ackermann} | 2 +- .../51001_axial_scx10_2_trail_honcho | 37 +++++++++++++++++++ ...d_vehicle => 59000_generic_ground_vehicle} | 2 +- ...robot_gpx => 59001_nxpcup_car_dfrobot_gpx} | 2 +- .../init.d/airframes/CMakeLists.txt | 19 ++++++---- .../init.d/rc.rover_ackermann_apps | 4 +- .../init.d/rc.rover_ackermann_defaults | 12 +++--- .../init.d/rc.rover_differential_apps | 4 +- .../init.d/rc.rover_differential_defaults | 13 ++++--- 11 files changed, 80 insertions(+), 30 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/airframes/50000_generic_rover_differential rename ROMFS/px4fmu_common/init.d/airframes/{50003_aion_robotics_r1_rover => 50001_aion_robotics_r1_rover} (85%) rename ROMFS/px4fmu_common/init.d/airframes/{50010_ackermann_rover_generic => 51000_generic_rover_ackermann} (83%) create mode 100644 ROMFS/px4fmu_common/init.d/airframes/51001_axial_scx10_2_trail_honcho rename ROMFS/px4fmu_common/init.d/airframes/{50000_generic_ground_vehicle => 59000_generic_ground_vehicle} (96%) rename ROMFS/px4fmu_common/init.d/airframes/{50004_nxpcup_car_dfrobot_gpx => 59001_nxpcup_car_dfrobot_gpx} (97%) diff --git a/ROMFS/px4fmu_common/init.d/airframes/50000_generic_rover_differential b/ROMFS/px4fmu_common/init.d/airframes/50000_generic_rover_differential new file mode 100644 index 000000000000..8c490d497be2 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/50000_generic_rover_differential @@ -0,0 +1,12 @@ +#!/bin/sh +# +# @name Generic Rover Differential +# +# @type Rover +# @class Rover +# +# @board px4_fmu-v2 exclude +# @board bitcraze_crazyflie exclude +# + +. ${R}etc/init.d/rc.rover_differential_defaults diff --git a/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover b/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover similarity index 85% rename from ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover rename to ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover index 20a16428b2ed..c06b158cf857 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover +++ b/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover @@ -15,9 +15,6 @@ param set-default BAT1_N_CELLS 4 -param set-default EKF2_GBIAS_INIT 0.01 -param set-default EKF2_ANGERR_INIT 0.01 - # Set geometry & output configration param set-default RBCLW_ADDRESS 128 param set-default RBCLW_FUNC1 101 diff --git a/ROMFS/px4fmu_common/init.d/airframes/50010_ackermann_rover_generic b/ROMFS/px4fmu_common/init.d/airframes/51000_generic_rover_ackermann similarity index 83% rename from ROMFS/px4fmu_common/init.d/airframes/50010_ackermann_rover_generic rename to ROMFS/px4fmu_common/init.d/airframes/51000_generic_rover_ackermann index 66b3dc0cabba..43620a50ddcd 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50010_ackermann_rover_generic +++ b/ROMFS/px4fmu_common/init.d/airframes/51000_generic_rover_ackermann @@ -1,6 +1,6 @@ #!/bin/sh # -# @name Generic ackermann rover +# @name Generic Rover Ackermann # # @type Rover # @class Rover diff --git a/ROMFS/px4fmu_common/init.d/airframes/51001_axial_scx10_2_trail_honcho b/ROMFS/px4fmu_common/init.d/airframes/51001_axial_scx10_2_trail_honcho new file mode 100644 index 000000000000..6f50c7f4a1e0 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/51001_axial_scx10_2_trail_honcho @@ -0,0 +1,37 @@ +#!/bin/sh +# +# @name Axial SCX10 2 Trail Honcho +# +# @url https://www.axialadventure.com/product/1-10-scx10-ii-trail-honcho-4wd-rock-crawler-brushed-rtr/AXID9059.html +# +# @type Rover +# @class Rover +# +# @board px4_fmu-v2 exclude +# @board bitcraze_crazyflie exclude +# + +. ${R}etc/init.d/rc.rover_ackermann_defaults + +param set-default BAT1_N_CELLS 3 + +# Rover parameters +param set-default NAV_ACC_RAD 0.5 +param set-default RA_ACC_RAD_GAIN 2 +param set-default RA_ACC_RAD_MAX 3 +param set-default RA_MAX_ACCEL 0.5 +param set-default RA_MAX_JERK 10 +param set-default RA_MAX_SPEED 2.7 +param set-default RA_MAX_STR_ANG 0.5236 +param set-default RA_MAX_STR_RATE 270 +param set-default RA_MISS_VEL_DEF 2.7 +param set-default RA_MISS_VEL_GAIN 3.5 +param set-default RA_MISS_VEL_MIN 1 +param set-default RA_SPEED_I 0.1 +param set-default RA_SPEED_P 0.5 +param set-default RA_WHEEL_BASE 0.321 + +# Pure pursuit parameters +param set-default PP_LOOKAHD_GAIN 1 +param set-default PP_LOOKAHD_MAX 10 +param set-default PP_LOOKAHD_MIN 1.5 diff --git a/ROMFS/px4fmu_common/init.d/airframes/50000_generic_ground_vehicle b/ROMFS/px4fmu_common/init.d/airframes/59000_generic_ground_vehicle similarity index 96% rename from ROMFS/px4fmu_common/init.d/airframes/50000_generic_ground_vehicle rename to ROMFS/px4fmu_common/init.d/airframes/59000_generic_ground_vehicle index 5ae6b09f4a8d..a773c1158d44 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50000_generic_ground_vehicle +++ b/ROMFS/px4fmu_common/init.d/airframes/59000_generic_ground_vehicle @@ -1,6 +1,6 @@ #!/bin/sh # -# @name Generic Ground Vehicle (Ackermann) +# @name Generic Ground Vehicle (Deprecated) # # @type Rover # @class Rover diff --git a/ROMFS/px4fmu_common/init.d/airframes/50004_nxpcup_car_dfrobot_gpx b/ROMFS/px4fmu_common/init.d/airframes/59001_nxpcup_car_dfrobot_gpx similarity index 97% rename from ROMFS/px4fmu_common/init.d/airframes/50004_nxpcup_car_dfrobot_gpx rename to ROMFS/px4fmu_common/init.d/airframes/59001_nxpcup_car_dfrobot_gpx index 67620ddc0375..9f08553d970f 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50004_nxpcup_car_dfrobot_gpx +++ b/ROMFS/px4fmu_common/init.d/airframes/59001_nxpcup_car_dfrobot_gpx @@ -1,6 +1,6 @@ #!/bin/sh # -# @name NXP Cup car: DF Robot GPX +# @name NXP Cup car: DF Robot GPX (Deprecated) # # @type Rover diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt index c6bae3566a1d..634eb3e1b882 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt @@ -136,22 +136,27 @@ if(CONFIG_MODULES_VTOL_ATT_CONTROL) ) endif() -if(CONFIG_MODULES_ROVER_POS_CONTROL) +if(CONFIG_MODULES_ROVER_DIFFERENTIAL) px4_add_romfs_files( - 50000_generic_ground_vehicle - 50004_nxpcup_car_dfrobot_gpx + # [50000, 50999] Differential rovers + 50000_generic_rover_differential + 50001_aion_robotics_r1_rover ) endif() -if(CONFIG_MODULES_ROVER_DIFFERENTIAL) +if(CONFIG_MODULES_ROVER_ACKERMANN) px4_add_romfs_files( - 50003_aion_robotics_r1_rover + # [51000, 51999] Ackermann rovers + 51000_generic_rover_ackermann + 51001_axial_scx10_2_trail_honcho ) endif() -if(CONFIG_MODULES_ROVER_ACKERMANN) +if(CONFIG_MODULES_ROVER_POS_CONTROL) px4_add_romfs_files( - 50010_ackermann_rover_generic + # [59000, 59999] Rover position control (deprecated) + 59000_generic_ground_vehicle + 59001_nxpcup_car_dfrobot_gpx ) endif() diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_apps b/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_apps index 5273d6aec068..181233babe26 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_apps +++ b/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_apps @@ -1,7 +1,7 @@ #!/bin/sh -# Standard apps for a ackermann drive rover. +# Standard apps for an ackermann rover. -# Start rover ackermann drive controller. +# Start rover ackermann module. rover_ackermann start # Start Land Detector. diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_defaults b/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_defaults index 27cf144a329a..fe0ae7aa9bb8 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_defaults @@ -2,12 +2,10 @@ # Ackermann rover parameters. set VEHICLE_TYPE rover_ackermann -param set-default MAV_TYPE 10 # MAV_TYPE_GROUND_ROVER -param set-default CA_AIRFRAME 5 # Rover (Ackermann) -param set-default CA_R_REV 1 # Motor is assumed to be reversible -param set-default EKF2_MAG_TYPE 1 # make sure magnetometer is fused even when not flying +param set-default MAV_TYPE 10 # MAV_TYPE_GROUND_ROVER +param set-default CA_AIRFRAME 5 # Rover (Ackermann) +param set-default CA_R_REV 1 # Motor is assumed to be reversible +param set-default EKF2_MAG_TYPE 1 # Make sure magnetometer is fused even when not flying +param set-default NAV_ACC_RAD 0.5 # Waypoint acceptance radius param set-default EKF2_GBIAS_INIT 0.01 param set-default EKF2_ANGERR_INIT 0.01 -param set-default NAV_ACC_RAD 0.5 # Waypoint acceptance radius -param set-default NAV_RCL_ACT 6 # Disarm on manual control loss -param set-default COM_FAIL_ACT_T 1 # Delay before failsafe after rc loss diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps b/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps index 4acbe8301404..e1a7ecd9ccd2 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps +++ b/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps @@ -1,7 +1,7 @@ #!/bin/sh -# Standard apps for a differential drive rover. +# Standard apps for a differential rover. -# Start rover differential drive controller. +# Start rover differential module. rover_differential start # Start Land Detector. diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_differential_defaults b/ROMFS/px4fmu_common/init.d/rc.rover_differential_defaults index 4cddd44f89e3..7c29ba364f3e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_differential_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.rover_differential_defaults @@ -2,9 +2,10 @@ # Differential rover parameters. set VEHICLE_TYPE rover_differential - -param set-default MAV_TYPE 10 # MAV_TYPE_GROUND_ROVER -param set-default CA_AIRFRAME 6 # Rover (Differential) -param set-default CA_R_REV 3 # Right and left motors reversible -param set-default NAV_ACC_RAD 0.5 # Waypoint acceptance radius -param set-default EKF2_MAG_TYPE 1 # Make sure magnetometer is fused even when not flying +param set-default MAV_TYPE 10 # MAV_TYPE_GROUND_ROVER +param set-default CA_AIRFRAME 6 # Rover (Differential) +param set-default CA_R_REV 3 # Right and left motors reversible +param set-default EKF2_MAG_TYPE 1 # Make sure magnetometer is fused even when not flying +param set-default NAV_ACC_RAD 0.5 # Waypoint acceptance radius +param set-default EKF2_GBIAS_INIT 0.01 +param set-default EKF2_ANGERR_INIT 0.01 From 588eedb8cbc3d0c777dc433972ba87befe40c6ea Mon Sep 17 00:00:00 2001 From: Peter van der Perk <57130844+PetervdPerk-NXP@users.noreply.github.com> Date: Wed, 7 Aug 2024 15:24:07 +0200 Subject: [PATCH 13/90] px4: sitl fix filepath regression (#23457) --- boards/px4/sitl/default.px4board | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index 784be31db67a..ac228113d5d8 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -1,7 +1,7 @@ CONFIG_PLATFORM_POSIX=y CONFIG_BOARD_TESTING=y CONFIG_BOARD_ETHERNET=y -CONFIG_BOARD_ROOT_PATH="./" +CONFIG_BOARD_ROOT_PATH="." CONFIG_DRIVERS_CAMERA_TRIGGER=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_OSD_MSP_OSD=y From 876730a9beefe721c6b723144909d33b699f75d2 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 7 Aug 2024 15:38:55 +0200 Subject: [PATCH 14/90] FW Position Controller: enable flaps during hand/catapult launch (#23460) Signed-off-by: Silvan Fuhrer --- src/modules/fw_pos_control/FixedwingPositionControl.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index f636b0e013fd..9571c2986591 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -1532,8 +1532,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _att_sp.pitch_body = _runway_takeoff.getPitch(get_tecs_pitch()); _att_sp.thrust_body[0] = _runway_takeoff.getThrottle(_param_fw_thr_idle.get(), get_tecs_thrust()); - _flaps_setpoint = _param_fw_flaps_to_scl.get(); - // retract ladning gear once passed the climbout state if (_runway_takeoff.getState() > RunwayTakeoffState::CLIMBOUT) { _new_landing_gear_position = landing_gear_s::GEAR_UP; @@ -1634,6 +1632,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _launch_detection_status_pub.publish(launch_detection_status); } + _flaps_setpoint = _param_fw_flaps_to_scl.get(); _att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, _takeoff_ground_alt); if (!_vehicle_status.in_transition_to_fw) { From 176f09b48b49c55eaa91c08cf4b293cf82a60dd1 Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Wed, 7 Aug 2024 13:56:31 +0200 Subject: [PATCH 15/90] gz_bridge: add rover world to cmake --- src/modules/simulation/gz_bridge/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/simulation/gz_bridge/CMakeLists.txt b/src/modules/simulation/gz_bridge/CMakeLists.txt index 003a712282f9..c7d36e36a5ad 100644 --- a/src/modules/simulation/gz_bridge/CMakeLists.txt +++ b/src/modules/simulation/gz_bridge/CMakeLists.txt @@ -93,6 +93,7 @@ if(gz-transport_FOUND) windy baylands lawn + rover ) # find corresponding airframes From b488e45e73288f96d54c08cc2778ecc71ec06b63 Mon Sep 17 00:00:00 2001 From: PX4 BuildBot Date: Wed, 7 Aug 2024 12:39:04 +0000 Subject: [PATCH 16/90] Update submodule sitl_gazebo-classic to latest Wed Aug 7 12:39:04 UTC 2024 - sitl_gazebo-classic in PX4/Firmware (28a0de63c5787c10b366aeb0371c5bfe710d0ca7): https://github.com/PX4/PX4-SITL_gazebo-classic/commit/67431d233f0f08de647f0eb11239816f9c8bd6c6 - sitl_gazebo-classic current upstream: https://github.com/PX4/PX4-SITL_gazebo-classic/commit/67af3c3a6da493bdc0a0b9de28b01a2a98d38659 - Changes: https://github.com/PX4/PX4-SITL_gazebo-classic/compare/67431d233f0f08de647f0eb11239816f9c8bd6c6...67af3c3a6da493bdc0a0b9de28b01a2a98d38659 67af3c3 2024-07-18 Silvan Fuhrer - model/lidar: incrase range to 50m (#1049) --- Tools/simulation/gazebo-classic/sitl_gazebo-classic | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/simulation/gazebo-classic/sitl_gazebo-classic b/Tools/simulation/gazebo-classic/sitl_gazebo-classic index 67431d233f0f..67af3c3a6da4 160000 --- a/Tools/simulation/gazebo-classic/sitl_gazebo-classic +++ b/Tools/simulation/gazebo-classic/sitl_gazebo-classic @@ -1 +1 @@ -Subproject commit 67431d233f0f08de647f0eb11239816f9c8bd6c6 +Subproject commit 67af3c3a6da493bdc0a0b9de28b01a2a98d38659 From a39a3e2099c493dbbbfdd08d43a92af6a40b964f Mon Sep 17 00:00:00 2001 From: PX4 BuildBot Date: Wed, 7 Aug 2024 12:39:06 +0000 Subject: [PATCH 17/90] Update submodule gz to latest Wed Aug 7 12:39:06 UTC 2024 - gz in PX4/Firmware (411a328e325e5109a453cf84d0c65393be86bfef): https://github.com/PX4/PX4-gazebo-models/commit/312cd002ff9602644efef58eef93e447c10dcbe8 - gz current upstream: https://github.com/PX4/PX4-gazebo-models/commit/536305adee09b9ace391b16107e625cf7c6db7e7 - Changes: https://github.com/PX4/PX4-gazebo-models/compare/312cd002ff9602644efef58eef93e447c10dcbe8...536305adee09b9ace391b16107e625cf7c6db7e7 536305a 2024-08-07 Claudio Chies - add world for collision prevention (#52) 36f49cb 2024-07-29 Stefano Colli - Add x500 with gimbal model (#47) 4ddfc13 2024-07-24 Jacob Dahl - Downward mono cam + aruco tag (#48) --- Tools/simulation/gz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/simulation/gz b/Tools/simulation/gz index 312cd002ff96..536305adee09 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 312cd002ff9602644efef58eef93e447c10dcbe8 +Subproject commit 536305adee09b9ace391b16107e625cf7c6db7e7 From 086c044f471bf6d79c4072481838f50f2ea08dfd Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Wed, 7 Aug 2024 07:26:12 -0800 Subject: [PATCH 18/90] mavlink: log handler rewrite for improved efficiency (#23421) --- src/modules/mavlink/mavlink_log_handler.cpp | 768 +++++++++----------- src/modules/mavlink/mavlink_log_handler.h | 118 +-- 2 files changed, 418 insertions(+), 468 deletions(-) diff --git a/src/modules/mavlink/mavlink_log_handler.cpp b/src/modules/mavlink/mavlink_log_handler.cpp index a75daf90bad7..1afa32382b19 100644 --- a/src/modules/mavlink/mavlink_log_handler.cpp +++ b/src/modules/mavlink/mavlink_log_handler.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-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 @@ -31,558 +31,498 @@ * ****************************************************************************/ -/// @file mavlink_log_handler.h -/// @author px4dev, Gus Grubba - #include "mavlink_log_handler.h" #include "mavlink_main.h" +#include #include -#include -#include - -#define MOUNTPOINT PX4_STORAGEDIR -static const char *kLogRoot = MOUNTPOINT "/log"; -static const char *kLogData = MOUNTPOINT "/logdata.txt"; -static const char *kTmpData = MOUNTPOINT "/$log$.txt"; +static constexpr int MAX_BYTES_BURST = 256 * 1024; +static const char *kLogListFilePath = PX4_STORAGEDIR "/logdata.txt"; +static const char *kLogListFilePathTemp = PX4_STORAGEDIR "/$log$.txt"; +static const char *kLogDir = PX4_STORAGEDIR "/log"; #ifdef __PX4_NUTTX #define PX4LOG_REGULAR_FILE DTYPE_FILE #define PX4LOG_DIRECTORY DTYPE_DIRECTORY +#define PX4_MAX_FILEPATH CONFIG_PATH_MAX #else +#ifndef PATH_MAX +#define PATH_MAX 1024 // maximum on macOS +#endif #define PX4LOG_REGULAR_FILE DT_REG #define PX4LOG_DIRECTORY DT_DIR +#define PX4_MAX_FILEPATH PATH_MAX #endif -//#define MAVLINK_LOG_HANDLER_VERBOSE - -#ifdef MAVLINK_LOG_HANDLER_VERBOSE -#define PX4LOG_WARN(fmt, ...) warnx(fmt, ##__VA_ARGS__) -#else -#define PX4LOG_WARN(fmt, ...) -#endif +MavlinkLogHandler::MavlinkLogHandler(Mavlink &mavlink) + : _mavlink(mavlink) +{} -//------------------------------------------------------------------- -static bool -stat_file(const char *file, time_t *date = nullptr, uint32_t *size = nullptr) +MavlinkLogHandler::~MavlinkLogHandler() { - struct stat st; - - if (stat(file, &st) == 0) { - if (date) { *date = st.st_mtime; } - - if (size) { *size = st.st_size; } + perf_free(_create_file_elapsed); + perf_free(_listing_elapsed); - return true; + if (_current_entry.fp) { + fclose(_current_entry.fp); } - return false; + unlink(kLogListFilePath); + unlink(kLogListFilePathTemp); } -//------------------------------------------------------------------- -MavlinkLogHandler::MavlinkLogHandler(Mavlink &mavlink) - : _mavlink(mavlink) +void MavlinkLogHandler::send() { + switch (_state) { + case LogHandlerState::Idle: { + state_idle(); + break; + } -} -MavlinkLogHandler::~MavlinkLogHandler() -{ - _close_and_unlink_files(); + case LogHandlerState::Listing: { + state_listing(); + break; + } + + case LogHandlerState::SendingData: { + state_sending_data(); + break; + } + } } -//------------------------------------------------------------------- -void -MavlinkLogHandler::handle_message(const mavlink_message_t *msg) +void MavlinkLogHandler::handle_message(const mavlink_message_t *msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_LOG_REQUEST_LIST: - _log_request_list(msg); + handle_log_request_list(msg); break; case MAVLINK_MSG_ID_LOG_REQUEST_DATA: - _log_request_data(msg); + handle_log_request_data(msg); break; - case MAVLINK_MSG_ID_LOG_ERASE: - _log_request_erase(msg); + case MAVLINK_MSG_ID_LOG_REQUEST_END: + handle_log_request_end(msg); break; - case MAVLINK_MSG_ID_LOG_REQUEST_END: - _log_request_end(msg); + case MAVLINK_MSG_ID_LOG_ERASE: + handle_log_erase(msg); break; } } -//------------------------------------------------------------------- -void -MavlinkLogHandler::send() +void MavlinkLogHandler::state_idle() { - //-- An arbitrary count of max bytes in one go (one of the two below but never both) -#define MAX_BYTES_SEND 256 * 1024 - size_t count = 0; - - //-- Log Entries - while (_current_status == LogHandlerState::Listing - && _mavlink.get_free_tx_buf() > MAVLINK_MSG_ID_LOG_ENTRY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES - && count < MAX_BYTES_SEND) { - count += _log_send_listing(); - } - - //-- Log Data - while (_current_status == LogHandlerState::SendingData - && _mavlink.get_free_tx_buf() > MAVLINK_MSG_ID_LOG_DATA_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES - && count < MAX_BYTES_SEND) { - count += _log_send_data(); + if (_current_entry.fp && _file_send_finished) { + fclose(_current_entry.fp); + _current_entry.fp = nullptr; + + _current_entry.id = 0xffff; + _current_entry.time_utc = 0; + _current_entry.size_bytes = 0; + _current_entry.filepath[0] = 0; + _current_entry.offset = 0; + + _entry_request.id = 0xffff; + _entry_request.start_offset = 0; + _entry_request.byte_count = 0; } } -//------------------------------------------------------------------- -void -MavlinkLogHandler::_log_request_list(const mavlink_message_t *msg) +void MavlinkLogHandler::state_listing() { - mavlink_log_request_list_t request; - mavlink_msg_log_request_list_decode(msg, &request); + static constexpr uint32_t MAVLINK_PACKET_SIZE = MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_LOG_ENTRY_LEN; - //-- Check for re-requests (data loss) or new request - if (_current_status != LogHandlerState::Inactive) { - //-- Is this a new request? - if (request.start == 0) { - _current_status = LogHandlerState::Inactive; - _close_and_unlink_files(); - - } else { - _current_status = LogHandlerState::Idle; - - } + if (_mavlink.get_free_tx_buf() <= MAVLINK_PACKET_SIZE) { + return; } - if (_current_status == LogHandlerState::Inactive) { - //-- Prepare new request - - _reset_list_helper(); - _init_list_helper(); - _current_status = LogHandlerState::Idle; - } + DIR *dp = opendir(kLogDir); - if (_log_count) { - //-- Define (and clamp) range - _next_entry = request.start < _log_count ? request.start : - _log_count - 1; - _last_entry = request.end < _log_count ? request.end : - _log_count - 1; + if (!dp) { + PX4_DEBUG("No logs available"); + return; } - PX4LOG_WARN("\nMavlinkLogHandler::_log_request_list: start: %d last: %d count: %d", - _next_entry, - _last_entry, - _log_count); - //-- Enable streaming - _current_status = LogHandlerState::Listing; -} + FILE *fp = fopen(kLogListFilePath, "r"); -//------------------------------------------------------------------- -void -MavlinkLogHandler::_log_request_data(const mavlink_message_t *msg) -{ - //-- If we haven't listed, we can't do much - if (_current_status == LogHandlerState::Inactive) { - PX4LOG_WARN("MavlinkLogHandler::_log_request_data Log request with no list requested."); + if (!fp) { + PX4_DEBUG("Failed to open log list file"); + closedir(dp); return; } - mavlink_log_request_data_t request; - mavlink_msg_log_request_data_decode(msg, &request); + fseek(fp, _list_request.file_index, SEEK_SET); - //-- Does the requested log exist? - if (request.id >= _log_count) { - PX4LOG_WARN("MavlinkLogHandler::_log_request_data Requested log %" PRIu16 " but we only have %u.", request.id, - _log_count); - return; - } + size_t bytes_sent = 0; - //-- If we were sending log entries, stop it - _current_status = LogHandlerState::Idle; + char line[PX4_MAX_FILEPATH]; - if (_current_log_index != request.id) { - //-- Init send log dataset - _current_log_filename[0] = 0; - _current_log_index = request.id; - uint32_t time_utc = 0; + perf_begin(_listing_elapsed); - if (!_get_entry(_current_log_index, _current_log_size, time_utc, - _current_log_filename, sizeof(_current_log_filename))) { - PX4LOG_WARN("LogListHelper::get_entry failed."); - return; + while (fgets(line, sizeof(line), fp)) { + + if (_list_request.current_id < _list_request.first_id) { + _list_request.current_id++; + continue; } - _open_for_transmit(); - } + // We can send! + uint32_t size_bytes = 0; + uint32_t time_utc = 0; + char filepath[PX4_MAX_FILEPATH]; - _current_log_data_offset = request.ofs; + // If parsed lined successfully, send the entry + if (sscanf(line, "%" PRIu32 " %" PRIu32 " %s", &time_utc, &size_bytes, filepath) != 3) { + PX4_DEBUG("sscanf failed"); + continue; + } - if (_current_log_data_offset >= _current_log_size) { - _current_log_data_remaining = 0; + send_log_entry(time_utc, size_bytes); + bytes_sent += sizeof(mavlink_log_entry_t); + _list_request.current_id++; - } else { - _current_log_data_remaining = _current_log_size - request.ofs; + // Yield if we've exceed mavlink burst or buffer limit + if (_mavlink.get_free_tx_buf() <= MAVLINK_PACKET_SIZE || bytes_sent >= MAX_BYTES_BURST) { + _list_request.file_index = ftell(fp); + fclose(fp); + closedir(dp); + perf_end(_listing_elapsed); + return; + } } - if (_current_log_data_remaining > request.count) { - _current_log_data_remaining = request.count; - } + perf_end(_listing_elapsed); - //-- Enable streaming - _current_status = LogHandlerState::SendingData; -} + fclose(fp); + closedir(dp); -//------------------------------------------------------------------- -void -MavlinkLogHandler::_log_request_erase(const mavlink_message_t * /*msg*/) -{ - /* - mavlink_log_erase_t request; - mavlink_msg_log_erase_decode(msg, &request); - */ - _current_status = LogHandlerState::Inactive; - _close_and_unlink_files(); - - //-- Delete all logs - _delete_all(kLogRoot); + _list_request.current_id = 0; + _list_request.file_index = 0; + _state = LogHandlerState::Idle; } -//------------------------------------------------------------------- -void -MavlinkLogHandler::_log_request_end(const mavlink_message_t * /*msg*/) +void MavlinkLogHandler::state_sending_data() { - PX4LOG_WARN("MavlinkLogHandler::_log_request_end"); + static constexpr uint32_t MAVLINK_PACKET_SIZE = MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_LOG_DATA_LEN; + size_t bytes_sent = 0; - _current_status = LogHandlerState::Inactive; - _close_and_unlink_files(); -} + while (_mavlink.get_free_tx_buf() > MAVLINK_PACKET_SIZE && bytes_sent < MAX_BYTES_BURST) { -//------------------------------------------------------------------- -size_t -MavlinkLogHandler::_log_send_listing() -{ - mavlink_log_entry_t response; - uint32_t size, date; - _get_entry(_next_entry, size, date); - response.size = size; - response.time_utc = date; - response.id = _next_entry; - response.num_logs = _log_count; - response.last_log_num = _last_entry; - mavlink_msg_log_entry_send_struct(_mavlink.get_channel(), &response); - - //-- If we're done listing, flag it. - if (_next_entry >= _last_entry) { - _current_status = LogHandlerState::Idle; - - } else { - _next_entry++; - } + // Only seek if we need to + long int offset = _current_entry.offset - ftell(_current_entry.fp); - PX4LOG_WARN("MavlinkLogHandler::_log_send_listing id: %" PRIu16 " count: %" PRIu16 " last: %" PRIu16 " size: %" PRIu32 - " date: %" PRIu32 " status: %" PRIu32, - response.id, - response.num_logs, - response.last_log_num, - response.size, - response.time_utc, - (uint32_t)_current_status); - return sizeof(response); -} + if (offset && fseek(_current_entry.fp, offset, SEEK_CUR)) { + fclose(_current_entry.fp); + _current_entry.fp = nullptr; + PX4_DEBUG("seek error"); + _state = LogHandlerState::Idle; + } -//------------------------------------------------------------------- -size_t -MavlinkLogHandler::_log_send_data() -{ - mavlink_log_data_t response; - memset(&response, 0, sizeof(response)); - uint32_t len = _current_log_data_remaining; + // Prepare mavlink message + mavlink_log_data_t msg; - if (len > sizeof(response.data)) { - len = sizeof(response.data); - } + if (_current_entry.offset >= _current_entry.size_bytes) { + PX4_DEBUG("Entry offset equal to file size"); + _state = LogHandlerState::Idle; + return; + } - size_t read_size = _get_log_data(len, response.data); - response.ofs = _current_log_data_offset; - response.id = _current_log_index; - response.count = read_size; - mavlink_msg_log_data_send_struct(_mavlink.get_channel(), &response); - _current_log_data_offset += read_size; - _current_log_data_remaining -= read_size; + size_t bytes_to_read = _current_entry.size_bytes - _current_entry.offset; - if (read_size < sizeof(response.data) || _current_log_data_remaining == 0) { - _current_status = LogHandlerState::Idle; - } + if (bytes_to_read > sizeof(msg.data)) { + bytes_to_read = sizeof(msg.data); + } - return sizeof(response); -} + msg.count = fread(msg.data, 1, bytes_to_read, _current_entry.fp); + msg.id = _current_entry.id; + msg.ofs = _current_entry.offset; -//------------------------------------------------------------------- -void MavlinkLogHandler::_close_and_unlink_files() -{ - if (_current_log_filep) { - ::fclose(_current_log_filep); - _reset_list_helper(); - } + mavlink_msg_log_data_send_struct(_mavlink.get_channel(), &msg); + + bytes_sent += MAVLINK_PACKET_SIZE; + _current_entry.offset += msg.count; + + bool chunk_finished = _current_entry.offset >= (_entry_request.byte_count + _entry_request.start_offset); + _file_send_finished = _current_entry.offset >= _current_entry.size_bytes; - // Remove log data files (if any) - unlink(kLogData); - unlink(kTmpData); + if (chunk_finished || _file_send_finished) { + _state = LogHandlerState::Idle; + return; + } + } } -//------------------------------------------------------------------- -bool -MavlinkLogHandler::_get_entry(int idx, uint32_t &size, uint32_t &date, char *filename, int filename_len) +void MavlinkLogHandler::handle_log_request_list(const mavlink_message_t *msg) { - //-- Find log file in log list file created during init() - size = 0; - date = 0; - bool result = false; - //-- Open list of log files - FILE *f = ::fopen(kLogData, "r"); - - if (f) { - //--- Find requested entry - char line[160]; - int count = 0; - - while (fgets(line, sizeof(line), f)) { - //-- Found our "index" - if (count++ == idx) { - char file[160]; - - if (sscanf(line, "%" PRIu32 " %" PRIu32 " %s", &date, &size, file) == 3) { - if (filename && filename_len > 0) { - strncpy(filename, file, filename_len); - filename[filename_len - 1] = 0; // ensure null-termination - } - - result = true; - break; - } - } - } + mavlink_log_request_list_t request; + mavlink_msg_log_request_list_decode(msg, &request); - fclose(f); + if (!create_log_list_file()) { + return; } - return result; + _list_request.first_id = request.start; + _list_request.last_id = request.end == 0xffff ? _num_logs : request.end; + _list_request.current_id = 0; + _list_request.file_index = 0; + _logs_listed = true; + _state = LogHandlerState::Listing; } -//------------------------------------------------------------------- -bool -MavlinkLogHandler::_open_for_transmit() +void MavlinkLogHandler::handle_log_request_data(const mavlink_message_t *msg) { - if (_current_log_filep) { - ::fclose(_current_log_filep); - _current_log_filep = nullptr; + if (!_logs_listed) { + PX4_DEBUG("Logs not yet listed"); + _state = LogHandlerState::Idle; + return; } - _current_log_filep = ::fopen(_current_log_filename, "rb"); + mavlink_log_request_data_t request; + mavlink_msg_log_request_data_decode(msg, &request); - if (!_current_log_filep) { - PX4LOG_WARN("MavlinkLogHandler::open_for_transmit Could not open %s", _current_log_filename); - return false; + if (request.id >= _num_logs) { + PX4_DEBUG("Requested log %" PRIu16 " but we only have %u", request.id, _num_logs); + _state = LogHandlerState::Idle; + return; } - return true; -} + // Handle switching to new request ID + if (request.id != _current_entry.id) { + // Close the old file + if (_current_entry.fp) { + fclose(_current_entry.fp); + _current_entry.fp = nullptr; + } -//------------------------------------------------------------------- -size_t -MavlinkLogHandler::_get_log_data(uint8_t len, uint8_t *buffer) -{ - if (!_current_log_filename[0]) { - return 0; - } + LogEntry entry = {}; - if (!_current_log_filep) { - PX4LOG_WARN("MavlinkLogHandler::get_log_data file not open %s", _current_log_filename); - return 0; - } + if (!log_entry_from_id(request.id, &entry)) { + PX4_DEBUG("Log file ID %u does not exist", request.id); + _state = LogHandlerState::Idle; + return; + } - long int offset = _current_log_data_offset - ftell(_current_log_filep); + entry.fp = fopen(entry.filepath, "rb"); + entry.offset = request.ofs; - if (offset && fseek(_current_log_filep, offset, SEEK_CUR)) { - fclose(_current_log_filep); - _current_log_filep = nullptr; - PX4LOG_WARN("MavlinkLogHandler::get_log_data Seek error in %s", _current_log_filename); - return 0; + if (!entry.fp) { + PX4_DEBUG("Failed to open file %s", entry.filepath); + return; + } + + _current_entry = entry; } - size_t result = fread(buffer, 1, len, _current_log_filep); - return result; -} + // Stop if offset request is larger than file + if (request.ofs >= _current_entry.size_bytes) { + PX4_DEBUG("Request offset %" PRIu32 "greater than file size %" PRIu32, request.ofs, + _current_entry.size_bytes); + _state = LogHandlerState::Idle; + return; + } + _entry_request.id = request.id; + _entry_request.start_offset = request.ofs; + _entry_request.byte_count = request.count; + // Set the offset of the current entry to the requested offset + _current_entry.offset = _entry_request.start_offset; + _file_send_finished = false; + _state = LogHandlerState::SendingData; +} -void -MavlinkLogHandler::_reset_list_helper() +void MavlinkLogHandler::handle_log_request_end(const mavlink_message_t *msg) { - _next_entry = 0; - _last_entry = 0; - _log_count = 0; - _current_log_index = UINT16_MAX; - _current_log_size = 0; - _current_log_data_offset = 0; - _current_log_data_remaining = 0; - _current_log_filep = nullptr; + _state = LogHandlerState::Idle; } -void -MavlinkLogHandler::_init_list_helper() +void MavlinkLogHandler::handle_log_erase(const mavlink_message_t *msg) { - /* + if (_current_entry.fp) { + fclose(_current_entry.fp); + _current_entry.fp = nullptr; + } + + _state = LogHandlerState::Idle; + unlink(kLogListFilePath); + unlink(kLogListFilePathTemp); - When this helper is created, it scans the log directory - and collects all log files found into one file for easy, - subsequent access. - */ + delete_all_logs(kLogDir); +} - _current_log_filename[0] = 0; +bool MavlinkLogHandler::create_log_list_file() +{ + perf_begin(_create_file_elapsed); - // Remove old log data file (if any) - unlink(kLogData); - // Open log directory - DIR *dp = opendir(kLogRoot); + // clean up old file + unlink(kLogListFilePath); + _num_logs = 0; - if (dp == nullptr) { - // No log directory. Nothing to do. - return; + DIR *dp = opendir(kLogDir); + + if (!dp) { + PX4_DEBUG("No logs available"); + return false; } - // Create work file - FILE *f = ::fopen(kTmpData, "w"); + FILE *temp_fp = fopen(kLogListFilePathTemp, "w"); - if (!f) { - PX4LOG_WARN("MavlinkLogHandler::init Error creating %s", kTmpData); + if (!temp_fp) { + PX4_DEBUG("Failed to create temp file"); closedir(dp); - return; + return false; } - // Scan directory and collect log files struct dirent *result = nullptr; - while ((result = readdir(dp))) { - if (result->d_type == PX4LOG_DIRECTORY) { - time_t tt = 0; - char log_path[128]; - int ret = snprintf(log_path, sizeof(log_path), "%s/%s", kLogRoot, result->d_name); - bool path_is_ok = (ret > 0) && (ret < (int)sizeof(log_path)); + // Iterate over the log/ directory which contains subdirectories formatted: yyyy-mm-dd + while (1) { + result = readdir(dp); - if (path_is_ok) { - if (_get_session_date(log_path, result->d_name, tt)) { - _scan_logs(f, log_path, tt); - } - } + if (!result) { + // Reached end of directory + break; + } + + if (result->d_type != PX4LOG_DIRECTORY) { + // Skip stray files + continue; + } + + // Skip the '.' and '..' entries + if (strcmp(result->d_name, ".") == 0 || strcmp(result->d_name, "..") == 0) { + continue; + } + + // Open up the sub directory + char dirpath[PX4_MAX_FILEPATH]; + int ret = snprintf(dirpath, sizeof(dirpath), "%s/%s", kLogDir, result->d_name); + + bool path_is_ok = (ret > 0) && (ret < (int)sizeof(dirpath)); + + if (!path_is_ok) { + PX4_DEBUG("Log subdir path error: %s", dirpath); + continue; } + + // Iterate over files inside the subdir and write them to the file + write_entries_to_file(temp_fp, dirpath); } + fclose(temp_fp); closedir(dp); - fclose(f); // Rename temp file to data file - if (rename(kTmpData, kLogData)) { - PX4LOG_WARN("MavlinkLogHandler::init Error renaming %s", kTmpData); - _log_count = 0; + if (rename(kLogListFilePathTemp, kLogListFilePath)) { + PX4_DEBUG("Failed to rename temp file"); + return false; } + + perf_end(_create_file_elapsed); + + return true; } -//------------------------------------------------------------------- -bool -MavlinkLogHandler::_get_session_date(const char *path, const char *dir, time_t &date) +void MavlinkLogHandler::write_entries_to_file(FILE *fp, const char *dir) { - if (strlen(dir) > 4) { - // Always try to get file time first - if (stat_file(path, &date)) { - // Try to prevent taking date if it's around 1970 (use the logic below instead) - if (date > 60 * 60 * 24) { - return true; - } + DIR *dp = opendir(dir); + struct dirent *result = nullptr; + + while (1) { + result = readdir(dp); + + if (!result) { + // Reached end of directory + break; } - // Convert "sess000" to 00:00 Jan 1 1970 (day per session) - if (strncmp(dir, "sess", 4) == 0) { - unsigned u; + if (result->d_type != PX4LOG_REGULAR_FILE) { + // Skip non files + continue; + } - if (sscanf(&dir[4], "%u", &u) == 1) { - date = u * 60 * 60 * 24; - return true; - } + char filepath[PX4_MAX_FILEPATH]; + int ret = snprintf(filepath, sizeof(filepath), "%s/%s", dir, result->d_name); + bool path_is_ok = (ret > 0) && (ret < (int)sizeof(filepath)); + + if (!path_is_ok) { + PX4_DEBUG("Log subdir path error: %s", filepath); + continue; + } + + struct stat filestat; + + if (stat(filepath, &filestat) != 0) { + PX4_DEBUG("stat() failed: %s", filepath); + continue; } + + // Write to file using format: + // [ time ] [ size_bytes ] [ filepath ] + fprintf(fp, "%u %u %s\n", unsigned(filestat.st_mtime), unsigned(filestat.st_size), filepath); + _num_logs++; } - return false; + closedir(dp); } -//------------------------------------------------------------------- -void -MavlinkLogHandler::_scan_logs(FILE *f, const char *dir, time_t &date) +void MavlinkLogHandler::send_log_entry(uint32_t time_utc, uint32_t size_bytes) { - DIR *dp = opendir(dir); + mavlink_log_entry_t msg; + msg.time_utc = time_utc; + msg.size = size_bytes; + msg.id = _list_request.current_id; + msg.num_logs = _num_logs; + msg.last_log_num = _list_request.last_id; + mavlink_msg_log_entry_send_struct(_mavlink.get_channel(), &msg); +} - if (dp) { - struct dirent *result = nullptr; - - while ((result = readdir(dp))) { - if (result->d_type == PX4LOG_REGULAR_FILE) { - time_t ldate = date; - uint32_t size = 0; - char log_file_path[128]; - int ret = snprintf(log_file_path, sizeof(log_file_path), "%s/%s", dir, result->d_name); - bool path_is_ok = (ret > 0) && (ret < (int)sizeof(log_file_path)); - - if (path_is_ok) { - if (_get_log_time_size(log_file_path, result->d_name, ldate, size)) { - //-- Write result->out to list file - fprintf(f, "%u %u %s\n", (unsigned)ldate, (unsigned)size, log_file_path); - _log_count++; - } - } - } - } +bool MavlinkLogHandler::log_entry_from_id(uint16_t log_id, LogEntry *entry) +{ + DIR *dp = opendir(kLogDir); + + if (!dp) { + PX4_INFO("No logs available"); + return false; + } + + FILE *fp = fopen(kLogListFilePath, "r"); + if (!fp) { + PX4_DEBUG("Failed to open %s", kLogListFilePath); closedir(dp); + return false; } -} -//------------------------------------------------------------------- -bool -MavlinkLogHandler::_get_log_time_size(const char *path, const char *file, time_t &date, uint32_t &size) -{ - if (file && file[0]) { - if (strstr(file, ".px4log") || strstr(file, ".ulg")) { - // Always try to get file time first - if (stat_file(path, &date, &size)) { - // Try to prevent taking date if it's around 1970 (use the logic below instead) - if (date > 60 * 60 * 24) { - return true; - } - } + bool found_entry = false; + uint16_t current_id = 0; + char line[PX4_MAX_FILEPATH]; - // Convert "log000" to 00:00 (minute per flight in session) - if (strncmp(file, "log", 3) == 0) { - unsigned u; + while (fgets(line, sizeof(line), fp)) { - if (sscanf(&file[3], "%u", &u) == 1) { - date += (u * 60); + if (current_id != log_id) { + current_id++; + continue; + } - if (stat_file(path, nullptr, &size)) { - return true; - } - } - } + if (sscanf(line, "%" PRIu32 " %" PRIu32 " %s", &(entry->time_utc), &(entry->size_bytes), entry->filepath) != 3) { + PX4_DEBUG("sscanf failed"); + continue; } + + entry->id = log_id; + found_entry = true; + break; } - return false; + fclose(fp); + closedir(dp); + + return found_entry; } -//------------------------------------------------------------------- -void -MavlinkLogHandler::_delete_all(const char *dir) +void MavlinkLogHandler::delete_all_logs(const char *dir) { //-- Open log directory DIR *dp = opendir(dir); @@ -600,27 +540,27 @@ MavlinkLogHandler::_delete_all(const char *dir) } if (result->d_type == PX4LOG_DIRECTORY && result->d_name[0] != '.') { - char log_path[128]; - int ret = snprintf(log_path, sizeof(log_path), "%s/%s", dir, result->d_name); - bool path_is_ok = (ret > 0) && (ret < (int)sizeof(log_path)); + char filepath[PX4_MAX_FILEPATH]; + int ret = snprintf(filepath, sizeof(filepath), "%s/%s", dir, result->d_name); + bool path_is_ok = (ret > 0) && (ret < (int)sizeof(filepath)); if (path_is_ok) { - _delete_all(log_path); //Recursive call. TODO: consider add protection + delete_all_logs(filepath); - if (rmdir(log_path)) { - PX4LOG_WARN("MavlinkLogHandler::delete_all Error removing %s", log_path); + if (rmdir(filepath)) { + PX4_DEBUG("Error removing %s", filepath); } } } if (result->d_type == PX4LOG_REGULAR_FILE) { - char log_path[128]; - int ret = snprintf(log_path, sizeof(log_path), "%s/%s", dir, result->d_name); - bool path_is_ok = (ret > 0) && (ret < (int)sizeof(log_path)); + char filepath[PX4_MAX_FILEPATH]; + int ret = snprintf(filepath, sizeof(filepath), "%s/%s", dir, result->d_name); + bool path_is_ok = (ret > 0) && (ret < (int)sizeof(filepath)); if (path_is_ok) { - if (unlink(log_path)) { - PX4LOG_WARN("MavlinkLogHandler::delete_all Error deleting %s", log_path); + if (unlink(filepath)) { + PX4_DEBUG("Error unlinking %s", filepath); } } } diff --git a/src/modules/mavlink/mavlink_log_handler.h b/src/modules/mavlink/mavlink_log_handler.h index 965c26712f1c..eb521a61ee4f 100644 --- a/src/modules/mavlink/mavlink_log_handler.h +++ b/src/modules/mavlink/mavlink_log_handler.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014, 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-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 @@ -33,76 +33,86 @@ #pragma once -/// @file mavlink_log_handler.h -/// @author px4dev, Gus Grubba - -#include -#include -#include -#include -#include -#include - +#include #include "mavlink_bridge_header.h" class Mavlink; -// MAVLink LOG_* Message Handler class MavlinkLogHandler { public: MavlinkLogHandler(Mavlink &mavlink); ~MavlinkLogHandler(); - // Handle possible LOG message + void send(); void handle_message(const mavlink_message_t *msg); - /** - * Handle sending of messages. Call this regularly at a fixed frequency. - * @param t current time - */ - void send(); +private: + struct LogEntry { + uint16_t id{0xffff}; + uint32_t time_utc{}; + uint32_t size_bytes{}; + FILE *fp{nullptr}; + char filepath[60]; + uint32_t offset{}; + }; - unsigned get_size(); + struct LogEntryRequest { + uint16_t id{0xffff}; + uint32_t start_offset{}; + uint32_t byte_count{}; + }; + + struct LogListRequest { + uint16_t first_id{0}; + uint16_t last_id{0}; + uint16_t current_id{0}; + int file_index{0}; + }; -private: enum class LogHandlerState { - Inactive, //There is no active action of log handler - Idle, //The log handler is not sending list/data, but list has been sent - Listing, //File list is being send - SendingData //File Data is being send + Idle, + Listing, + SendingData }; - void _log_message(const mavlink_message_t *msg); - void _log_request_list(const mavlink_message_t *msg); - void _log_request_data(const mavlink_message_t *msg); - void _log_request_erase(const mavlink_message_t *msg); - void _log_request_end(const mavlink_message_t *msg); - - void _reset_list_helper(); - void _init_list_helper(); - bool _get_session_date(const char *path, const char *dir, time_t &date); - void _scan_logs(FILE *f, const char *dir, time_t &date); - bool _get_log_time_size(const char *path, const char *file, time_t &date, uint32_t &size); - static void _delete_all(const char *dir); - bool _get_entry(int idx, uint32_t &size, uint32_t &date, char *filename = 0, int filename_len = 0); - bool _open_for_transmit(); - size_t _get_log_data(uint8_t len, uint8_t *buffer); - void _close_and_unlink_files(); - - size_t _log_send_listing(); - size_t _log_send_data(); - - LogHandlerState _current_status{LogHandlerState::Inactive}; + + // mavlink message handlers + void handle_log_request_list(const mavlink_message_t *msg); + void handle_log_request_data(const mavlink_message_t *msg); + void handle_log_request_end(const mavlink_message_t *msg); + void handle_log_erase(const mavlink_message_t *msg); + + // state functions + void state_idle(); + void state_listing(); + void state_sending_data(); + + // Log request list + bool create_log_list_file(); + void write_entries_to_file(FILE *f, const char *dir); + void send_log_entry(uint32_t size, uint32_t time_utc); + + // Log request data + bool log_entry_from_id(uint16_t log_id, LogEntry *entry); + + // Log erase + void delete_all_logs(const char *dir); + + +private: + LogHandlerState _state{LogHandlerState::Idle}; Mavlink &_mavlink; - int _next_entry{0}; - int _last_entry{0}; - int _log_count{0}; + // Log list + LogListRequest _list_request{}; + int _num_logs{0}; + bool _logs_listed{false}; + + // Log data + LogEntry _current_entry{}; + LogEntryRequest _entry_request{}; + bool _file_send_finished{}; - uint16_t _current_log_index{UINT16_MAX}; - uint32_t _current_log_size{0}; - uint32_t _current_log_data_offset{0}; - uint32_t _current_log_data_remaining{0}; - FILE *_current_log_filep{nullptr}; - char _current_log_filename[128]; //TODO: consider to allocate on runtime + perf_counter_t _create_file_elapsed{perf_alloc(PC_ELAPSED, MODULE_NAME": create file")}; + perf_counter_t _listing_elapsed{perf_alloc(PC_ELAPSED, MODULE_NAME": listing")}; }; From 2d99ae18ad2a6645305130fa659e9bb911de1306 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 8 Aug 2024 03:41:50 +1200 Subject: [PATCH 19/90] stm32h7: Reset USART clock selection (#23498) This resets the USARTs' clock source selection to the default, in case it has been changed by the bootloader. This is required if booting from the ArduPilot bootloader which happens to reset the clock selection to PLL. Without this fix, UARTs (including the console) is garbled, so presumably at an invalid baudrate. --- boards/cubepilot/cubeorange/nuttx-config/include/board.h | 6 ++++++ .../cubepilot/cubeorangeplus/nuttx-config/include/board.h | 6 ++++++ boards/px4/fmu-v6c/nuttx-config/include/board.h | 6 ++++++ boards/px4/fmu-v6x/nuttx-config/include/board.h | 6 ++++++ platforms/nuttx/NuttX/nuttx | 2 +- 5 files changed, 25 insertions(+), 1 deletion(-) diff --git a/boards/cubepilot/cubeorange/nuttx-config/include/board.h b/boards/cubepilot/cubeorange/nuttx-config/include/board.h index 5827b1bc81d1..62605f98d13a 100644 --- a/boards/cubepilot/cubeorange/nuttx-config/include/board.h +++ b/boards/cubepilot/cubeorange/nuttx-config/include/board.h @@ -194,6 +194,12 @@ #define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */ +/* UART clock selection */ +/* reset to default to overwrite any changes done by any bootloader */ + +#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC +#define STM32_RCC_D2CCIP2R_USART16_SEL RCC_D2CCIP2R_USART16SEL_RCC + /* FLASH wait states */ #define BOARD_FLASH_WAITSTATES 2 diff --git a/boards/cubepilot/cubeorangeplus/nuttx-config/include/board.h b/boards/cubepilot/cubeorangeplus/nuttx-config/include/board.h index d650c6169118..babaf587e450 100644 --- a/boards/cubepilot/cubeorangeplus/nuttx-config/include/board.h +++ b/boards/cubepilot/cubeorangeplus/nuttx-config/include/board.h @@ -195,6 +195,12 @@ #define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */ +/* UART clock selection */ +/* reset to default to overwrite any changes done by any bootloader */ + +#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC +#define STM32_RCC_D2CCIP2R_USART16_SEL RCC_D2CCIP2R_USART16SEL_RCC + /* FLASH wait states */ #define BOARD_FLASH_WAITSTATES 2 diff --git a/boards/px4/fmu-v6c/nuttx-config/include/board.h b/boards/px4/fmu-v6c/nuttx-config/include/board.h index 1951031cb825..4b4b128cf2ba 100644 --- a/boards/px4/fmu-v6c/nuttx-config/include/board.h +++ b/boards/px4/fmu-v6c/nuttx-config/include/board.h @@ -246,6 +246,12 @@ #define STM32_RCC_D2CCIP2R_USBSRC RCC_D2CCIP2R_USBSEL_PLL3 +/* UART clock selection */ +/* reset to default to overwrite any changes done by any bootloader */ + +#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC +#define STM32_RCC_D2CCIP2R_USART16_SEL RCC_D2CCIP2R_USART16SEL_RCC + /* ADC 1 2 3 clock source */ #define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 diff --git a/boards/px4/fmu-v6x/nuttx-config/include/board.h b/boards/px4/fmu-v6x/nuttx-config/include/board.h index d6b2925b795c..7907eafad194 100644 --- a/boards/px4/fmu-v6x/nuttx-config/include/board.h +++ b/boards/px4/fmu-v6x/nuttx-config/include/board.h @@ -250,6 +250,12 @@ #define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 +/* UART clock selection */ +/* reset to default to overwrite any changes done by any bootloader */ + +#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC +#define STM32_RCC_D2CCIP2R_USART16_SEL RCC_D2CCIP2R_USART16SEL_RCC + /* FDCAN 1 2 clock source */ #define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */ diff --git a/platforms/nuttx/NuttX/nuttx b/platforms/nuttx/NuttX/nuttx index d3b85f3e545d..5d74bc138955 160000 --- a/platforms/nuttx/NuttX/nuttx +++ b/platforms/nuttx/NuttX/nuttx @@ -1 +1 @@ -Subproject commit d3b85f3e545de3692c100143f4a9ae67762ce679 +Subproject commit 5d74bc138955e6f010a38e0f87f34e9a9019aecc From a8d54c7fae573852a1c7b81d6bf4ae5f0b7bfce8 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 7 Aug 2024 14:31:32 +0200 Subject: [PATCH 20/90] mixer_module: Reset RC passthrough functions to disarmed value when no stick input --- .../functions/FunctionManualRC.hpp | 38 ++++++++++++------- 1 file changed, 24 insertions(+), 14 deletions(-) diff --git a/src/lib/mixer_module/functions/FunctionManualRC.hpp b/src/lib/mixer_module/functions/FunctionManualRC.hpp index 38f7d096a94e..171c28a81750 100644 --- a/src/lib/mixer_module/functions/FunctionManualRC.hpp +++ b/src/lib/mixer_module/functions/FunctionManualRC.hpp @@ -45,9 +45,7 @@ class FunctionManualRC : public FunctionProviderBase public: FunctionManualRC() { - for (int i = 0; i < num_data_points; ++i) { - _data[i] = NAN; - } + resetAllToDisarmedValue(); } static FunctionProviderBase *allocate(const Context &context) { return new FunctionManualRC(); } @@ -57,17 +55,22 @@ class FunctionManualRC : public FunctionProviderBase manual_control_setpoint_s manual_control_setpoint; if (_topic.update(&manual_control_setpoint)) { - _data[0] = manual_control_setpoint.roll; - _data[1] = manual_control_setpoint.pitch; - _data[2] = manual_control_setpoint.throttle; - _data[3] = manual_control_setpoint.yaw; - _data[4] = manual_control_setpoint.flaps; - _data[5] = manual_control_setpoint.aux1; - _data[6] = manual_control_setpoint.aux2; - _data[7] = manual_control_setpoint.aux3; - _data[8] = manual_control_setpoint.aux4; - _data[9] = manual_control_setpoint.aux5; - _data[10] = manual_control_setpoint.aux6; + if (manual_control_setpoint.valid) { + _data[0] = manual_control_setpoint.roll; + _data[1] = manual_control_setpoint.pitch; + _data[2] = manual_control_setpoint.throttle; + _data[3] = manual_control_setpoint.yaw; + _data[4] = manual_control_setpoint.flaps; + _data[5] = manual_control_setpoint.aux1; + _data[6] = manual_control_setpoint.aux2; + _data[7] = manual_control_setpoint.aux3; + _data[8] = manual_control_setpoint.aux4; + _data[9] = manual_control_setpoint.aux5; + _data[10] = manual_control_setpoint.aux6; + + } else { + resetAllToDisarmedValue(); + } } } @@ -76,6 +79,13 @@ class FunctionManualRC : public FunctionProviderBase private: static constexpr int num_data_points = 11; + void resetAllToDisarmedValue() + { + for (int i = 0; i < num_data_points; ++i) { + _data[i] = NAN; + } + } + static_assert(num_data_points == (int)OutputFunction::RC_AUXMax - (int)OutputFunction::RC_Roll + 1, "number of functions mismatch"); From e2f5debf777023aa15f79caea8cfce7c2371c76a Mon Sep 17 00:00:00 2001 From: Jeremy Zanzig Date: Fri, 10 May 2024 13:26:59 -0700 Subject: [PATCH 21/90] change pairing LED feedback to 20 fast flashes of white --- src/lib/button/ButtonPublisher.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/lib/button/ButtonPublisher.cpp b/src/lib/button/ButtonPublisher.cpp index b3b35a7b3af1..826e5ed49f9a 100644 --- a/src/lib/button/ButtonPublisher.cpp +++ b/src/lib/button/ButtonPublisher.cpp @@ -70,9 +70,9 @@ void ButtonPublisher::pairingButtonTriggerEvent() led_control_s led_control{}; led_control.led_mask = 0xff; led_control.mode = led_control_s::MODE_BLINK_FAST; - led_control.color = led_control_s::COLOR_GREEN; - led_control.num_blinks = 1; - led_control.priority = 0; + led_control.color = led_control_s::COLOR_WHITE; + led_control.num_blinks = 20; + led_control.priority = 2; led_control.timestamp = hrt_absolute_time(); _led_control_pub.publish(led_control); From b01c179eed3564066a43ff0f734730ffc9f4dfca Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 2 Aug 2024 16:17:45 +0200 Subject: [PATCH 22/90] NavigatorMissionItem.msg: remove instance_count This information is duplicate to mission_result/mission_id. Signed-off-by: Silvan Fuhrer --- msg/NavigatorMissionItem.msg | 2 -- src/modules/navigator/mission_base.cpp | 1 - src/modules/navigator/navigator.h | 2 -- 3 files changed, 5 deletions(-) diff --git a/msg/NavigatorMissionItem.msg b/msg/NavigatorMissionItem.msg index 64af762f75d8..a10aa4656d51 100644 --- a/msg/NavigatorMissionItem.msg +++ b/msg/NavigatorMissionItem.msg @@ -1,7 +1,5 @@ uint64 timestamp # time since system start (microseconds) -uint32 instance_count # Instance count of this mission. Increments monotonically whenever the mission is modified - uint16 sequence_current # Sequence of the current mission item uint16 nav_cmd diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index 0a9a76bd2b34..d41456ccce66 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -870,7 +870,6 @@ void MissionBase::publish_navigator_mission_item() { navigator_mission_item_s navigator_mission_item{}; - navigator_mission_item.instance_count = _navigator->mission_instance_count(); navigator_mission_item.sequence_current = _mission.current_seq; navigator_mission_item.nav_cmd = _mission_item.nav_cmd; navigator_mission_item.latitude = _mission_item.lat; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 93f630dbf525..45c4f158b7c4 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -249,8 +249,6 @@ class Navigator : public ModuleBase, public ModuleParams orb_advert_t *get_mavlink_log_pub() { return &_mavlink_log_pub; } - int mission_instance_count() const { return _mission_result.mission_id; } - void set_mission_failure_heading_timeout(); bool get_mission_start_land_available() { return _mission.get_land_start_available(); } From 588c4a04c8154a9a94c04133f2eb11895c0029fb Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 6 Aug 2024 22:15:25 +0200 Subject: [PATCH 23/90] RTL direct: publish NavigatorMissionItem Signed-off-by: Silvan Fuhrer --- src/modules/navigator/rtl_direct.cpp | 31 ++++++++++++++++++++++++++++ src/modules/navigator/rtl_direct.h | 8 +++++++ 2 files changed, 39 insertions(+) diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index b028c7fb358f..52e853733eeb 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -337,6 +337,8 @@ void RtlDirect::set_rtl_item() _navigator->set_position_setpoint_triplet_updated(); } } + + publish_rtl_direct_navigator_mission_item(); // for logging } RtlDirect::RTLState RtlDirect::getActivationLandState() @@ -515,3 +517,32 @@ loiter_point_s RtlDirect::sanitizeLandApproach(loiter_point_s land_approach) con return sanitized_land_approach; } + +void RtlDirect::publish_rtl_direct_navigator_mission_item() +{ + navigator_mission_item_s navigator_mission_item{}; + + navigator_mission_item.sequence_current = static_cast(_rtl_state); + navigator_mission_item.nav_cmd = _mission_item.nav_cmd; + navigator_mission_item.latitude = _mission_item.lat; + navigator_mission_item.longitude = _mission_item.lon; + navigator_mission_item.altitude = _mission_item.altitude; + + navigator_mission_item.time_inside = get_time_inside(_mission_item); + navigator_mission_item.acceptance_radius = _mission_item.acceptance_radius; + navigator_mission_item.loiter_radius = _mission_item.loiter_radius; + navigator_mission_item.yaw = _mission_item.yaw; + + navigator_mission_item.frame = _mission_item.frame; + navigator_mission_item.frame = _mission_item.origin; + + navigator_mission_item.loiter_exit_xtrack = _mission_item.loiter_exit_xtrack; + navigator_mission_item.force_heading = _mission_item.force_heading; + navigator_mission_item.altitude_is_relative = _mission_item.altitude_is_relative; + navigator_mission_item.autocontinue = _mission_item.autocontinue; + navigator_mission_item.vtol_back_transition = _mission_item.vtol_back_transition; + + navigator_mission_item.timestamp = hrt_absolute_time(); + + _navigator_mission_item_pub.publish(navigator_mission_item); +} diff --git a/src/modules/navigator/rtl_direct.h b/src/modules/navigator/rtl_direct.h index 211b2779eae0..b1f6a75b81ae 100644 --- a/src/modules/navigator/rtl_direct.h +++ b/src/modules/navigator/rtl_direct.h @@ -47,6 +47,7 @@ #include #include #include +#include #include #include #include @@ -137,6 +138,12 @@ class RtlDirect : public MissionBlock, public ModuleParams */ void parameters_update(); + /** + * @brief Publish navigator mission item + * + */ + void publish_rtl_direct_navigator_mission_item(); + RTLState getActivationLandState(); void setLoiterPosition(); @@ -167,4 +174,5 @@ class RtlDirect : public MissionBlock, public ModuleParams uORB::SubscriptionData _land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ uORB::SubscriptionData _wind_sub{ORB_ID(wind)}; + uORB::Publication _navigator_mission_item_pub{ORB_ID::navigator_mission_item}; /**< Navigator mission item publication*/ }; From a737036633764b760d36828d16378dedbe0faed1 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 6 Aug 2024 23:38:38 +0200 Subject: [PATCH 24/90] RTLDirect: check for terrain collision in every state of RTL beside when landing Signed-off-by: Silvan Fuhrer --- src/modules/navigator/rtl_direct.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index 52e853733eeb..62a940f6c0cf 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -104,8 +104,9 @@ void RtlDirect::on_active() set_rtl_item(); } - if (_rtl_state == RTLState::LOITER_HOLD) { //TODO: rename _rtl_state to _rtl_state_next + if (_rtl_state != RTLState::IDLE) { //TODO: rename _rtl_state to _rtl_state_next (when in IDLE we're actually in LAND) //check for terrain collision and update altitude if needed + // note: it may trigger multiple times during a RTL, as every time the altitude set is reset updateAltToAvoidTerrainCollisionAndRepublishTriplet(_mission_item); } From 0381e148225ab8ef2380090319be9ebd4ca5608a Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 8 Aug 2024 13:51:00 +0200 Subject: [PATCH 25/90] FlightTaskOrbit: Avoid sending NAN altitude in status telemetry while it's changed by stick --- .../flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp index 18adf4d3eb80..144c6c7ba158 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp @@ -140,9 +140,11 @@ bool FlightTaskOrbit::sendTelemetry() orbit_status.yaw_behaviour = _yaw_behaviour; if (_geo_projection.isInitialized()) { + // While chainging altitude by stick _position_setpoint(2) is not set (NAN) + float local_altitude = PX4_ISFINITE(_position_setpoint(2)) ? _position_setpoint(2) : _position(2); // local -> global _geo_projection.reproject(_center(0), _center(1), orbit_status.x, orbit_status.y); - orbit_status.z = _global_local_alt0 - _position_setpoint(2); + orbit_status.z = _global_local_alt0 - local_altitude; } else { return false; // don't send the message if the transformation failed From a91aa40a3d7d3cc9ba6a5bbe40b35237b69aacb3 Mon Sep 17 00:00:00 2001 From: chfriedrich98 <125505139+chfriedrich98@users.noreply.github.com> Date: Thu, 8 Aug 2024 17:09:36 +0200 Subject: [PATCH 26/90] battery: only reset soc filter with valid voltage measurement (#23513) --- src/lib/battery/battery.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index 4d4715a0d529..5b41dde3b846 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -108,10 +108,6 @@ void Battery::updateCurrent(const float current_a) void Battery::updateBatteryStatus(const hrt_abstime ×tamp) { - if (!_battery_initialized && _internal_resistance_initialized && _params.n_cells > 0) { - resetInternalResistanceEstimation(_voltage_v, _current_a); - } - // Require minimum voltage otherwise override connected status if (_voltage_v < LITHIUM_BATTERY_RECOGNITION_VOLTAGE) { _connected = false; @@ -121,9 +117,13 @@ void Battery::updateBatteryStatus(const hrt_abstime ×tamp) _last_unconnected_timestamp = timestamp; } - // wait with initializing filters to avoid relying on a voltage sample from the rising edge + // Wait with initializing filters to avoid relying on a voltage sample from the rising edge _battery_initialized = _connected && (timestamp > _last_unconnected_timestamp + 2_s); + if (_connected && !_battery_initialized && _internal_resistance_initialized && _params.n_cells > 0) { + resetInternalResistanceEstimation(_voltage_v, _current_a); + } + sumDischarged(timestamp, _current_a); _state_of_charge_volt_based = calculateStateOfChargeVoltageBased(_voltage_v, _current_a); From cd231d0eed6a58f059dae995c360fbdd78ec1a98 Mon Sep 17 00:00:00 2001 From: Niklas Hauser Date: Thu, 8 Aug 2024 15:08:33 +0200 Subject: [PATCH 27/90] fmuv6x: Add GPIO expander to check overcurrent pins --- boards/px4/fmu-v6x/default.px4board | 1 + boards/px4/fmu-v6x/init/rc.board_defaults | 11 +++++++++++ boards/px4/fmu-v6x/nuttx-config/nsh/defconfig | 1 + boards/px4/fmu-v6x/src/CMakeLists.txt | 5 +++-- boards/px4/fmu-v6x/src/board_config.h | 5 +++++ boards/px4/fmu-v6x/src/{init.c => init.cpp} | 10 +++++++++- 6 files changed, 30 insertions(+), 3 deletions(-) rename boards/px4/fmu-v6x/src/{init.c => init.cpp} (98%) diff --git a/boards/px4/fmu-v6x/default.px4board b/boards/px4/fmu-v6x/default.px4board index 7b9488933fb4..7c4bf0b6f6cb 100644 --- a/boards/px4/fmu-v6x/default.px4board +++ b/boards/px4/fmu-v6x/default.px4board @@ -18,6 +18,7 @@ CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPIO_MCP23009=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_HEATER=y CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16470=y diff --git a/boards/px4/fmu-v6x/init/rc.board_defaults b/boards/px4/fmu-v6x/init/rc.board_defaults index cb1eb580a429..bdc99a68e7bd 100644 --- a/boards/px4/fmu-v6x/init/rc.board_defaults +++ b/boards/px4/fmu-v6x/init/rc.board_defaults @@ -27,3 +27,14 @@ else fi safety_button start + +# GPIO Expander driver on external I2C3 +if ver hwbasecmp 009 +then + # No USB + mcp23009 start -b 3 -X -D 0xf0 -O 0xf0 -P 0x0f -U 10 +fi +if ver hwbasecmp 00a 008 +then + mcp23009 start -b 3 -X -D 0xf1 -O 0xf0 -P 0x0f -U 10 +fi diff --git a/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig b/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig index ec6b1c7e2b0e..f0a8d6ae8653 100644 --- a/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig @@ -91,6 +91,7 @@ CONFIG_DEBUG_SYMBOLS=y CONFIG_DEBUG_TCBINFO=y CONFIG_DEFAULT_SMALL=y CONFIG_DEV_FIFO_SIZE=0 +CONFIG_DEV_GPIO=y CONFIG_DEV_PIPE_MAXSIZE=1024 CONFIG_DEV_PIPE_SIZE=70 CONFIG_DEV_URANDOM=y diff --git a/boards/px4/fmu-v6x/src/CMakeLists.txt b/boards/px4/fmu-v6x/src/CMakeLists.txt index a120ebe33623..39ec808e1e9a 100644 --- a/boards/px4/fmu-v6x/src/CMakeLists.txt +++ b/boards/px4/fmu-v6x/src/CMakeLists.txt @@ -34,7 +34,7 @@ if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") add_compile_definitions(BOOTLOADER) add_library(drivers_board bootloader_main.c - init.c + init.cpp usb.c timer_config.cpp ) @@ -52,7 +52,7 @@ else() add_library(drivers_board can.c i2c.cpp - init.c + init.cpp led.c mtd.cpp sdio.c @@ -71,5 +71,6 @@ else() nuttx_arch # sdio nuttx_drivers # sdio px4_layer + platform_gpio_mcp23009 ) endif() diff --git a/boards/px4/fmu-v6x/src/board_config.h b/boards/px4/fmu-v6x/src/board_config.h index 7cad5497ed2b..7c98f9490207 100644 --- a/boards/px4/fmu-v6x/src/board_config.h +++ b/boards/px4/fmu-v6x/src/board_config.h @@ -270,6 +270,11 @@ #define GPIO_VDD_3V3_SPEKTRUM_POWER_EN /* PH2 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN2) #define GPIO_VDD_3V3_SD_CARD_EN /* PC13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) +/* MCP23009 GPIO expander */ +#define BOARD_GPIO_VDD_5V_COMP_VALID "/dev/gpio4" +#define BOARD_GPIO_VDD_5V_CAN1_GPS1_VALID "/dev/gpio5" + + /* Spare GPIO */ #define GPIO_PG6 /* PG6 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN6) diff --git a/boards/px4/fmu-v6x/src/init.c b/boards/px4/fmu-v6x/src/init.cpp similarity index 98% rename from boards/px4/fmu-v6x/src/init.c rename to boards/px4/fmu-v6x/src/init.cpp index cd6714af1664..8901bd1766f3 100644 --- a/boards/px4/fmu-v6x/src/init.c +++ b/boards/px4/fmu-v6x/src/init.cpp @@ -74,6 +74,7 @@ #include #include #include +#include /**************************************************************************** * Pre-Processor Definitions @@ -159,7 +160,7 @@ __EXPORT void board_on_reset(int status) * ************************************************************************************/ -__EXPORT void +extern "C" __EXPORT void stm32_boardinitialize(void) { board_on_reset(-1); /* Reset PWM first thing */ @@ -280,6 +281,13 @@ __EXPORT int board_app_initialize(uintptr_t arg) # endif /* CONFIG_MMCSD */ + ret = mcp23009_register_gpios(3, 0x25); + + if (ret != OK) { + led_on(LED_RED); + return ret; + } + #endif /* !defined(BOOTLOADER) */ return OK; From 1af295f1a91b8e95fd31a3a3915dd8ebd01972ea Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Mon, 5 Aug 2024 15:46:41 +0200 Subject: [PATCH 28/90] ackermann: refactor main files --- .../rover_ackermann/RoverAckermann.cpp | 146 ++++++++++-------- .../rover_ackermann/RoverAckermann.hpp | 25 ++- 2 files changed, 105 insertions(+), 66 deletions(-) diff --git a/src/modules/rover_ackermann/RoverAckermann.cpp b/src/modules/rover_ackermann/RoverAckermann.cpp index b5468880587c..5409e40d841a 100644 --- a/src/modules/rover_ackermann/RoverAckermann.cpp +++ b/src/modules/rover_ackermann/RoverAckermann.cpp @@ -73,32 +73,15 @@ void RoverAckermann::Run() return; } - // uORB subscriber updates - if (_parameter_update_sub.updated()) { - updateParams(); - } - - if (_vehicle_status_sub.updated()) { - vehicle_status_s vehicle_status; - _vehicle_status_sub.copy(&vehicle_status); - _nav_state = vehicle_status.nav_state; - _armed = vehicle_status.arming_state == 2; - } - - if (_local_position_sub.updated()) { - vehicle_local_position_s local_position{}; - _local_position_sub.copy(&local_position); - const Vector3f rover_velocity = {local_position.vx, local_position.vy, local_position.vz}; - _actual_speed = rover_velocity.norm(); - } + updateSubscriptions(); // Timestamps hrt_abstime timestamp_prev = _timestamp; _timestamp = hrt_absolute_time(); const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + // Generate motor setpoints if (_armed) { - // Navigation modes switch (_nav_state) { case vehicle_status_s::NAVIGATION_STATE_MANUAL: { manual_control_setpoint_s manual_control_setpoint{}; @@ -118,62 +101,97 @@ void RoverAckermann::Run() default: // Unimplemented nav states will stop the rover _motor_setpoint.steering = 0.f; _motor_setpoint.throttle = 0.f; + _throttle_with_accel_limit.setForcedValue(0.f); + _steering_with_rate_limit.setForcedValue(0.f); break; } - // Sanitize actuator commands - if (!PX4_ISFINITE(_motor_setpoint.steering)) { - _motor_setpoint.steering = 0.f; - } + } else { // Reset on disarm + _motor_setpoint.steering = 0.f; + _motor_setpoint.throttle = 0.f; + _throttle_with_accel_limit.setForcedValue(0.f); + _steering_with_rate_limit.setForcedValue(0.f); + } - if (!PX4_ISFINITE(_motor_setpoint.throttle)) { - _motor_setpoint.throttle = 0.f; - } + publishMotorSetpoints(applySlewRates(_motor_setpoint, dt)); +} - // Acceleration slew rate - if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_speed.get() > FLT_EPSILON - && fabsf(_motor_setpoint.throttle) > fabsf(_throttle_with_accel_limit.getState())) { - _throttle_with_accel_limit.update(_motor_setpoint.throttle, dt); +void RoverAckermann::updateSubscriptions() +{ + if (_parameter_update_sub.updated()) { + updateParams(); + } - } else { - _throttle_with_accel_limit.setForcedValue(_motor_setpoint.throttle); - } + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + _vehicle_status_sub.copy(&vehicle_status); + _nav_state = vehicle_status.nav_state; + _armed = vehicle_status.arming_state == 2; + } - // Steering slew rate - if (_param_ra_max_steering_rate.get() > FLT_EPSILON && _param_ra_max_steer_angle.get() > FLT_EPSILON) { - _steering_with_rate_limit.update(_motor_setpoint.steering, dt); + if (_local_position_sub.updated()) { + vehicle_local_position_s local_position{}; + _local_position_sub.copy(&local_position); + const Vector3f rover_velocity = {local_position.vx, local_position.vy, local_position.vz}; + _actual_speed = rover_velocity.norm(); + } +} +motor_setpoint_struct RoverAckermann::applySlewRates(motor_setpoint_struct motor_setpoint, const float dt) +{ + // Sanitize actuator commands + if (!PX4_ISFINITE(motor_setpoint.steering)) { + motor_setpoint.steering = 0.f; + } - } else { - _steering_with_rate_limit.setForcedValue(_motor_setpoint.steering); - } + if (!PX4_ISFINITE(motor_setpoint.throttle)) { + motor_setpoint.throttle = 0.f; + } - // Publish rover ackermann status (logging) - rover_ackermann_status_s rover_ackermann_status{}; - rover_ackermann_status.timestamp = _timestamp; - rover_ackermann_status.throttle_setpoint = _motor_setpoint.throttle; - rover_ackermann_status.steering_setpoint = _motor_setpoint.steering; - rover_ackermann_status.actual_speed = _actual_speed; - _rover_ackermann_status_pub.publish(rover_ackermann_status); - - // Publish to motor - actuator_motors_s actuator_motors{}; - actuator_motors.reversible_flags = _param_r_rev.get(); - actuator_motors.control[0] = math::constrain(_throttle_with_accel_limit.getState(), -1.f, 1.f); - actuator_motors.timestamp = _timestamp; - _actuator_motors_pub.publish(actuator_motors); - - // Publish to servo - actuator_servos_s actuator_servos{}; - actuator_servos.control[0] = math::constrain(_steering_with_rate_limit.getState(), -1.f, 1.f); - actuator_servos.timestamp = _timestamp; - _actuator_servos_pub.publish(actuator_servos); + // Acceleration slew rate + if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_speed.get() > FLT_EPSILON + && fabsf(motor_setpoint.throttle) > fabsf(_throttle_with_accel_limit.getState())) { + _throttle_with_accel_limit.update(motor_setpoint.throttle, dt); - } else { // Reset on disarm - _motor_setpoint.steering = 0.f; - _motor_setpoint.throttle = 0.f; - _throttle_with_accel_limit.setForcedValue(0.f); - _steering_with_rate_limit.setForcedValue(0.f); + } else { + _throttle_with_accel_limit.setForcedValue(motor_setpoint.throttle); + } + + // Steering slew rate + if (_param_ra_max_steering_rate.get() > FLT_EPSILON && _param_ra_max_steer_angle.get() > FLT_EPSILON) { + _steering_with_rate_limit.update(motor_setpoint.steering, dt); + + } else { + _steering_with_rate_limit.setForcedValue(motor_setpoint.steering); } + + motor_setpoint_struct motor_setpoint_temp{}; + motor_setpoint_temp.steering = math::constrain(_steering_with_rate_limit.getState(), -1.f, 1.f); + motor_setpoint_temp.throttle = math::constrain(_throttle_with_accel_limit.getState(), -1.f, 1.f); + return motor_setpoint_temp; +} + +void RoverAckermann::publishMotorSetpoints(motor_setpoint_struct motor_setpoint_with_slew_rates) +{ + // Publish rover Ackermann status (logging) + rover_ackermann_status_s rover_ackermann_status{}; + rover_ackermann_status.timestamp = _timestamp; + rover_ackermann_status.throttle_setpoint = _motor_setpoint.throttle; + rover_ackermann_status.steering_setpoint = _motor_setpoint.steering; + rover_ackermann_status.actual_speed = _actual_speed; + _rover_ackermann_status_pub.publish(rover_ackermann_status); + + // Publish to motor + actuator_motors_s actuator_motors{}; + actuator_motors.reversible_flags = _param_r_rev.get(); + actuator_motors.control[0] = motor_setpoint_with_slew_rates.throttle; + actuator_motors.timestamp = _timestamp; + _actuator_motors_pub.publish(actuator_motors); + + // Publish to servo + actuator_servos_s actuator_servos{}; + actuator_servos.control[0] = motor_setpoint_with_slew_rates.steering; + actuator_servos.timestamp = _timestamp; + _actuator_servos_pub.publish(actuator_servos); } int RoverAckermann::task_spawn(int argc, char *argv[]) diff --git a/src/modules/rover_ackermann/RoverAckermann.hpp b/src/modules/rover_ackermann/RoverAckermann.hpp index cea9300a1b3d..3617b5b80ba6 100644 --- a/src/modules/rover_ackermann/RoverAckermann.hpp +++ b/src/modules/rover_ackermann/RoverAckermann.hpp @@ -59,6 +59,7 @@ // Local includes #include "RoverAckermannGuidance/RoverAckermannGuidance.hpp" +using motor_setpoint_struct = RoverAckermannGuidance::motor_setpoint; using namespace time_literals; @@ -83,12 +84,32 @@ class RoverAckermann : public ModuleBase, public ModuleParams, bool init(); + protected: void updateParams() override; private: void Run() override; + /** + * @brief Update uORB subscriptions. + */ + void updateSubscriptions(); + + /** + * @brief Apply slew rates to motor setpoints. + * @param motor_setpoint Normalized steering and throttle setpoints. + * @param dt Time since last update [s]. + * @return Motor setpoint with applied slew rates. + */ + motor_setpoint_struct applySlewRates(motor_setpoint_struct motor_setpoint, float dt); + + /** + * @brief Publish motor setpoints to ActuatorMotors/ActuatorServos and logging values to RoverAckermannStatus. + * @param motor_setpoint_with_slew_rate Normalized motor_setpoint with applied slew rates. + */ + void publishMotorSetpoints(motor_setpoint_struct motor_setpoint_with_slew_rates); + // uORB subscriptions uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; @@ -100,12 +121,12 @@ class RoverAckermann : public ModuleBase, public ModuleParams, uORB::Publication _actuator_servos_pub{ORB_ID(actuator_servos)}; uORB::Publication _rover_ackermann_status_pub{ORB_ID(rover_ackermann_status)}; - // Instances + // Class instances RoverAckermannGuidance _ackermann_guidance{this}; // Variables int _nav_state{0}; - RoverAckermannGuidance::motor_setpoint _motor_setpoint; + motor_setpoint_struct _motor_setpoint; hrt_abstime _timestamp{0}; float _actual_speed{0.f}; SlewRate _steering_with_rate_limit{0.f}; From 1a7717b5d99d1f32dedd746dcb9bb961b0d8b047 Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Mon, 5 Aug 2024 15:46:55 +0200 Subject: [PATCH 29/90] ackermann: refactor guidance files --- .../RoverAckermannGuidance.cpp | 276 +++++++++--------- .../RoverAckermannGuidance.hpp | 120 +++++--- 2 files changed, 230 insertions(+), 166 deletions(-) diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp index ade89f6c7e18..1acaebb6386f 100644 --- a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp +++ b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp @@ -58,13 +58,54 @@ void RoverAckermannGuidance::updateParams() RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::computeGuidance(const int nav_state) { - // Initializations - float desired_speed{0.f}; - float vehicle_yaw{0.f}; - float actual_speed{0.f}; - bool mission_finished{false}; + updateSubscriptions(); - // uORB subscriber updates + // Catch return to launch + if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { + _mission_finished = _distance_to_next_wp < _acceptance_radius; + } + + // Guidance logic + if (_mission_finished) { // Mission is finished + _desired_steering = 0.f; + _desired_speed = 0.f; + + } else if (_distance_to_curr_wp < _acceptance_radius) { // Catch delay command + _desired_speed = 0.f; + + } else { // Regular guidance algorithm + _desired_speed = calcDesiredSpeed(_param_ra_miss_vel_def.get(), _param_ra_miss_vel_min.get(), + _param_ra_miss_vel_gain.get(), _distance_to_prev_wp, _distance_to_curr_wp, _acceptance_radius, + _prev_acceptance_radius, _param_ra_max_accel.get(), _param_ra_max_jerk.get(), nav_state); + + _desired_steering = calcDesiredSteering(_pure_pursuit, _curr_wp_ned, _prev_wp_ned, _curr_pos_ned, + _param_ra_wheel_base.get(), _desired_speed, _vehicle_yaw, _param_ra_max_steer_angle.get()); + + } + + // Calculate throttle setpoint + hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + const float throttle = calcThrottleSetpoint(_pid_throttle, _desired_speed, _actual_speed, _param_ra_max_speed.get(), + dt); + + // Publish ackermann controller status (logging) + _rover_ackermann_guidance_status.timestamp = _timestamp; + _rover_ackermann_guidance_status.desired_speed = _desired_speed; + _rover_ackermann_guidance_status.pid_throttle_integral = _pid_throttle.integral; + _rover_ackermann_guidance_status_pub.publish(_rover_ackermann_guidance_status); + + // Return motor setpoints + motor_setpoint motor_setpoint_temp; + motor_setpoint_temp.steering = math::interpolate(_desired_steering, -_param_ra_max_steer_angle.get(), + _param_ra_max_steer_angle.get(), -1.f, 1.f); // Normalize steering setpoint + motor_setpoint_temp.throttle = throttle; + return motor_setpoint_temp; +} + +void RoverAckermannGuidance::updateSubscriptions() +{ if (_vehicle_global_position_sub.updated()) { vehicle_global_position_s vehicle_global_position{}; _vehicle_global_position_sub.copy(&vehicle_global_position); @@ -82,7 +123,7 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::computeGuidance(c _curr_pos_ned = Vector2f(local_position.x, local_position.y); const Vector3f rover_velocity = {local_position.vx, local_position.vy, local_position.vz}; - actual_speed = rover_velocity.norm(); + _actual_speed = rover_velocity.norm(); } if (_home_position_sub.updated()) { @@ -92,158 +133,63 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::computeGuidance(c } if (_position_setpoint_triplet_sub.updated()) { - updateWaypoints(); + updateWaypointsAndAcceptanceRadius(); } if (_vehicle_attitude_sub.updated()) { vehicle_attitude_s vehicle_attitude{}; _vehicle_attitude_sub.copy(&vehicle_attitude); matrix::Quatf vehicle_attitude_quaternion = Quatf(vehicle_attitude.q); - vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi(); + _vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi(); } if (_mission_result_sub.updated()) { mission_result_s mission_result{}; _mission_result_sub.copy(&mission_result); - mission_finished = mission_result.finished; - } - - if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL - && get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), _next_wp(0), - _next_wp(1)) < _acceptance_radius) { // Return to launch - mission_finished = true; - } - - // Guidance logic - if (mission_finished) { - _desired_steering = 0.f; - - } else { - const float distance_to_prev_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), - _prev_wp(0), - _prev_wp(1)); - const float distance_to_curr_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), - _curr_wp(0), - _curr_wp(1)); - - if (distance_to_curr_wp < _acceptance_radius) { // Catch delay command - desired_speed = 0.f; - - } else { - // Calculate desired speed - if (_param_ra_miss_vel_min.get() > 0.f && _param_ra_miss_vel_min.get() < _param_ra_miss_vel_def.get() - && _param_ra_miss_vel_gain.get() > FLT_EPSILON) { // Cornering slow down effect - if (distance_to_prev_wp <= _prev_acceptance_radius && _prev_acceptance_radius > FLT_EPSILON) { - const float cornering_speed = _param_ra_miss_vel_gain.get() / _prev_acceptance_radius; - desired_speed = math::constrain(cornering_speed, _param_ra_miss_vel_min.get(), _param_ra_miss_vel_def.get()); - - } else if (distance_to_curr_wp <= _acceptance_radius && _acceptance_radius > FLT_EPSILON) { - const float cornering_speed = _param_ra_miss_vel_gain.get() / _acceptance_radius; - desired_speed = math::constrain(cornering_speed, _param_ra_miss_vel_min.get(), _param_ra_miss_vel_def.get()); - - } else { // Straight line speed - if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_jerk.get() > FLT_EPSILON - && _acceptance_radius > FLT_EPSILON) { - float max_velocity{0.f}; - - if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { - max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_ra_max_jerk.get(), - _param_ra_max_accel.get(), distance_to_curr_wp, 0.f); - - } else { - const float cornering_speed = _param_ra_miss_vel_gain.get() / _acceptance_radius; - max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_ra_max_jerk.get(), - _param_ra_max_accel.get(), distance_to_curr_wp - _acceptance_radius, cornering_speed); - } - - desired_speed = math::constrain(max_velocity, _param_ra_miss_vel_min.get(), _param_ra_miss_vel_def.get()); - - } else { - desired_speed = _param_ra_miss_vel_def.get(); - } - } - - } else { - desired_speed = _param_ra_miss_vel_def.get(); - } - - // Calculate desired steering - _desired_steering = calcDesiredSteering(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, _param_ra_wheel_base.get(), - desired_speed, vehicle_yaw); - _desired_steering = math::constrain(_desired_steering, -_param_ra_max_steer_angle.get(), - _param_ra_max_steer_angle.get()); - } - } - - // Throttle PID - hrt_abstime timestamp_prev = _timestamp; - _timestamp = hrt_absolute_time(); - const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; - float throttle = 0.f; - - if (desired_speed < FLT_EPSILON) { - pid_reset_integral(&_pid_throttle); - - } else { - throttle = pid_calculate(&_pid_throttle, desired_speed, actual_speed, 0, - dt); - } - - if (_param_ra_max_speed.get() > 0.f) { // Feed-forward term - throttle += math::interpolate(desired_speed, - 0.f, _param_ra_max_speed.get(), - 0.f, 1.f); + _mission_finished = mission_result.finished; } - - // Publish ackermann controller status (logging) - _rover_ackermann_guidance_status.timestamp = _timestamp; - _rover_ackermann_guidance_status.desired_speed = desired_speed; - _rover_ackermann_guidance_status.pid_throttle_integral = _pid_throttle.integral; - _rover_ackermann_guidance_status_pub.publish(_rover_ackermann_guidance_status); - - // Return motor setpoints - motor_setpoint motor_setpoint_temp; - motor_setpoint_temp.steering = math::interpolate(_desired_steering, -_param_ra_max_steer_angle.get(), - _param_ra_max_steer_angle.get(), - -1.f, 1.f); - motor_setpoint_temp.throttle = math::constrain(throttle, 0.f, 1.f); - return motor_setpoint_temp; } -void RoverAckermannGuidance::updateWaypoints() +void RoverAckermannGuidance::updateWaypointsAndAcceptanceRadius() { position_setpoint_triplet_s position_setpoint_triplet{}; _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); // Global waypoint coordinates + Vector2d prev_wp = _curr_pos.isAllFinite() ? _curr_pos : Vector2d(0, 0); // Fallback if previous waypoint is invalid + Vector2d curr_wp(0, 0); + Vector2d next_wp = _home_position.isAllFinite() ? _home_position : Vector2d(0, 0); // Enables corner slow down with RTL + 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); + 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); + 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); + next_wp = Vector2d(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon); - } else { - _next_wp = _home_position; // Enables corner slow down with RTL } + // Distances to waypoints + _distance_to_prev_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), + prev_wp(0), prev_wp(1)); + _distance_to_curr_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), + curr_wp(0), curr_wp(1)); + _distance_to_next_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), + next_wp(0), next_wp(1)); + // 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)); + _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)); // Update acceptance radius _prev_acceptance_radius = _acceptance_radius; @@ -287,24 +233,90 @@ float RoverAckermannGuidance::updateAcceptanceRadius(const Vector2f &curr_wp_ned return acceptance_radius; } -float RoverAckermannGuidance::calcDesiredSteering(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, - const Vector2f &curr_pos_ned, const float wheel_base, const float desired_speed, const float vehicle_yaw) +float RoverAckermannGuidance::calcDesiredSpeed(const float miss_vel_def, const float miss_vel_min, + const float miss_vel_gain, const float distance_to_prev_wp, const float distance_to_curr_wp, const float acc_rad, + const float prev_acc_rad, const float max_accel, const float max_jerk, const int nav_state) +{ + // Catch improper values + if (miss_vel_min < 0.f || miss_vel_min > miss_vel_def || miss_vel_gain < FLT_EPSILON) { + return miss_vel_def; + } + + // Cornering slow down effect + if (distance_to_prev_wp <= prev_acc_rad && prev_acc_rad > FLT_EPSILON) { + const float cornering_speed = miss_vel_gain / prev_acc_rad; + return math::constrain(cornering_speed, miss_vel_min, miss_vel_def); + + } else if (distance_to_curr_wp <= acc_rad && acc_rad > FLT_EPSILON) { + const float cornering_speed = miss_vel_gain / acc_rad; + return math::constrain(cornering_speed, miss_vel_min, miss_vel_def); + + } + + // Straight line speed + if (max_accel > FLT_EPSILON && max_jerk > FLT_EPSILON && acc_rad > FLT_EPSILON) { + float max_velocity{0.f}; + + if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { + max_velocity = math::trajectory::computeMaxSpeedFromDistance(max_jerk, + max_accel, distance_to_curr_wp, 0.f); + + } else { + const float cornering_speed = miss_vel_gain / acc_rad; + max_velocity = math::trajectory::computeMaxSpeedFromDistance(max_jerk, + max_accel, distance_to_curr_wp - acc_rad, cornering_speed); + } + + return math::constrain(max_velocity, miss_vel_min, miss_vel_def); + + } else { + return miss_vel_def; + } + +} + +float RoverAckermannGuidance::calcDesiredSteering(PurePursuit &pure_pursuit, const Vector2f &curr_wp_ned, + const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned, const float wheel_base, const float desired_speed, + const float vehicle_yaw, const float max_steering) { - // Calculate desired steering to reach lookahead point - const float desired_heading = _pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, curr_pos_ned, + const float desired_heading = pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, curr_pos_ned, desired_speed); - const float lookahead_distance = _pure_pursuit.getLookaheadDistance(); + const float lookahead_distance = pure_pursuit.getLookaheadDistance(); const float heading_error = matrix::wrap_pi(desired_heading - vehicle_yaw); // For logging _rover_ackermann_guidance_status.lookahead_distance = lookahead_distance; _rover_ackermann_guidance_status.heading_error = (heading_error * 180.f) / (M_PI_F); + float desired_steering{0.f}; + if (math::abs_t(heading_error) <= M_PI_2_F) { - return atanf(2 * wheel_base * sinf(heading_error) / lookahead_distance); + desired_steering = atanf(2 * wheel_base * sinf(heading_error) / lookahead_distance); + } else { - return atanf(2 * wheel_base * (sign(heading_error) * 1.0f + sinf(heading_error - sign(heading_error) * M_PI_2_F)) / - lookahead_distance); + desired_steering = atanf(2 * wheel_base * (sign(heading_error) * 1.0f + sinf(heading_error - + sign(heading_error) * M_PI_2_F)) / lookahead_distance); + } + + return math::constrain(desired_steering, -max_steering, max_steering); + +} + +float RoverAckermannGuidance::calcThrottleSetpoint(PID_t &pid_throttle, const float desired_speed, + const float actual_speed, const float max_speed, const float dt) +{ + float throttle = 0.f; + + if (desired_speed < FLT_EPSILON) { + pid_reset_integral(&pid_throttle); + + } else { + throttle = pid_calculate(&pid_throttle, desired_speed, actual_speed, 0, dt); + } + + if (_param_ra_max_speed.get() > 0.f) { // Feed-forward term + throttle += math::interpolate(desired_speed, 0.f, max_speed, 0.f, 1.f); } + return math::constrain(throttle, 0.f, 1.f); } diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp index 592083556f13..23f63ed9280e 100644 --- a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp +++ b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp @@ -73,58 +73,106 @@ class RoverAckermannGuidance : public ModuleParams RoverAckermannGuidance(ModuleParams *parent); ~RoverAckermannGuidance() = default; + /** + * @brief Struct for steering and throttle setpoints. + * @param steering Steering setpoint. + * @param throttle Throttle setpoint. + */ struct motor_setpoint { float steering{0.f}; float throttle{0.f}; }; /** - * @brief Compute guidance for ackermann rover and return motor_setpoint for throttle and steering. - * @param nav_state Vehicle navigation state + * @brief Calculate motor setpoints based on the mission plan. + * @param nav_state Vehicle navigation state. + * @return Motor setpoints for throttle and steering. */ motor_setpoint computeGuidance(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 and acceptance radius + * @brief Update global/NED waypoint coordinates and acceptance radius. */ - void updateWaypoints(); + void updateWaypointsAndAcceptanceRadius(); /** - * @brief Returns and publishes the acceptance radius for current waypoint based on the angle between a line segment - * from the previous to the current waypoint/current to the next waypoint and maximum steer angle of - * the vehicle. - * @param curr_wp_ned Current waypoint in NED frame. - * @param prev_wp_ned Previous waypoint in NED frame. - * @param next_wp_ned Next waypoint in NED frame. - * @param default_acceptance_radius Default acceptance radius for waypoints. - * @param acceptance_radius_gain Scaling of the geometric optimal acceptance radius for the rover to cut corners. - * @param acceptance_radius_max Maximum value for the acceptance radius. - * @param wheel_base Rover wheelbase. - * @param max_steer_angle Rover maximum steer angle. + * @brief Publish the acceptance radius for current waypoint based on the angle between a line segment + * from the previous to the current waypoint/current to the next waypoint and maximum steer angle of the vehicle. + * @param curr_wp_ned Current waypoint in NED frame [m]. + * @param prev_wp_ned Previous waypoint in NED frame [m]. + * @param next_wp_ned Next waypoint in NED frame [m]. + * @param default_acceptance_radius Default acceptance radius for waypoints [m]. + * @param acceptance_radius_gain Tuning parameter that scales the geometric optimal acceptance radius for the corner cutting [-]. + * @param acceptance_radius_max Maximum value for the acceptance radius [m]. + * @param wheel_base Rover wheelbase [m]. + * @param max_steer_angle Rover maximum steer angle [rad]. + * @return Updated acceptance radius [m]. */ float updateAcceptanceRadius(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, const Vector2f &next_wp_ned, float default_acceptance_radius, float acceptance_radius_gain, float acceptance_radius_max, float wheel_base, float max_steer_angle); + /** - * @brief Calculate and return desired steering input - * @param curr_wp_ned Current waypoint in NED frame. - * @param prev_wp_ned Previous waypoint in NED frame. - * @param curr_pos_ned Current position of the vehicle in NED frame. - * @param wheel_base Rover wheelbase. - * @param desired_speed Desired speed for the rover. - * @param vehicle_yaw Current yaw of the rover. + * @brief Calculate the desired speed setpoint. During cornering the speed is calculated as the inverse + * of the acceptance radius multiplied with a tuning factor. On straight lines it is based on a velocity trajectory + * such that the rover will arrive at the next corner with the desired cornering speed under consideration of the + * maximum acceleration and jerk. + * @param miss_vel_def Default desired velocity for the rover during mission [m/s]. + * @param miss_vel_min Minimum desired velocity for the rover during mission [m/s]. + * @param miss_vel_gain Tuning parameter for the slow down effect during cornering [-]. + * @param distance_to_prev_wp Distance to the previous waypoint [m]. + * @param distance_to_curr_wp Distance to the current waypoint [m]. + * @param acc_rad Acceptance radius of the current waypoint [m]. + * @param prev_acc_rad Acceptance radius of the previous waypoint [m]. + * @param max_accel Maximum allowed acceleration for the rover [m/s^2]. + * @param max_jerk Maximum allowed jerk for the rover [m/s^3]. + * @param nav_state Current nav_state of the rover. + * @return Speed setpoint for the rover [m/s]. */ - float calcDesiredSteering(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned, - float wheel_base, float desired_speed, float vehicle_yaw); + float calcDesiredSpeed(float miss_vel_def, float miss_vel_min, float miss_vel_gain, float distance_to_prev_wp, + float distance_to_curr_wp, float acc_rad, float prev_acc_rad, float max_accel, float max_jerk, int nav_state); -protected: /** - * @brief Update the parameters of the module. + * @brief Calculate desired steering angle. The desired steering is calulated as the steering that is required to + * reach the point calculated using the pure pursuit algorithm (see PurePursuit.hpp). + * @param pure_pursuit Pure pursuit class instance. + * @param curr_wp_ned Current waypoint in NED frame [m]. + * @param prev_wp_ned Previous waypoint in NED frame [m]. + * @param curr_pos_ned Current position of the vehicle in NED frame [m]. + * @param wheel_base Rover wheelbase [m]. + * @param desired_speed Desired speed for the rover [m/s]. + * @param vehicle_yaw Current yaw of the rover [rad]. + * @param max_steering Maximum steering angle of the rover [rad]. + * @return Steering setpoint for the rover [rad]. */ - void updateParams() override; + float calcDesiredSteering(PurePursuit &pure_pursuit, const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, + const Vector2f &curr_pos_ned, float wheel_base, float desired_speed, float vehicle_yaw, float max_steering); + + /** + * @brief Calculate the throttle setpoint. Calculated with a PID controller using the difference between + * the desired/actual speed and a feedforward term based on the full throttle speed. + * @param pid_throttle Reference to PID instance. + * @param desired_speed Reference speed for the rover [m/s]. + * @param actual_speed Actual speed of the rover [m/s]. + * @param max_speed Rover speed at full throttle [m/s]. + * @param dt Time interval since last update [s]. + * @return Normalized throttle setpoint [0, 1]. + */ + float calcThrottleSetpoint(PID_t &pid_throttle, float desired_speed, float actual_speed, float max_speed, float dt); -private: // uORB subscriptions uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; @@ -138,27 +186,31 @@ class RoverAckermannGuidance : public ModuleParams uORB::Publication _position_controller_status_pub{ORB_ID(position_controller_status)}; rover_ackermann_guidance_status_s _rover_ackermann_guidance_status{}; - - MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates. + // Class instances + MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates PurePursuit _pure_pursuit{this}; // Pure pursuit library // Rover variables + float _desired_steering{0.f}; + float _vehicle_yaw{0.f}; + float _desired_speed{0.f}; + float _actual_speed{0.f}; Vector2d _curr_pos{}; Vector2f _curr_pos_ned{}; PID_t _pid_throttle; hrt_abstime _timestamp{0}; - float _desired_steering{0.f}; // Waypoint variables - Vector2d _curr_wp{}; - Vector2d _next_wp{}; - Vector2d _prev_wp{}; Vector2d _home_position{}; Vector2f _curr_wp_ned{}; Vector2f _prev_wp_ned{}; Vector2f _next_wp_ned{}; + float _distance_to_prev_wp{0.f}; + float _distance_to_curr_wp{0.f}; + float _distance_to_next_wp{0.f}; float _acceptance_radius{0.5f}; float _prev_acceptance_radius{0.5f}; + bool _mission_finished{false}; // Parameters DEFINE_PARAMETERS( From a294e011ab1aac3fc3a2239c33eaa1326b914dab Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Mon, 5 Aug 2024 15:47:14 +0200 Subject: [PATCH 30/90] purePursuit: fix commenting error --- src/lib/pure_pursuit/PurePursuit.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/lib/pure_pursuit/PurePursuit.hpp b/src/lib/pure_pursuit/PurePursuit.hpp index 66b9b8ce7cfe..9de498c2e1b1 100644 --- a/src/lib/pure_pursuit/PurePursuit.hpp +++ b/src/lib/pure_pursuit/PurePursuit.hpp @@ -95,9 +95,9 @@ class PurePursuit : public ModuleParams * @param prev_wp_ned North/East coordinates of previous waypoint in NED frame [m]. * @param curr_pos_ned North/East coordinates of current position of the vehicle in NED frame [m]. * @param vehicle_speed Vehicle speed [m/s]. - * @param RA_LOOKAHD_GAIN Tuning parameter [-] - * @param RA_LOOKAHD_MAX Maximum lookahead distance [m] - * @param RA_LOOKAHD_MIN Minimum lookahead distance [m] + * @param PP_LOOKAHD_GAIN Tuning parameter [-] + * @param PP_LOOKAHD_MAX Maximum lookahead distance [m] + * @param PP_LOOKAHD_MIN Minimum lookahead distance [m] */ float calcDesiredHeading(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned, float vehicle_speed); From 718d308d918e8fc99176d8dba86e05ee36757e27 Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Thu, 8 Aug 2024 12:06:06 +1000 Subject: [PATCH 31/90] parameter markdown - correct reboot in tables --- src/lib/parameters/px4params/markdownout.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/parameters/px4params/markdownout.py b/src/lib/parameters/px4params/markdownout.py index 34e6f9e38666..182324f6711d 100644 --- a/src/lib/parameters/px4params/markdownout.py +++ b/src/lib/parameters/px4params/markdownout.py @@ -81,7 +81,7 @@ def __init__(self, groups): if bitmask_list: result += bitmask_output # Format the ranges as a table. - result += f"Reboot | minValue | maxValue | increment | default | unit\n--- | --- | --- | --- | --- | ---\n{reboot_required} | {min_val} | {max_val} | {increment} | {def_val} | {unit} \n\n" + result += f"Reboot | minValue | maxValue | increment | default | unit\n--- | --- | --- | --- | --- | ---\n{'✓' if reboot_required else ' ' } | {min_val} | {max_val} | {increment} | {def_val} | {unit} \n\n" self.output = result From 82be5cd44fb27df8d14c3eca2a833046509d8f5a Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Thu, 8 Aug 2024 12:38:06 +1000 Subject: [PATCH 32/90] Strip short description from long one --- src/lib/parameters/px4params/markdownout.py | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/lib/parameters/px4params/markdownout.py b/src/lib/parameters/px4params/markdownout.py index 182324f6711d..3666182568c5 100644 --- a/src/lib/parameters/px4params/markdownout.py +++ b/src/lib/parameters/px4params/markdownout.py @@ -26,12 +26,20 @@ def __init__(self, groups): for param in group.GetParams(): name = param.GetName() short_desc = param.GetFieldValue("short_desc") or '' + #short_desc = html.escape(short_desc) + + # Add fullstop to short_desc if not present if short_desc: if not short_desc.strip().endswith('.'): short_desc += "." - #short_desc = html.escape(short_desc) + long_desc = param.GetFieldValue("long_desc") or '' - #long_desc = html.escape(long_desc) + #long_desc = html.escape(long_desc) + + #Strip out short text from start of long text, if it ends in fullstop + if long_desc.startswith(short_desc): + long_desc = long_desc[len(short_desc):].lstrip() + min_val = param.GetFieldValue("min") or '' max_val = param.GetFieldValue("max") or '' increment = param.GetFieldValue("increment") or '' From 58a699e3cbb317ab2252b6cec512dff655e8fe21 Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Fri, 9 Aug 2024 06:12:38 +1000 Subject: [PATCH 33/90] Strip out html escape comments --- src/lib/parameters/px4params/markdownout.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/lib/parameters/px4params/markdownout.py b/src/lib/parameters/px4params/markdownout.py index 3666182568c5..4c698fb5115a 100644 --- a/src/lib/parameters/px4params/markdownout.py +++ b/src/lib/parameters/px4params/markdownout.py @@ -26,7 +26,6 @@ def __init__(self, groups): for param in group.GetParams(): name = param.GetName() short_desc = param.GetFieldValue("short_desc") or '' - #short_desc = html.escape(short_desc) # Add fullstop to short_desc if not present if short_desc: @@ -34,7 +33,6 @@ def __init__(self, groups): short_desc += "." long_desc = param.GetFieldValue("long_desc") or '' - #long_desc = html.escape(long_desc) #Strip out short text from start of long text, if it ends in fullstop if long_desc.startswith(short_desc): From 6cf0bf5e19ca006cd434d29fa7e5279396087bc2 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 20 Sep 2023 15:14:36 +0200 Subject: [PATCH 34/90] Support MAVLink extension MANUAL_CONTROL.aux Note that in uORB we don't currently know if the aux fields are specifically valid or not so we can also not set the corresponding bits in the field. --- src/modules/mavlink/mavlink_receiver.cpp | 13 +++++++ .../mavlink/streams/MANUAL_CONTROL.hpp | 38 +++++++++++++++++-- 2 files changed, 47 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 06fb97c8a65f..e7c679295fa2 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2110,6 +2110,19 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) manual_control_setpoint.yaw = mavlink_manual_control.r / 1000.f; // Pass along the button states manual_control_setpoint.buttons = mavlink_manual_control.buttons; + + if (mavlink_manual_control.enabled_extensions & (1u << 2)) { manual_control_setpoint.aux1 = mavlink_manual_control.aux1 / 1000.0f; } + + if (mavlink_manual_control.enabled_extensions & (1u << 3)) { manual_control_setpoint.aux2 = mavlink_manual_control.aux2 / 1000.0f; } + + if (mavlink_manual_control.enabled_extensions & (1u << 4)) { manual_control_setpoint.aux3 = mavlink_manual_control.aux3 / 1000.0f; } + + if (mavlink_manual_control.enabled_extensions & (1u << 5)) { manual_control_setpoint.aux4 = mavlink_manual_control.aux4 / 1000.0f; } + + if (mavlink_manual_control.enabled_extensions & (1u << 6)) { manual_control_setpoint.aux5 = mavlink_manual_control.aux5 / 1000.0f; } + + if (mavlink_manual_control.enabled_extensions & (1u << 7)) { manual_control_setpoint.aux6 = mavlink_manual_control.aux6 / 1000.0f; } + manual_control_setpoint.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0 + _mavlink.get_instance_id(); manual_control_setpoint.timestamp = manual_control_setpoint.timestamp_sample = hrt_absolute_time(); manual_control_setpoint.valid = true; diff --git a/src/modules/mavlink/streams/MANUAL_CONTROL.hpp b/src/modules/mavlink/streams/MANUAL_CONTROL.hpp index 8be68fdc3fad..3feabf124d68 100644 --- a/src/modules/mavlink/streams/MANUAL_CONTROL.hpp +++ b/src/modules/mavlink/streams/MANUAL_CONTROL.hpp @@ -68,10 +68,10 @@ class MavlinkStreamManualControl : public MavlinkStream mavlink_manual_control_t msg{}; msg.target = mavlink_system.sysid; - msg.x = manual_control_setpoint.pitch * 1000; - msg.y = manual_control_setpoint.roll * 1000; - msg.z = manual_control_setpoint.throttle * 1000; - msg.r = manual_control_setpoint.yaw * 1000; + msg.x = manual_control_setpoint.pitch * 1000.f; + msg.y = manual_control_setpoint.roll * 1000.f; + msg.z = manual_control_setpoint.throttle * 1000.f; + msg.r = manual_control_setpoint.yaw * 1000.f; manual_control_switches_s manual_control_switches{}; @@ -84,6 +84,36 @@ class MavlinkStreamManualControl : public MavlinkStream msg.buttons |= (manual_control_switches.kill_switch << (shift * 6)); } + if (PX4_ISFINITE(manual_control_setpoint.aux1)) { + msg.enabled_extensions |= (1u << 2); + msg.aux1 = manual_control_setpoint.aux1 * 1000.f; + } + + if (PX4_ISFINITE(manual_control_setpoint.aux2)) { + msg.enabled_extensions |= (1u << 3); + msg.aux2 = manual_control_setpoint.aux2 * 1000.f; + } + + if (PX4_ISFINITE(manual_control_setpoint.aux3)) { + msg.enabled_extensions |= (1u << 4); + msg.aux3 = manual_control_setpoint.aux3 * 1000.f; + } + + if (PX4_ISFINITE(manual_control_setpoint.aux4)) { + msg.enabled_extensions |= (1u << 5); + msg.aux4 = manual_control_setpoint.aux4 * 1000.f; + } + + if (PX4_ISFINITE(manual_control_setpoint.aux5)) { + msg.enabled_extensions |= (1u << 6); + msg.aux5 = manual_control_setpoint.aux5 * 1000.f; + } + + if (PX4_ISFINITE(manual_control_setpoint.aux6)) { + msg.enabled_extensions |= (1u << 7); + msg.aux6 = manual_control_setpoint.aux6 * 1000.f; + } + mavlink_msg_manual_control_send_struct(_mavlink->get_channel(), &msg); return true; From f04aa2494b39f851b883b1f53b132d7f2086c92f Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 31 Jul 2024 15:14:16 +0200 Subject: [PATCH 35/90] FW pos control: do not requre global pos in manual position control --- .../fw_pos_control/FixedwingPositionControl.cpp | 15 ++++++--------- .../fw_pos_control/FixedwingPositionControl.hpp | 2 +- 2 files changed, 7 insertions(+), 10 deletions(-) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 9571c2986591..daef50572dc9 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -2158,28 +2158,25 @@ FixedwingPositionControl::control_manual_position(const float control_interval, if (_yaw_lock_engaged) { - /* just switched back from non heading-hold to heading hold */ + Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; + if (!_hdg_hold_enabled) { + // just switched back from non heading-hold to heading hold _hdg_hold_enabled = true; _hdg_hold_yaw = _yaw; - _hdg_hold_position.lat = _current_latitude; - _hdg_hold_position.lon = _current_longitude; + _hdg_hold_position = curr_pos_local; } // if there's a reset-by-fusion, the ekf needs some time to converge, // therefore we go into track holiding for 2 seconds if (_local_pos.timestamp - _time_last_xy_reset < 2_s) { - _hdg_hold_position.lat = _current_latitude; - _hdg_hold_position.lon = _current_longitude; + _hdg_hold_position = curr_pos_local; } - Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; - Vector2f curr_wp_local = _global_local_proj_ref.project(_hdg_hold_position.lat, _hdg_hold_position.lon); - _npfg.setAirspeedNom(calibrated_airspeed_sp * _eas2tas); _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); - navigateLine(curr_wp_local, _hdg_hold_yaw, curr_pos_local, ground_speed, _wind_vel); + navigateLine(_hdg_hold_position, _hdg_hold_yaw, curr_pos_local, ground_speed, _wind_vel); _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); calibrated_airspeed_sp = _npfg.getAirspeedRef() / _eas2tas; diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index 77eb88eb6511..84300ae1b409 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -298,7 +298,7 @@ class FixedwingPositionControl final : public ModuleBase Date: Wed, 31 Jul 2024 15:15:56 +0200 Subject: [PATCH 36/90] commander: extend local position 'relaxed' validity Relaxed position is valid as long as a velocity aiding source is active (e.g.: optical flow or airspeed+sideslip) --- .../checks/estimatorCheck.cpp | 26 +++---------------- .../checks/estimatorCheck.hpp | 2 -- 2 files changed, 4 insertions(+), 24 deletions(-) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index 2ede2baf6381..888835ba7ef6 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -533,19 +533,6 @@ void EstimatorChecks::checkEstimatorStatusFlags(const Context &context, Report & estimator_status_flags_s estimator_status_flags; if (_estimator_status_flags_sub.copy(&estimator_status_flags)) { - - bool dead_reckoning = estimator_status_flags.cs_wind_dead_reckoning - || estimator_status_flags.cs_inertial_dead_reckoning; - - if (!dead_reckoning) { - // position requirements (update if not dead reckoning) - bool gps = estimator_status_flags.cs_gps; - bool optical_flow = estimator_status_flags.cs_opt_flow; - bool vision_position = estimator_status_flags.cs_ev_pos; - - _position_reliant_on_optical_flow = !gps && optical_flow && !vision_position; - } - // Check for a magnetometer fault and notify the user if (estimator_status_flags.cs_mag_fault) { /* EVENT @@ -722,15 +709,6 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f // Check if quality checking of position accuracy and consistency is to be performed const float lpos_eph_threshold = (_param_com_pos_fs_eph.get() < 0) ? INFINITY : _param_com_pos_fs_eph.get(); - float lpos_eph_threshold_relaxed = lpos_eph_threshold; - - // Set the allowable position uncertainty based on combination of flight and estimator state - // When we are in a operator demanded position control mode and are solely reliant on optical flow, - // do not check position error because it will gradually increase throughout flight and the operator will compensate for the drift - if (_position_reliant_on_optical_flow) { - lpos_eph_threshold_relaxed = INFINITY; - } - bool xy_valid = lpos.xy_valid && !_nav_test_failed; bool v_xy_valid = lpos.v_xy_valid && !_nav_test_failed; @@ -793,6 +771,10 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f !checkPosVelValidity(now, xy_valid, lpos.eph, lpos_eph_threshold, lpos.timestamp, _last_lpos_fail_time_us, !failsafe_flags.local_position_invalid); + + // In some modes we assume that the operator will compensate for the drift so we do not need to check the position error + const float lpos_eph_threshold_relaxed = INFINITY; + failsafe_flags.local_position_invalid_relaxed = !checkPosVelValidity(now, xy_valid, lpos.eph, lpos_eph_threshold_relaxed, lpos.timestamp, _last_lpos_relaxed_fail_time_us, !failsafe_flags.local_position_invalid_relaxed); diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp index 6578776ee99d..4f61df88670f 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp @@ -100,8 +100,6 @@ class EstimatorChecks : public HealthAndArmingCheckBase bool _nav_test_passed{false}; ///< true if the post takeoff navigation test has passed bool _nav_test_failed{false}; ///< true if the post takeoff navigation test has failed - bool _position_reliant_on_optical_flow{false}; - bool _gps_was_fused{false}; bool _gnss_spoofed{false}; From 3f17f155058e264d75b9e1184417c4b1d5856a9c Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 31 Jul 2024 15:17:52 +0200 Subject: [PATCH 37/90] commander: allow FW manual pos control without global position --- src/modules/commander/ModeUtil/mode_requirements.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/modules/commander/ModeUtil/mode_requirements.cpp b/src/modules/commander/ModeUtil/mode_requirements.cpp index 0718804df282..32eb5b1e8116 100644 --- a/src/modules/commander/ModeUtil/mode_requirements.cpp +++ b/src/modules/commander/ModeUtil/mode_requirements.cpp @@ -82,11 +82,6 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags) setRequirement(vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW, flags.mode_req_local_position_relaxed); setRequirement(vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW, flags.mode_req_manual_control); - if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_global_position); - setRequirement(vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW, flags.mode_req_global_position); - } - // NAVIGATION_STATE_AUTO_MISSION setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_angular_velocity); setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_attitude); From fdfe43471e60d2ff220a7e666bf77cc99d2b280c Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 9 Aug 2024 16:13:44 +0200 Subject: [PATCH 38/90] mavlink_receiver: limit access through instances with gimbal mode This adds explicit handling for the few things we want to allow through a MAVLink instance dedicated to a gimbal/(camera) payload as per the MAVLink gimbal mode configuration. --- src/modules/mavlink/mavlink_receiver.cpp | 39 +++++++++++++++++++++++- src/modules/mavlink/mavlink_receiver.h | 1 + 2 files changed, 39 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e7c679295fa2..8e03e8bd40bc 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -406,6 +406,34 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) _mavlink.handle_message(msg); } +void MavlinkReceiver::handle_messages_in_gimbal_mode(mavlink_message_t &msg) +{ + switch (msg.msgid) { + case MAVLINK_MSG_ID_HEARTBEAT: + handle_message_heartbeat(&msg); + break; + + case MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE: + handle_message_gimbal_manager_set_attitude(&msg); + break; + + case MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL: + handle_message_gimbal_manager_set_manual_control(&msg); + break; + + case MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION: + handle_message_gimbal_device_information(&msg); + break; + + case MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS: + handle_message_gimbal_device_attitude_status(&msg); + break; + } + + // Message forwarding + _mavlink.handle_message(&msg); +} + bool MavlinkReceiver::evaluate_target_ok(int command, int target_system, int target_component) { @@ -3176,7 +3204,16 @@ MavlinkReceiver::run() _mavlink.set_proto_version(2); } - handle_message(&msg); + switch (_mavlink.get_mode()) { + case Mavlink::MAVLINK_MODE::MAVLINK_MODE_GIMBAL: + handle_messages_in_gimbal_mode(msg); + break; + + default: + handle_message(&msg); + break; + } + _mavlink.set_has_received_messages(true); // Received first message, unlock wait to transmit '-w' command-line flag update_rx_stats(msg); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 9f2882e84835..b95bdca59a72 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -155,6 +155,7 @@ class MavlinkReceiver : public ModuleParams float param4 = 0.0f, float param5 = 0.0f, float param6 = 0.0f, float param7 = 0.0f); void handle_message(mavlink_message_t *msg); + void handle_messages_in_gimbal_mode(mavlink_message_t &msg); void handle_message_adsb_vehicle(mavlink_message_t *msg); void handle_message_att_pos_mocap(mavlink_message_t *msg); From f4f93118e6ea00fc2a848fc2ce6319eeecf48af8 Mon Sep 17 00:00:00 2001 From: Alexis Guijarro Date: Thu, 8 Aug 2024 02:39:54 +0000 Subject: [PATCH 39/90] mRo boards: Fix for USART clock selection --- boards/mro/ctrl-zero-classic/nuttx-config/include/board.h | 6 ++++++ boards/mro/ctrl-zero-h7-oem/nuttx-config/include/board.h | 6 ++++++ boards/mro/ctrl-zero-h7/nuttx-config/include/board.h | 6 ++++++ boards/mro/pixracerpro/nuttx-config/include/board.h | 6 ++++++ 4 files changed, 24 insertions(+) diff --git a/boards/mro/ctrl-zero-classic/nuttx-config/include/board.h b/boards/mro/ctrl-zero-classic/nuttx-config/include/board.h index 52206cccd00f..ea2147442699 100644 --- a/boards/mro/ctrl-zero-classic/nuttx-config/include/board.h +++ b/boards/mro/ctrl-zero-classic/nuttx-config/include/board.h @@ -197,6 +197,12 @@ #define STM32_FDCANCLK STM32_HSE_FREQUENCY +/* UART clock selection */ +/* reset to default to overwrite any changes done by any bootloader */ + +#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC +#define STM32_RCC_D2CCIP2R_USART16_SEL RCC_D2CCIP2R_USART16SEL_RCC + /* FLASH wait states */ #define BOARD_FLASH_WAITSTATES 2 diff --git a/boards/mro/ctrl-zero-h7-oem/nuttx-config/include/board.h b/boards/mro/ctrl-zero-h7-oem/nuttx-config/include/board.h index d5ffa2c5848e..14a6a28d2b88 100644 --- a/boards/mro/ctrl-zero-h7-oem/nuttx-config/include/board.h +++ b/boards/mro/ctrl-zero-h7-oem/nuttx-config/include/board.h @@ -197,6 +197,12 @@ #define STM32_FDCANCLK STM32_HSE_FREQUENCY +/* UART clock selection */ +/* reset to default to overwrite any changes done by any bootloader */ + +#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC +#define STM32_RCC_D2CCIP2R_USART16_SEL RCC_D2CCIP2R_USART16SEL_RCC + /* FLASH wait states */ #define BOARD_FLASH_WAITSTATES 2 diff --git a/boards/mro/ctrl-zero-h7/nuttx-config/include/board.h b/boards/mro/ctrl-zero-h7/nuttx-config/include/board.h index cdbd3dd3e629..4a20575d70ae 100644 --- a/boards/mro/ctrl-zero-h7/nuttx-config/include/board.h +++ b/boards/mro/ctrl-zero-h7/nuttx-config/include/board.h @@ -196,6 +196,12 @@ #define STM32_FDCANCLK STM32_HSE_FREQUENCY +/* UART clock selection */ +/* reset to default to overwrite any changes done by any bootloader */ + +#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC +#define STM32_RCC_D2CCIP2R_USART16_SEL RCC_D2CCIP2R_USART16SEL_RCC + /* FLASH wait states */ #define BOARD_FLASH_WAITSTATES 2 diff --git a/boards/mro/pixracerpro/nuttx-config/include/board.h b/boards/mro/pixracerpro/nuttx-config/include/board.h index 5edda931dc10..96b2ed2022fb 100644 --- a/boards/mro/pixracerpro/nuttx-config/include/board.h +++ b/boards/mro/pixracerpro/nuttx-config/include/board.h @@ -196,6 +196,12 @@ #define STM32_FDCANCLK STM32_HSE_FREQUENCY +/* UART clock selection */ +/* reset to default to overwrite any changes done by any bootloader */ + +#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC +#define STM32_RCC_D2CCIP2R_USART16_SEL RCC_D2CCIP2R_USART16SEL_RCC + /* FLASH wait states */ #define BOARD_FLASH_WAITSTATES 2 From 1211aad9b043bf747ceefc9c6b5e8ad47acf3a08 Mon Sep 17 00:00:00 2001 From: Sergei Date: Fri, 9 Aug 2024 09:37:20 -0500 Subject: [PATCH 40/90] Reasonable defaults for Lawnmower SITL --- .../init.d-posix/airframes/4011_gz_lawnmower | 27 ++++++++++++++----- src/modules/rover_differential/module.yaml | 4 +-- 2 files changed, 23 insertions(+), 8 deletions(-) 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 01fc515c4370..c0d91e222365 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower @@ -15,14 +15,29 @@ 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 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 -# Set Differential Drive Kinematics Library parameters: -param set RD_WHEEL_BASE 0.9 -param set RD_WHEEL_RADIUS 0.22 -param set RD_WHEEL_SPEED 10.0 # Maximum wheel speed rad/s, approx 8 km/h +# Rover parameters +param set-default RD_WHEEL_TRACK 0.9 +param set-default RD_MAN_YAW_SCALE 0.1 +param set-default RD_YAW_RATE_I 0.1 +param set-default RD_YAW_RATE_P 5 +param set-default RD_MAX_ACCEL 1 +param set-default RD_MAX_JERK 3 +param set-default RD_MAX_SPEED 8 +param set-default RD_HEADING_P 5 +param set-default RD_HEADING_I 0.1 +param set-default RD_MAX_YAW_RATE 30 +param set-default RD_MISS_SPD_DEF 8 +param set-default RD_TRANS_DRV_TRN 0.349066 +param set-default RD_TRANS_TRN_DRV 0.174533 + +# Pure pursuit parameters +param set-default PP_LOOKAHD_MAX 30 +param set-default PP_LOOKAHD_MIN 2 +param set-default PP_LOOKAHD_GAIN 1 # Actuator mapping - set SITL motors/servos output parameters: @@ -36,7 +51,7 @@ param set-default SIM_GZ_WH_FUNC1 101 # right wheel param set-default SIM_GZ_WH_FUNC2 102 # left wheel #param set-default SIM_GZ_WH_MIN2 0 #param set-default SIM_GZ_WH_MAX2 200 -#aram set-default SIM_GZ_WH_DIS2 100 +#param set-default SIM_GZ_WH_DIS2 100 #param set-default SIM_GZ_WH_FAIL2 100 param set-default SIM_GZ_WH_REV 0 # no need to reverse any wheels diff --git a/src/modules/rover_differential/module.yaml b/src/modules/rover_differential/module.yaml index b6819305bc5f..be0a76c60e9b 100644 --- a/src/modules/rover_differential/module.yaml +++ b/src/modules/rover_differential/module.yaml @@ -155,7 +155,7 @@ parameters: type: float unit: rad min: 0.001 - max: 180 + max: 3.14159 increment: 0.01 decimal: 3 default: 0.0872665 @@ -166,7 +166,7 @@ parameters: type: float unit: rad min: 0.001 - max: 180 + max: 3.14159 increment: 0.01 decimal: 3 default: 0.174533 From 64056fc7bbafc701f6ac3f84c1a63902525ff4f3 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 5 Aug 2024 16:55:36 +0200 Subject: [PATCH 41/90] SYS_STATUS: fill correct attitude, horizontal position flags --- src/modules/mavlink/streams/SYS_STATUS.hpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/streams/SYS_STATUS.hpp b/src/modules/mavlink/streams/SYS_STATUS.hpp index 272947b1ad77..c875eaceaae9 100644 --- a/src/modules/mavlink/streams/SYS_STATUS.hpp +++ b/src/modules/mavlink/streams/SYS_STATUS.hpp @@ -150,7 +150,11 @@ class MavlinkStreamSysStatus : public MavlinkStream fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_3D_MAG, health_component_t::magnetometer, msg); fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_GPS, health_component_t::gps, msg); fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_RC_RECEIVER, health_component_t::remote_control, msg); - fillOutComponent(health_report, MAV_SYS_STATUS_AHRS, health_component_t::local_position_estimate, msg); + fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION, health_component_t::attitude_estimate, + msg); + fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL, health_component_t::local_position_estimate, + msg); + fillOutComponent(health_report, MAV_SYS_STATUS_AHRS, health_component_t::attitude_estimate, msg); fillOutComponent(health_report, MAV_SYS_STATUS_LOGGING, health_component_t::logging, msg); fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE, health_component_t::absolute_pressure, msg); fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE, health_component_t::differential_pressure, From 478875c00615a2a287dd2ec9f9907597613b3669 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 12 Aug 2024 14:01:52 +0200 Subject: [PATCH 42/90] ekf tools: compare gyro integral with attitude estimate --- .../python/tuning_tools/gyro_integration.py | 158 ++++++++++++++++++ 1 file changed, 158 insertions(+) create mode 100644 src/modules/ekf2/EKF/python/tuning_tools/gyro_integration.py diff --git a/src/modules/ekf2/EKF/python/tuning_tools/gyro_integration.py b/src/modules/ekf2/EKF/python/tuning_tools/gyro_integration.py new file mode 100644 index 000000000000..457c1eb8e9bd --- /dev/null +++ b/src/modules/ekf2/EKF/python/tuning_tools/gyro_integration.py @@ -0,0 +1,158 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +""" + Copyright (c) 2024 PX4 Development Team + 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. +""" + +import matplotlib.pylab as plt +from pyulog import ULog +from pyulog.px4 import PX4ULog +import numpy as np +import sym +import symforce.symbolic as sf + +def getAllData(logfile): + log = ULog(logfile) + + gyro = np.matrix([getData(log, 'sensor_combined', 'gyro_rad[0]'), + getData(log, 'sensor_combined', 'gyro_rad[1]'), + getData(log, 'sensor_combined', 'gyro_rad[2]')]) + t_gyro = getTimestampsSeconds(log, 'sensor_combined') + + t_gyro -= t_gyro[0] + + q = np.matrix([getData(log, 'vehicle_attitude', 'q[0]'), + getData(log, 'vehicle_attitude', 'q[1]'), + getData(log, 'vehicle_attitude', 'q[2]'), + getData(log, 'vehicle_attitude', 'q[3]')]) + t_q = getTimestampsSeconds(log, 'vehicle_attitude') + t_q -= t_q[0] + + return (t_gyro, gyro, t_q, q) + +def getData(log, topic_name, variable_name, instance=0): + variable_data = np.array([]) + for elem in log.data_list: + if elem.name == topic_name: + if instance == elem.multi_id: + variable_data = elem.data[variable_name] + break + + return variable_data + +def us2s(time_us): + return time_us * 1e-6 + +def getTimestampsSeconds(log, topic_name, instance=0): + return us2s(getData(log, topic_name, 'timestamp', instance)) + +def integrateAngularRate(t, angular_rate, rot_init=sym.Rot3()): + R = rot_init + roll = [] + pitch = [] + yaw = [] + t_prev = 0 + + for i in range(len(t)): + dt = t[i] - t_prev + R = R * sym.Rot3.from_tangent(angular_rate[:, i] * dt) + att = R.to_yaw_pitch_roll() + yaw = np.append(yaw, att[0]) + pitch = np.append(pitch, att[1]) + roll = np.append(roll, att[2]) + + t_prev = t[i] + + return (roll, pitch, yaw) + +def quat2RollPitchYaw(t, q): + roll = [] + pitch = [] + yaw = [] + + for i in range(len(t)): + vect = sf.V3(float(q[1, i]), float(q[2, i]), float(q[3, i])) + quat = sf.Quaternion(w=q[0, i], xyz=vect) + R = sf.Rot3(quat) + att = R.to_yaw_pitch_roll() + yaw = np.append(yaw, float(att[0].evalf())) + pitch = np.append(pitch, float(att[1].evalf())) + roll = np.append(roll, float(att[2].evalf())) + + return (roll, pitch, yaw) + + +def run(logfile): + (t, gyro, t_q, q) = getAllData(logfile) + + (roll, pitch, yaw) = quat2RollPitchYaw(t_q, q) + (roll_raw, pitch_raw, yaw_raw) = integrateAngularRate(t, gyro, rot_init=sym.Rot3.from_yaw_pitch_roll(yaw[0], pitch[0], roll[0])) + + # Plot data + plt.figure(1) + plt.suptitle(logfile.split('/')[-1]) + + ax1 = plt.subplot(3, 1, 1) + ax1.plot(t_q, np.rad2deg(roll), '-') + ax1.plot(t, np.rad2deg(roll_raw), '--') + ax1.set_ylabel("roll (deg)") + ax1.legend(["estimated", "integrated"]) + ax1.grid() + + ax2 = plt.subplot(3, 1, 2, sharex=ax1) + ax2.plot(t_q, np.rad2deg(pitch)) + ax2.plot(t, np.rad2deg(pitch_raw), '--') + ax2.set_ylabel("pitch (deg)") + ax2.legend(["estimated", "integrated"]) + ax2.grid() + + ax3 = plt.subplot(3, 1, 3, sharex=ax1) + ax3.plot(t_q, np.rad2deg(yaw)) + ax3.plot(t, np.rad2deg(yaw_raw), '--') + ax3.set_xlabel("time (s)") + ax3.set_ylabel("yaw (deg)") + ax3.legend(["estimated", "integrated"]) + ax3.grid() + plt.show() + +if __name__ == '__main__': + import os + import argparse + + script_path = os.path.split(os.path.realpath(__file__))[0] + + parser = argparse.ArgumentParser( + description='Integrate angular velocity to attitude and compare it with attitude estimate') + + parser.add_argument('logfile', help='Full ulog file path, name and extension', type=str) + args = parser.parse_args() + + logfile = os.path.abspath(args.logfile) + + run(logfile) From af06bee8d0e4665a37787279d8237bad36a5526a Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 5 Aug 2024 09:58:24 +0200 Subject: [PATCH 43/90] update mavlink & adapt to pymavlink generator reporting failures by default --- src/modules/mavlink/CMakeLists.txt | 2 -- src/modules/mavlink/mavlink | 2 +- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index 3038a0f705cc..f42dd6fa9b7b 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -44,7 +44,6 @@ add_custom_command( COMMAND ${PYTHON_EXECUTABLE} ${MAVLINK_GIT_DIR}/pymavlink/tools/mavgen.py --lang C --wire-protocol 2.0 - --exit-code --output ${MAVLINK_LIBRARY_DIR} ${MAVLINK_GIT_DIR}/message_definitions/v1.0/${MAVLINK_DIALECT_UAVIONIX}.xml > ${CMAKE_CURRENT_BINARY_DIR}/mavgen_${MAVLINK_DIALECT_UAVIONIX}.log DEPENDS @@ -63,7 +62,6 @@ add_custom_command( COMMAND ${PYTHON_EXECUTABLE} ${MAVLINK_GIT_DIR}/pymavlink/tools/mavgen.py --lang C --wire-protocol 2.0 - --exit-code --output ${MAVLINK_LIBRARY_DIR} ${MAVLINK_GIT_DIR}/message_definitions/v1.0/${CONFIG_MAVLINK_DIALECT}.xml > ${CMAKE_CURRENT_BINARY_DIR}/mavgen_${CONFIG_MAVLINK_DIALECT}.log DEPENDS diff --git a/src/modules/mavlink/mavlink b/src/modules/mavlink/mavlink index d65952bacc02..bb52e30d2b92 160000 --- a/src/modules/mavlink/mavlink +++ b/src/modules/mavlink/mavlink @@ -1 +1 @@ -Subproject commit d65952bacc02c4a5a1ed8249be73e6a66fa800ab +Subproject commit bb52e30d2b924d5a250f2400144d33012271a19d From c9343ca11d1dd8670cb15b04b7775254bb107913 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 9 Aug 2024 12:04:07 -0400 Subject: [PATCH 44/90] sitl_gazebo-classic update submodule to latest --- Tools/simulation/gazebo-classic/sitl_gazebo-classic | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/simulation/gazebo-classic/sitl_gazebo-classic b/Tools/simulation/gazebo-classic/sitl_gazebo-classic index 67af3c3a6da4..f1d11a612699 160000 --- a/Tools/simulation/gazebo-classic/sitl_gazebo-classic +++ b/Tools/simulation/gazebo-classic/sitl_gazebo-classic @@ -1 +1 @@ -Subproject commit 67af3c3a6da493bdc0a0b9de28b01a2a98d38659 +Subproject commit f1d11a6126990d487d4aa8ff68c23ff370516510 From e008ca24f1ecc1c7e41e2da555c366291145695e Mon Sep 17 00:00:00 2001 From: Jaeyoung Lim Date: Mon, 12 Aug 2024 16:42:51 +0200 Subject: [PATCH 45/90] Remove euler angles from attitude setpoint (#23482) * Remove euler angles from attitude setpoint message * Remove usage of euler angles in attitude setpoint messages This commit removes the usage of euler angles in the vehicle_attitude_setpoint messages * Fix standard vtol --- msg/VehicleAttitudeSetpoint.msg | 4 - .../FixedwingAttitudeControl.cpp | 29 ++- .../FixedwingPositionControl.cpp | 227 ++++++++++++------ src/modules/mavlink/mavlink_receiver.cpp | 5 - src/modules/mavlink/streams/HIGH_LATENCY2.hpp | 4 +- .../mc_att_control/mc_att_control_main.cpp | 6 - .../PositionControl/ControlMath.cpp | 6 - .../PositionControl/ControlMathTest.cpp | 21 +- .../PositionControl/PositionControlTest.cpp | 20 +- .../RoverPositionControl.cpp | 8 +- .../uuv_att_control/uuv_att_control.cpp | 12 +- .../uuv_pos_control/uuv_pos_control.cpp | 5 +- src/modules/vtol_att_control/standard.cpp | 17 +- src/modules/vtol_att_control/tailsitter.cpp | 10 +- src/modules/vtol_att_control/tiltrotor.cpp | 11 +- src/modules/vtol_att_control/vtol_type.cpp | 6 +- 16 files changed, 234 insertions(+), 157 deletions(-) diff --git a/msg/VehicleAttitudeSetpoint.msg b/msg/VehicleAttitudeSetpoint.msg index f52025d2bb51..74a753023df5 100644 --- a/msg/VehicleAttitudeSetpoint.msg +++ b/msg/VehicleAttitudeSetpoint.msg @@ -1,9 +1,5 @@ uint64 timestamp # time since system start (microseconds) -float32 roll_body # body angle in NED frame (can be NaN for FW) -float32 pitch_body # body angle in NED frame (can be NaN for FW) -float32 yaw_body # body angle in NED frame (can be NaN for FW) - float32 yaw_sp_move_rate # rad/s (commanded by user) # For quaternion-based attitude control diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index b08fc895ab9c..988853fdeaaf 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -96,17 +96,16 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body) // STABILIZED mode generate the attitude setpoint from manual user inputs - _att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_man_r_max.get()); + const float roll_body = _manual_control_setpoint.roll * radians(_param_fw_man_r_max.get()); - _att_sp.pitch_body = -_manual_control_setpoint.pitch * radians(_param_fw_man_p_max.get()) - + radians(_param_fw_psp_off.get()); - _att_sp.pitch_body = constrain(_att_sp.pitch_body, - -radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get())); + float pitch_body = -_manual_control_setpoint.pitch * radians(_param_fw_man_p_max.get()) + + radians(_param_fw_psp_off.get()); + pitch_body = constrain(pitch_body, + -radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get())); - _att_sp.yaw_body = yaw_body; // yaw is not controlled, so set setpoint to current yaw _att_sp.thrust_body[0] = (_manual_control_setpoint.throttle + 1.f) * .5f; - Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); + const Quatf q(Eulerf(roll_body, pitch_body, yaw_body)); q.copyTo(_att_sp.q_d); _att_sp.reset_integral = false; @@ -325,16 +324,22 @@ void FixedwingAttitudeControl::Run() /* Run attitude controllers */ if (_vcontrol_mode.flag_control_attitude_enabled && _in_fw_or_transition_wo_tailsitter_transition) { - if (PX4_ISFINITE(_att_sp.roll_body) && PX4_ISFINITE(_att_sp.pitch_body)) { - _roll_ctrl.control_roll(_att_sp.roll_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), + const Eulerf setpoint(Quatf(_att_sp.q_d)); + const float roll_body = setpoint.phi(); + const float pitch_body = setpoint.theta(); + + if (PX4_ISFINITE(roll_body) && PX4_ISFINITE(pitch_body)) { + + _roll_ctrl.control_roll(roll_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), euler_angles.theta()); - _pitch_ctrl.control_pitch(_att_sp.pitch_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), + _pitch_ctrl.control_pitch(pitch_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), euler_angles.theta()); - _yaw_ctrl.control_yaw(_att_sp.roll_body, _pitch_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), + _yaw_ctrl.control_yaw(roll_body, _pitch_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), euler_angles.theta(), get_airspeed_constrained()); if (wheel_control) { - _wheel_ctrl.control_attitude(_att_sp.yaw_body, euler_angles.psi()); + Eulerf attitude_setpoint(Quatf(_att_sp.q_d)); + _wheel_ctrl.control_attitude(attitude_setpoint.psi(), euler_angles.psi()); } else { _wheel_ctrl.reset_integrator(); diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index daef50572dc9..4da7168255f9 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -451,9 +451,6 @@ FixedwingPositionControl::status_publish() { position_controller_status_s pos_ctrl_status = {}; - pos_ctrl_status.nav_roll = _att_sp.roll_body; - pos_ctrl_status.nav_pitch = _att_sp.pitch_body; - npfg_status_s npfg_status = {}; npfg_status.wind_est_valid = _wind_valid; @@ -791,8 +788,11 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now) /* reset setpoints from other modes (auto) otherwise we won't * level out without new manual input */ - _att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + float roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); + float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + const Eulerf current_setpoint(Quatf(_att_sp.q_d)); + const Quatf setpoint(Eulerf(roll_body, current_setpoint.theta(), yaw_body)); + setpoint.copyTo(_att_sp.q_d); } _control_mode_current = FW_POSCTRL_MODE_MANUAL_POSITION; @@ -877,11 +877,16 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto } switch (position_sp_type) { - case position_setpoint_s::SETPOINT_TYPE_IDLE: - _att_sp.thrust_body[0] = 0.0f; - _att_sp.roll_body = 0.0f; - _att_sp.pitch_body = radians(_param_fw_psp_off.get()); - break; + case position_setpoint_s::SETPOINT_TYPE_IDLE: { + _att_sp.thrust_body[0] = 0.0f; + const float roll_body = 0.0f; + const float pitch_body = radians(_param_fw_psp_off.get()); + const float yaw_body = 0.0f; + + const Quatf setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + setpoint.copyTo(_att_sp.q_d); + break; + } case position_setpoint_s::SETPOINT_TYPE_POSITION: control_auto_position(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp); @@ -927,9 +932,6 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto _att_sp.thrust_body[0] = (_landed) ? min(_param_fw_thr_idle.get(), 1.f) : get_tecs_thrust(); } - /* Copy thrust and pitch values from tecs */ - _att_sp.pitch_body = get_tecs_pitch(); - if (!_vehicle_status.in_transition_to_fw) { publishLocalPositionSetpoint(current_sp); } @@ -950,8 +952,8 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i _param_sinkrate_target.get(), _param_climbrate_target.get()); - _att_sp.roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle - _att_sp.yaw_body = 0.f; + const float roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle + const float yaw_body = 0.f; if (_landed) { _att_sp.thrust_body[0] = _param_fw_thr_min.get(); @@ -960,7 +962,9 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i _att_sp.thrust_body[0] = min(get_tecs_thrust(), _param_fw_thr_max.get()); } - _att_sp.pitch_body = get_tecs_pitch(); + const float pitch_body = get_tecs_pitch(); + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } @@ -983,11 +987,13 @@ FixedwingPositionControl::control_auto_descend(const float control_interval) false, descend_rate); - _att_sp.roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle - _att_sp.yaw_body = 0.f; + const float roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle + const float yaw_body = 0.f; _att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_min.get() : min(get_tecs_thrust(), _param_fw_thr_max.get()); - _att_sp.pitch_body = get_tecs_pitch(); + const float pitch_body = get_tecs_pitch(); + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } uint8_t @@ -1109,10 +1115,10 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co navigateWaypoint(curr_wp_local, curr_pos_local, ground_speed, _wind_vel); } - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + float roll_body = getCorrectedNpfgRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw tecs_update_pitch_throttle(control_interval, position_sp_alt, @@ -1123,6 +1129,9 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co tecs_fw_thr_max, _param_sinkrate_target.get(), _param_climbrate_target.get()); + const float pitch_body = get_tecs_pitch(); + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } void @@ -1158,10 +1167,10 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co _npfg.setAirspeedNom(target_airspeed * _eas2tas); _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateBearing(curr_pos_local, _target_bearing, ground_speed, _wind_vel); - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + float roll_body = getCorrectedNpfgRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.yaw_body = _yaw; + float yaw_body = _yaw; tecs_update_pitch_throttle(control_interval, position_sp_alt, @@ -1174,6 +1183,10 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co _param_climbrate_target.get(), false, pos_sp_curr.vz); + const float pitch_body = get_tecs_pitch(); + + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } void @@ -1253,10 +1266,10 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, pos_sp_curr.loiter_direction_counter_clockwise, ground_speed, _wind_vel); - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + float roll_body = getCorrectedNpfgRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw float alt_sp = pos_sp_curr.alt; @@ -1267,7 +1280,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons } else { // continue straight until vehicle has sufficient altitude - _att_sp.roll_body = 0.0f; + roll_body = 0.0f; } _tecs.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get()); @@ -1282,6 +1295,11 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons tecs_fw_thr_max, _param_sinkrate_target.get(), _param_climbrate_target.get()); + + const float pitch_body = get_tecs_pitch(); + + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } #ifdef CONFIG_FIGURE_OF_EIGHT @@ -1306,7 +1324,7 @@ FixedwingPositionControl::controlAutoFigureEight(const float control_interval, c // Apply control _figure_eight.updateSetpoint(curr_pos_local, ground_speed, params, target_airspeed); - _att_sp.roll_body = _figure_eight.getRollSetpoint(); + float roll_body = _figure_eight.getRollSetpoint(); target_airspeed = _figure_eight.getAirspeedSetpoint(); _target_bearing = _figure_eight.getTargetBearing(); _closest_point_on_path = _figure_eight.getClosestPoint(); @@ -1337,7 +1355,11 @@ FixedwingPositionControl::controlAutoFigureEight(const float control_interval, c _param_climbrate_target.get()); // Yaw - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + const float pitch_body = get_tecs_pitch(); + + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } void FixedwingPositionControl::publishFigureEightStatus(const position_setpoint_s pos_sp) @@ -1392,10 +1414,10 @@ FixedwingPositionControl::control_auto_path(const float control_interval, const 0.0f; navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), ground_speed, _wind_vel, curvature); - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + float roll_body = getCorrectedNpfgRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw tecs_update_pitch_throttle(control_interval, pos_sp_curr.alt, @@ -1408,7 +1430,10 @@ FixedwingPositionControl::control_auto_path(const float control_interval, const _param_climbrate_target.get()); _att_sp.thrust_body[0] = min(get_tecs_thrust(), tecs_fw_thr_max); - _att_sp.pitch_body = get_tecs_pitch(); + const float pitch_body = get_tecs_pitch(); + + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } void @@ -1494,7 +1519,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateLine(start_pos_local, takeoff_bearing, local_2D_position, ground_speed, _wind_vel); - _att_sp.roll_body = _runway_takeoff.getRoll(getCorrectedNpfgRollSetpoint()); + float roll_body = _runway_takeoff.getRoll(getCorrectedNpfgRollSetpoint()); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; @@ -1502,7 +1527,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo const float bearing = _npfg.getBearing(); // heading hold mode will override this bearing setpoint - _att_sp.yaw_body = _runway_takeoff.getYaw(bearing); + float yaw_body = _runway_takeoff.getYaw(bearing); // update tecs const float pitch_max = _runway_takeoff.getMaxPitch(math::radians(_param_fw_p_lim_max.get())); @@ -1529,9 +1554,16 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation - _att_sp.pitch_body = _runway_takeoff.getPitch(get_tecs_pitch()); + const float pitch_body = _runway_takeoff.getPitch(get_tecs_pitch()); _att_sp.thrust_body[0] = _runway_takeoff.getThrottle(_param_fw_thr_idle.get(), get_tecs_thrust()); + roll_body = constrainRollNearGround(roll_body, _current_altitude, _takeoff_ground_alt); + + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); + + _flaps_setpoint = _param_fw_flaps_to_scl.get(); + // retract ladning gear once passed the climbout state if (_runway_takeoff.getState() > RunwayTakeoffState::CLIMBOUT) { _new_landing_gear_position = landing_gear_s::GEAR_UP; @@ -1588,7 +1620,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _npfg.setAirspeedNom(target_airspeed * _eas2tas); _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateLine(launch_local_position, takeoff_bearing, local_2D_position, ground_speed, _wind_vel); - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + float roll_body = getCorrectedNpfgRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; @@ -1613,17 +1645,27 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _att_sp.thrust_body[0] = get_tecs_thrust(); } - _att_sp.pitch_body = get_tecs_pitch(); - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + float pitch_body = get_tecs_pitch(); + float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + + roll_body = constrainRollNearGround(roll_body, _current_altitude, _takeoff_ground_alt); + + Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } else { /* Tell the attitude controller to stop integrating while we are waiting for the launch */ _att_sp.reset_integral = true; /* Set default roll and pitch setpoints during detection phase */ - _att_sp.roll_body = 0.0f; + float roll_body = 0.0f; + float yaw_body = _yaw; _att_sp.thrust_body[0] = _param_fw_thr_idle.get(); - _att_sp.pitch_body = radians(_takeoff_pitch_min.get()); + float pitch_body = radians(_takeoff_pitch_min.get()); + roll_body = constrainRollNearGround(roll_body, _current_altitude, _takeoff_ground_alt); + Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); + } launch_detection_status_s launch_detection_status; @@ -1633,7 +1675,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo } _flaps_setpoint = _param_fw_flaps_to_scl.get(); - _att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, _takeoff_ground_alt); if (!_vehicle_status.in_transition_to_fw) { publishLocalPositionSetpoint(pos_sp_curr); @@ -1738,10 +1779,10 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateLine(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + float roll_body = getCorrectedNpfgRollSetpoint(); // use npfg's bearing to commanded course, controlled via yaw angle while on runway - _att_sp.yaw_body = _npfg.getBearing(); + float yaw_body = _npfg.getBearing(); /* longitudinal guidance */ @@ -1787,7 +1828,13 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, /* set the attitude and throttle commands */ // TECS has authority (though constrained) over pitch during flare, throttle is hard set to idle - _att_sp.pitch_body = get_tecs_pitch(); + float pitch_body = get_tecs_pitch(); + + roll_body = constrainRollNearGround(roll_body, _current_altitude, terrain_alt); + + + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); // enable direct yaw control using rudder/wheel _att_sp.fw_control_yaw_wheel = true; @@ -1817,7 +1864,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateLine(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + float roll_body = getCorrectedNpfgRollSetpoint(); /* longitudinal guidance */ @@ -1839,10 +1886,15 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, /* set the attitude and throttle commands */ - _att_sp.pitch_body = get_tecs_pitch(); + float pitch_body = get_tecs_pitch(); + + roll_body = constrainRollNearGround(roll_body, _current_altitude, terrain_alt); // yaw is not controlled in nominal flight - _att_sp.yaw_body = _yaw; + float yaw_body = _yaw; + + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); // enable direct yaw control using rudder/wheel _att_sp.fw_control_yaw_wheel = false; @@ -1852,8 +1904,6 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation - _att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, terrain_alt); - _flaps_setpoint = _param_fw_flaps_lnd_scl.get(); _spoilers_setpoint = _param_fw_spoilers_lnd.get(); @@ -1911,6 +1961,10 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, } // the terrain estimate (if enabled) is always used to determine the flaring altitude + float roll_body; + float yaw_body; + float pitch_body; + if ((_current_altitude < terrain_alt + flare_rel_alt) || _flare_states.flaring) { // flare and land with minimal speed @@ -1943,9 +1997,9 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, pos_sp_curr.loiter_direction_counter_clockwise, ground_speed, _wind_vel); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + roll_body = getCorrectedNpfgRollSetpoint(); - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw /* longitudinal guidance */ @@ -1990,7 +2044,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, /* set the attitude and throttle commands */ // TECS has authority (though constrained) over pitch during flare, throttle is hard set to idle - _att_sp.pitch_body = get_tecs_pitch(); + pitch_body = get_tecs_pitch(); // enable direct yaw control using rudder/wheel _att_sp.fw_control_yaw_wheel = true; @@ -2020,7 +2074,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, pos_sp_curr.loiter_direction_counter_clockwise, ground_speed, _wind_vel); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + roll_body = getCorrectedNpfgRollSetpoint(); /* longitudinal guidance */ @@ -2044,10 +2098,10 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, /* set the attitude and throttle commands */ - _att_sp.pitch_body = get_tecs_pitch(); + pitch_body = get_tecs_pitch(); // yaw is not controlled in nominal flight - _att_sp.yaw_body = _yaw; + yaw_body = _yaw; // enable direct yaw control using rudder/wheel _att_sp.fw_control_yaw_wheel = false; @@ -2057,7 +2111,11 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation - _att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, terrain_alt); + roll_body = constrainRollNearGround(roll_body, _current_altitude, terrain_alt); + + Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); + _flaps_setpoint = _param_fw_flaps_lnd_scl.get(); _spoilers_setpoint = _param_fw_spoilers_lnd.get(); @@ -2104,11 +2162,14 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval, false, height_rate_sp); - _att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + float roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); + float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw _att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max); - _att_sp.pitch_body = get_tecs_pitch(); + const float pitch_body = get_tecs_pitch(); + + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } void @@ -2137,6 +2198,11 @@ FixedwingPositionControl::control_manual_position(const float control_interval, _time_last_xy_reset = _local_pos.timestamp; } + Eulerf current_setpoint(Quatf(_att_sp.q_d)); + float yaw_body = current_setpoint.psi(); + float roll_body = current_setpoint.phi(); + float pitch_body = current_setpoint.theta(); + /* heading control */ // TODO: either make it course hold (easier) or a real heading hold (minus all the complexity here) if (fabsf(_manual_control_setpoint.roll) < HDG_HOLD_MAN_INPUT_THRESH && @@ -2177,10 +2243,10 @@ FixedwingPositionControl::control_manual_position(const float control_interval, _npfg.setAirspeedNom(calibrated_airspeed_sp * _eas2tas); _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateLine(_hdg_hold_position, _hdg_hold_yaw, curr_pos_local, ground_speed, _wind_vel); - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + roll_body = getCorrectedNpfgRollSetpoint(); calibrated_airspeed_sp = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw } } @@ -2202,12 +2268,16 @@ FixedwingPositionControl::control_manual_position(const float control_interval, _hdg_hold_enabled = false; _yaw_lock_engaged = false; - _att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); + yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw } _att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max); - _att_sp.pitch_body = get_tecs_pitch(); + + pitch_body = get_tecs_pitch(); + + Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } void FixedwingPositionControl::control_backtransition(const float control_interval, const Vector2f &ground_speed, @@ -2230,10 +2300,10 @@ void FixedwingPositionControl::control_backtransition(const float control_interv navigateLine(_lpos_where_backtrans_started, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + float roll_body = getCorrectedNpfgRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw tecs_update_pitch_throttle(control_interval, pos_sp_curr.alt, @@ -2246,7 +2316,10 @@ void FixedwingPositionControl::control_backtransition(const float control_interv _param_climbrate_target.get()); _att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_min.get() : min(get_tecs_thrust(), _param_fw_thr_max.get()); - _att_sp.pitch_body = get_tecs_pitch(); + const float pitch_body = get_tecs_pitch(); + + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } float FixedwingPositionControl::get_tecs_pitch() @@ -2556,12 +2629,16 @@ FixedwingPositionControl::Run() if (_control_mode_current != FW_POSCTRL_MODE_OTHER) { + Eulerf attitude_setpoint(Quatf(_att_sp.q_d)); + float roll_body = attitude_setpoint.phi(); + float pitch_body = attitude_setpoint.theta(); + float yaw_body = attitude_setpoint.psi(); if (_control_mode.flag_control_manual_enabled) { - _att_sp.roll_body = constrain(_att_sp.roll_body, -radians(_param_fw_r_lim.get()), - radians(_param_fw_r_lim.get())); - _att_sp.pitch_body = constrain(_att_sp.pitch_body, radians(_param_fw_p_lim_min.get()), - radians(_param_fw_p_lim_max.get())); + roll_body = constrain(roll_body, -radians(_param_fw_r_lim.get()), + radians(_param_fw_r_lim.get())); + pitch_body = constrain(pitch_body, radians(_param_fw_p_lim_min.get()), + radians(_param_fw_p_lim_max.get())); } if (_control_mode.flag_control_position_enabled || @@ -2571,9 +2648,9 @@ FixedwingPositionControl::Run() _control_mode.flag_control_climb_rate_enabled) { // roll slew rate - _att_sp.roll_body = _roll_slew_rate.update(_att_sp.roll_body, control_interval); + roll_body = _roll_slew_rate.update(roll_body, control_interval); - const Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); + const Quatf q(Eulerf(roll_body, pitch_body, yaw_body)); q.copyTo(_att_sp.q_d); _att_sp.timestamp = hrt_absolute_time(); @@ -3162,8 +3239,10 @@ float FixedwingPositionControl::getLoadFactor() { float load_factor_from_bank_angle = 1.0f; - if (PX4_ISFINITE(_att_sp.roll_body)) { - load_factor_from_bank_angle = 1.0f / math::max(cosf(_att_sp.roll_body), FLT_EPSILON); + float roll_body = Eulerf(Quatf(_att_sp.q_d)).phi(); + + if (PX4_ISFINITE(roll_body)) { + load_factor_from_bank_angle = 1.0f / math::max(cosf(roll_body), FLT_EPSILON); } return load_factor_from_bank_angle; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 8e03e8bd40bc..58d2c044ccd0 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1626,11 +1626,6 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) const matrix::Quatf q{attitude_target.q}; q.copyTo(attitude_setpoint.q_d); - matrix::Eulerf euler{q}; - attitude_setpoint.roll_body = euler.phi(); - attitude_setpoint.pitch_body = euler.theta(); - attitude_setpoint.yaw_body = euler.psi(); - // TODO: review use case attitude_setpoint.yaw_sp_move_rate = (type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE) ? (float)NAN : attitude_target.body_yaw_rate; diff --git a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp index 32d244b138da..6aca9f7d9837 100644 --- a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp +++ b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp @@ -264,7 +264,9 @@ class MavlinkStreamHighLatency2 : public MavlinkStream vehicle_attitude_setpoint_s attitude_sp; if (_attitude_sp_sub.update(&attitude_sp)) { - msg->target_heading = static_cast(math::degrees(matrix::wrap_2pi(attitude_sp.yaw_body)) * 0.5f); + + msg->target_heading = static_cast(math::degrees(matrix::wrap_2pi(matrix::Eulerf(matrix::Quatf( + attitude_sp.q_d)).psi())) * 0.5f); return true; } diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 39cb033e74dc..f6cc5db43d88 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -182,12 +182,6 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, q_sp.copyTo(attitude_setpoint.q_d); - // Transform to euler angles for logging only - const Eulerf euler_sp(q_sp); - attitude_setpoint.roll_body = euler_sp(0); - attitude_setpoint.pitch_body = euler_sp(1); - attitude_setpoint.yaw_body = euler_sp(2); - attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_setpoint.throttle); attitude_setpoint.timestamp = hrt_absolute_time(); diff --git a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp index b4eba96115cd..3d1b1268c2cc 100644 --- a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp +++ b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp @@ -111,12 +111,6 @@ void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpo // copy quaternion setpoint to attitude setpoint topic const Quatf q_sp{R_sp}; q_sp.copyTo(att_sp.q_d); - - // calculate euler angles, for logging only, must not be used for control - const Eulerf euler{R_sp}; - att_sp.roll_body = euler.phi(); - att_sp.pitch_body = euler.theta(); - att_sp.yaw_body = euler.psi(); } Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max) diff --git a/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp b/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp index e9b6fba12fb4..ebb23d7ed14e 100644 --- a/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp +++ b/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp @@ -108,18 +108,20 @@ TEST(ControlMathTest, ThrottleAttitudeMapping) float yaw = 0.f; vehicle_attitude_setpoint_s att{}; thrustToAttitude(thr, yaw, att); - EXPECT_FLOAT_EQ(att.roll_body, 0.f); - EXPECT_FLOAT_EQ(att.pitch_body, 0.f); - EXPECT_FLOAT_EQ(att.yaw_body, 0.f); + EXPECT_FLOAT_EQ(att.q_d[0], 1.f); + EXPECT_FLOAT_EQ(att.q_d[1], 0.f); + EXPECT_FLOAT_EQ(att.q_d[2], 0.f); + EXPECT_FLOAT_EQ(att.q_d[3], 0.f); EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f); /* expected: same as before but with 90 yaw * reason: only yaw changed */ yaw = M_PI_2_F; thrustToAttitude(thr, yaw, att); - EXPECT_FLOAT_EQ(att.roll_body, 0.f); - EXPECT_FLOAT_EQ(att.pitch_body, 0.f); - EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F); + Eulerf euler_att(Quatf(att.q_d)); + EXPECT_FLOAT_EQ(euler_att.phi(), 0.f); + EXPECT_FLOAT_EQ(euler_att.theta(), 0.f); + EXPECT_FLOAT_EQ(euler_att.psi(), M_PI_2_F); EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f); /* expected: same as before but roll 180 @@ -127,9 +129,10 @@ TEST(ControlMathTest, ThrottleAttitudeMapping) * order is: 1. roll, 2. pitch, 3. yaw */ thr = Vector3f(0.f, 0.f, 1.f); thrustToAttitude(thr, yaw, att); - EXPECT_FLOAT_EQ(att.roll_body, -M_PI_F); - EXPECT_FLOAT_EQ(att.pitch_body, 0.f); - EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F); + Eulerf euler_att2(Quatf(att.q_d)); + EXPECT_FLOAT_EQ(std::abs(euler_att2.phi()), std::abs(M_PI_F)); + EXPECT_FLOAT_EQ(euler_att2.theta(), 0.f); + EXPECT_FLOAT_EQ(euler_att2.psi(), M_PI_2_F); EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f); } diff --git a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp index 0e5a60619a44..9c1e72b1a5c8 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp @@ -56,9 +56,10 @@ TEST(PositionControlTest, EmptySetpoint) vehicle_attitude_setpoint_s attitude{}; position_control.getAttitudeSetpoint(attitude); - EXPECT_FLOAT_EQ(attitude.roll_body, 0.f); - EXPECT_FLOAT_EQ(attitude.pitch_body, 0.f); - EXPECT_FLOAT_EQ(attitude.yaw_body, 0.f); + Eulerf euler_att(Quatf(attitude.q_d)); + EXPECT_FLOAT_EQ(euler_att.phi(), 0.f); + EXPECT_FLOAT_EQ(euler_att.theta(), 0.f); + EXPECT_FLOAT_EQ(euler_att.psi(), 0.f); EXPECT_FLOAT_EQ(attitude.yaw_sp_move_rate, 0.f); EXPECT_EQ(Quatf(attitude.q_d), Quatf(1.f, 0.f, 0.f, 0.f)); EXPECT_EQ(Vector3f(attitude.thrust_body), Vector3f(0.f, 0.f, 0.f)); @@ -184,7 +185,8 @@ TEST_F(PositionControlBasicTest, PositionControlMaxThrustLimit) EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -MAXIMUM_THRUST); // Then the horizontal margin results in a tilt with the ratio of: horizontal margin / maximum thrust - EXPECT_FLOAT_EQ(_attitude.roll_body, asin((HORIZONTAL_THRUST_MARGIN / sqrt(2.f)) / MAXIMUM_THRUST)); + Eulerf euler_att(Quatf(_attitude.q_d)); + EXPECT_FLOAT_EQ(euler_att.phi(), asin((HORIZONTAL_THRUST_MARGIN / sqrt(2.f)) / MAXIMUM_THRUST)); // TODO: add this line back once attitude setpoint generation strategy does not align body yaw with heading all the time anymore // EXPECT_FLOAT_EQ(_attitude.pitch_body, -asin((HORIZONTAL_THRUST_MARGIN / sqrt(2.f)) / MAXIMUM_THRUST)); } @@ -198,9 +200,10 @@ TEST_F(PositionControlBasicTest, PositionControlMinThrustLimit) EXPECT_FLOAT_EQ(thrust.length(), 0.1f); EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -0.1f); + Eulerf euler_att(Quatf(_attitude.q_d)); - EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f); - EXPECT_FLOAT_EQ(_attitude.pitch_body, -1.f); + EXPECT_FLOAT_EQ(euler_att.phi(), 0.f); + EXPECT_FLOAT_EQ(euler_att.theta(), -1.f); } TEST_F(PositionControlBasicTest, FailsafeInput) @@ -377,6 +380,7 @@ TEST_F(PositionControlBasicTest, IntegratorWindupWithInvalidSetpoint) EXPECT_TRUE(runController()); // THEN: the integral did not wind up and produce unexpected deviation - EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f); - EXPECT_FLOAT_EQ(_attitude.pitch_body, 0.f); + Eulerf euler_att(Quatf(_attitude.q_d)); + EXPECT_FLOAT_EQ(euler_att.phi(), 0.f); + EXPECT_FLOAT_EQ(euler_att.theta(), 0.f); } diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index 24909280e99c..928ce13c6edb 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -122,8 +122,8 @@ RoverPositionControl::manual_control_setpoint_poll() if (_control_mode.flag_control_attitude_enabled) { // STABILIZED mode generate the attitude setpoint from manual user inputs - _att_sp.roll_body = 0.0; - _att_sp.pitch_body = 0.0; + float roll_body = 0.0; + float pitch_body = 0.0; /* reset yaw setpoint to current position if needed */ if (_reset_yaw_sp) { @@ -137,10 +137,10 @@ RoverPositionControl::manual_control_setpoint_poll() _manual_yaw_sp = wrap_pi(_manual_yaw_sp + _att_sp.yaw_sp_move_rate * dt); } - _att_sp.yaw_body = _manual_yaw_sp; + float yaw_body = _manual_yaw_sp; _att_sp.thrust_body[0] = _manual_control_setpoint.throttle; - Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); + const Quatf q(Eulerf(roll_body, pitch_body, yaw_body)); q.copyTo(_att_sp.q_d); _att_sp.timestamp = hrt_absolute_time(); diff --git a/src/modules/uuv_att_control/uuv_att_control.cpp b/src/modules/uuv_att_control/uuv_att_control.cpp index 58992ccb7803..52baccb8dde6 100644 --- a/src/modules/uuv_att_control/uuv_att_control.cpp +++ b/src/modules/uuv_att_control/uuv_att_control.cpp @@ -138,9 +138,10 @@ void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &attitude */ Eulerf euler_angles(matrix::Quatf(attitude.q)); - float roll_body = attitude_setpoint.roll_body; - float pitch_body = attitude_setpoint.pitch_body; - float yaw_body = attitude_setpoint.yaw_body; + const Eulerf setpoint_euler_angles(matrix::Quatf(attitude_setpoint.q_d)); + const float roll_body = setpoint_euler_angles(0); + const float pitch_body = setpoint_euler_angles(1); + const float yaw_body = setpoint_euler_angles(2); float roll_rate_desired = rates_setpoint.roll; float pitch_rate_desired = rates_setpoint.pitch; @@ -227,9 +228,8 @@ void UUVAttitudeControl::Run() _vehicle_rates_setpoint_sub.update(&_rates_setpoint); if (input_mode == 1) { // process manual data - _attitude_setpoint.roll_body = _param_direct_roll.get(); - _attitude_setpoint.pitch_body = _param_direct_pitch.get(); - _attitude_setpoint.yaw_body = _param_direct_yaw.get(); + Quatf attitude_setpoint(Eulerf(_param_direct_roll.get(), _param_direct_pitch.get(), _param_direct_yaw.get())); + attitude_setpoint.copyTo(_attitude_setpoint.q_d); _attitude_setpoint.thrust_body[0] = _param_direct_thrust.get(); _attitude_setpoint.thrust_body[1] = 0.f; _attitude_setpoint.thrust_body[2] = 0.f; diff --git a/src/modules/uuv_pos_control/uuv_pos_control.cpp b/src/modules/uuv_pos_control/uuv_pos_control.cpp index d402941b4a16..272ea56def8d 100644 --- a/src/modules/uuv_pos_control/uuv_pos_control.cpp +++ b/src/modules/uuv_pos_control/uuv_pos_control.cpp @@ -97,9 +97,8 @@ void UUVPOSControl::publish_attitude_setpoint(const float thrust_x, const float vehicle_attitude_setpoint_s vehicle_attitude_setpoint = {}; vehicle_attitude_setpoint.timestamp = hrt_absolute_time(); - vehicle_attitude_setpoint.roll_body = roll_des; - vehicle_attitude_setpoint.pitch_body = pitch_des; - vehicle_attitude_setpoint.yaw_body = yaw_des; + const Quatf attitude_setpoint(Eulerf(roll_des, pitch_des, yaw_des)); + attitude_setpoint.copyTo(vehicle_attitude_setpoint.q_d); vehicle_attitude_setpoint.thrust_body[0] = thrust_x; vehicle_attitude_setpoint.thrust_body[1] = thrust_y; diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 84606aad4327..105ee1ee6efa 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -172,6 +172,11 @@ 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) { @@ -181,7 +186,7 @@ void Standard::update_transition_state() } memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); - _v_att_sp->roll_body = _fw_virtual_att_sp->roll_body; + 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) @@ -227,21 +232,19 @@ void Standard::update_transition_state() } // ramp up FW_PSP_OFF - _v_att_sp->pitch_body = math::radians(_param_fw_psp_off.get()) * (1.0f - mc_weight); - + pitch_body = math::radians(_param_fw_psp_off.get()) * (1.0f - mc_weight); _v_att_sp->thrust_body[0] = _pusher_throttle; - - const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); + const Quatf q_sp(Eulerf(roll_body, pitch_body, yaw_body)); q_sp.copyTo(_v_att_sp->q_d); } else if (_vtol_mode == vtol_mode::TRANSITION_TO_MC) { if (_v_control_mode->flag_control_climb_rate_enabled) { // control backtransition deceleration using pitch. - _v_att_sp->pitch_body = update_and_get_backtransition_pitch_sp(); + pitch_body = update_and_get_backtransition_pitch_sp(); } - const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); + const Quatf q_sp(Eulerf(roll_body, pitch_body, yaw_body)); q_sp.copyTo(_v_att_sp->q_d); _pusher_throttle = 0.0f; diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index af95a05083c3..eaacbe68d611 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -179,7 +179,8 @@ void Tailsitter::update_transition_state() // the yaw setpoint and zero roll since we want wings level transition. // If for some reason the fw attitude setpoint is not recent then don't use it and assume 0 pitch if (_fw_virtual_att_sp->timestamp > (now - 1_s)) { - _q_trans_start = Eulerf(0.f, _fw_virtual_att_sp->pitch_body, yaw_sp); + const float pitch_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).theta(); + _q_trans_start = Eulerf(0.f, pitch_body, yaw_sp); } else { _q_trans_start = Eulerf(0.f, 0.f, yaw_sp); @@ -191,7 +192,8 @@ void Tailsitter::update_transition_state() } else if (_vtol_mode == vtol_mode::TRANSITION_FRONT_P1) { // initial attitude setpoint for the transition should be with wings level - _q_trans_start = Eulerf(0.f, _mc_virtual_att_sp->pitch_body, _mc_virtual_att_sp->yaw_body); + const Eulerf setpoint_euler(Quatf(_mc_virtual_att_sp->q_d)); + _q_trans_start = Eulerf(0.f, setpoint_euler.theta(), setpoint_euler.psi()); Vector3f x = Dcmf(Quatf(_v_att->q)) * Vector3f(1.f, 0.f, 0.f); _trans_rot_axis = -x.cross(Vector3f(0.f, 0.f, -1.f)); } @@ -238,10 +240,6 @@ void Tailsitter::update_transition_state() _v_att_sp->timestamp = hrt_absolute_time(); const Eulerf euler_sp(_q_trans_sp); - _v_att_sp->roll_body = euler_sp.phi(); - _v_att_sp->pitch_body = euler_sp.theta(); - _v_att_sp->yaw_body = euler_sp.psi(); - _q_trans_sp.copyTo(_v_att_sp->q_d); } diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 0956e1003698..3a1904257993 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -203,6 +203,11 @@ 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) { @@ -212,7 +217,7 @@ void Tiltrotor::update_transition_state() } memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); - _v_att_sp->roll_body = _fw_virtual_att_sp->roll_body; + roll_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).phi(); _thrust_transition = -_mc_virtual_att_sp->thrust_body[2]; } else { @@ -290,7 +295,7 @@ void Tiltrotor::update_transition_state() // control backtransition deceleration using pitch. if (_v_control_mode->flag_control_climb_rate_enabled) { - _v_att_sp->pitch_body = update_and_get_backtransition_pitch_sp(); + pitch_body = update_and_get_backtransition_pitch_sp(); } if (_time_since_trans_start < BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) { @@ -321,7 +326,7 @@ void Tiltrotor::update_transition_state() _v_att_sp->thrust_body[2] = -_thrust_transition; - const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); + const Quatf q_sp(Eulerf(roll_body, pitch_body, yaw_body)); q_sp.copyTo(_v_att_sp->q_d); _mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f); diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index e43dd7c912ad..458b3927fb9c 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -563,10 +563,10 @@ float VtolType::pusher_assist() tilt_new = R_yaw_correction * tilt_new; // now extract roll and pitch setpoints - _v_att_sp->pitch_body = atan2f(tilt_new(0), tilt_new(2)); - _v_att_sp->roll_body = -asinf(tilt_new(1)); + const float pitch_body = atan2f(tilt_new(0), tilt_new(2)); + const float roll_body = -asinf(tilt_new(1)); - const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, euler_sp(2))); + const Quatf q_sp(Eulerf(roll_body, pitch_body, euler_sp(2))); q_sp.copyTo(_v_att_sp->q_d); } From db8781e5311c876cb948fc2e10c0ec6d0ff96372 Mon Sep 17 00:00:00 2001 From: "murata,katsutoshi" Date: Tue, 13 Aug 2024 02:09:22 +0900 Subject: [PATCH 46/90] navigator: Align MAVLINK message level with EVENT message level (#23535) --- src/modules/navigator/geofence.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 55917331a614..ea2c30a18eee 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -648,7 +648,7 @@ Geofence::loadFromFile(const char *filename) } else { mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence: import error\t"); - events::send(events::ID("navigator_geofence_import_failed"), events::Log::Error, "Geofence: import error"); + events::send(events::ID("navigator_geofence_import_failed"), events::Log::Critical, "Geofence: import error"); } updateFence(); From afc360d637d4ce9eca67a01c1faef15c16d4ebc6 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 9 Aug 2024 14:18:39 +0200 Subject: [PATCH 47/90] FW Position control: do not invalidate airspeed from negative readings Signed-off-by: Silvan Fuhrer --- src/modules/fw_pos_control/FixedwingPositionControl.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 4da7168255f9..9d126262fb17 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -212,8 +212,7 @@ FixedwingPositionControl::airspeed_poll() _eas2tas = 1.0f; //this is the default value, taken in case of invalid airspeed if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s) - && PX4_ISFINITE(airspeed_validated.true_airspeed_m_s) - && (airspeed_validated.calibrated_airspeed_m_s > FLT_EPSILON)) { + && PX4_ISFINITE(airspeed_validated.true_airspeed_m_s)) { airspeed_valid = true; From acc0cd7e8ad915eaede3f7efaa58bbb831c12628 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 9 Aug 2024 14:22:33 +0200 Subject: [PATCH 48/90] FW Position Control: disable underspeed handling during auto takeoff Signed-off-by: Silvan Fuhrer --- src/modules/fw_pos_control/FixedwingPositionControl.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 9d126262fb17..5143a4da3899 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -1549,7 +1549,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _param_fw_thr_min.get(), _param_fw_thr_max.get(), _param_sinkrate_target.get(), - _performance_model.getMaximumClimbRate(_air_density)); + _performance_model.getMaximumClimbRate(_air_density), + true); // disable underspeed handling _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation @@ -1634,7 +1635,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _param_fw_thr_min.get(), max_takeoff_throttle, _param_sinkrate_target.get(), - _performance_model.getMaximumClimbRate(_air_density)); + _performance_model.getMaximumClimbRate(_air_density), + true); // disable underspeed handling if (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) { // explicitly set idle throttle until motors are enabled From a0d22a4d212a87980a47524b5d5633cf6e918ace Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 12 Aug 2024 10:39:29 +0200 Subject: [PATCH 49/90] FW Position Control: make explicit when underspeed detection logic is en-/disabled Signed-off-by: Silvan Fuhrer --- .../FixedwingPositionControl.cpp | 31 +++++++++++++------ 1 file changed, 22 insertions(+), 9 deletions(-) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 5143a4da3899..7858fe2c1c5f 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -973,6 +973,7 @@ FixedwingPositionControl::control_auto_descend(const float control_interval) // Hard-code descend rate to 0.5m/s. This is a compromise to give the system to recover, // but not letting it drift too far away. const float descend_rate = -0.5f; + const bool disable_underspeed_handling = false; tecs_update_pitch_throttle(control_interval, _current_altitude, @@ -983,7 +984,7 @@ FixedwingPositionControl::control_auto_descend(const float control_interval) _param_fw_thr_max.get(), _param_sinkrate_target.get(), _param_climbrate_target.get(), - false, + disable_underspeed_handling, descend_rate); const float roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle @@ -1170,6 +1171,7 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co target_airspeed = _npfg.getAirspeedRef() / _eas2tas; float yaw_body = _yaw; + const bool disable_underspeed_handling = false; tecs_update_pitch_throttle(control_interval, position_sp_alt, @@ -1180,7 +1182,7 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co tecs_fw_thr_max, _param_sinkrate_target.get(), _param_climbrate_target.get(), - false, + disable_underspeed_handling, pos_sp_curr.vz); const float pitch_body = get_tecs_pitch(); @@ -1541,6 +1543,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _tecs.resetIntegrals(); } + const bool disable_underspeed_handling = true; + tecs_update_pitch_throttle(control_interval, altitude_setpoint_amsl, target_airspeed, @@ -1550,7 +1554,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _param_fw_thr_max.get(), _param_sinkrate_target.get(), _performance_model.getMaximumClimbRate(_air_density), - true); // disable underspeed handling + disable_underspeed_handling); _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation @@ -1626,6 +1630,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo const float max_takeoff_throttle = (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) ? _param_fw_thr_idle.get() : _param_fw_thr_max.get(); + const bool disable_underspeed_handling = true; tecs_update_pitch_throttle(control_interval, altitude_setpoint_amsl, @@ -1636,7 +1641,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo max_takeoff_throttle, _param_sinkrate_target.get(), _performance_model.getMaximumClimbRate(_air_density), - true); // disable underspeed handling + disable_underspeed_handling); if (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) { // explicitly set idle throttle until motors are enabled @@ -1813,6 +1818,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, const float throttle_max = flare_ramp_interpolator_sqrt * _param_fw_thr_idle.get() + (1.0f - flare_ramp_interpolator_sqrt) * _param_fw_thr_max.get(); + const bool disable_underspeed_handling = true; tecs_update_pitch_throttle(control_interval, altitude_setpoint, @@ -1823,7 +1829,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, throttle_max, _param_sinkrate_target.get(), _param_climbrate_target.get(), - true, + disable_underspeed_handling, height_rate_setpoint); /* set the attitude and throttle commands */ @@ -2029,6 +2035,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, const float throttle_max = flare_ramp_interpolator_sqrt * _param_fw_thr_idle.get() + (1.0f - flare_ramp_interpolator_sqrt) * _param_fw_thr_max.get(); + const bool disable_underspeed_handling = true; tecs_update_pitch_throttle(control_interval, _current_altitude, // is not controlled, control descend rate @@ -2039,7 +2046,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, throttle_max, _param_sinkrate_target.get(), _param_climbrate_target.get(), - true, + disable_underspeed_handling, height_rate_setpoint); /* set the attitude and throttle commands */ @@ -2085,6 +2092,8 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, const float glide_slope_sink_rate = airspeed_land * glide_slope / sqrtf(glide_slope * glide_slope + 1.0f); const float desired_max_sinkrate = math::min(math::max(glide_slope_sink_rate, _param_sinkrate_target.get()), _param_fw_t_sink_max.get()); + const bool disable_underspeed_handling = false; + tecs_update_pitch_throttle(control_interval, _current_altitude, // is not controlled, control descend rate target_airspeed, @@ -2094,7 +2103,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, _param_fw_thr_max.get(), desired_max_sinkrate, _param_climbrate_target.get(), - false, + disable_underspeed_handling, -glide_slope_sink_rate); // heightrate = -sinkrate /* set the attitude and throttle commands */ @@ -2151,6 +2160,8 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval, throttle_max = 0.0f; } + const bool disable_underspeed_handling = false; + tecs_update_pitch_throttle(control_interval, _current_altitude, calibrated_airspeed_sp, @@ -2160,7 +2171,7 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval, throttle_max, _param_sinkrate_target.get(), _param_climbrate_target.get(), - false, + disable_underspeed_handling, height_rate_sp); float roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); @@ -2251,6 +2262,8 @@ FixedwingPositionControl::control_manual_position(const float control_interval, } } + const bool disable_underspeed_handling = false; + tecs_update_pitch_throttle(control_interval, _current_altitude, // TODO: check if this is really what we want.. or if we want to lock the altitude. calibrated_airspeed_sp, @@ -2260,7 +2273,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval, throttle_max, _param_sinkrate_target.get(), _param_climbrate_target.get(), - false, + disable_underspeed_handling, height_rate_sp); if (!_yaw_lock_engaged || fabsf(_manual_control_setpoint.roll) >= HDG_HOLD_MAN_INPUT_THRESH || From aad607e0dd1b216ec3e156d2d00fff7b870a74f5 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 13 Aug 2024 14:37:52 +0200 Subject: [PATCH 50/90] ekf2: send airspeed data to ekf backend regardless of sign On ground the airspeed can sometimes be slightly negative but the ekf should still know that airspeed data is flowing regularly --- src/modules/ekf2/EKF2.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index bceb4d3dcc23..af2eb6615f40 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -2076,14 +2076,19 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps) if (_airspeed_validated_sub.update(&airspeed_validated)) { if (PX4_ISFINITE(airspeed_validated.true_airspeed_m_s) - && PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s) - && (airspeed_validated.calibrated_airspeed_m_s > 0.f) && (airspeed_validated.selected_airspeed_index > 0) ) { + float cas2tas = 1.f; + + if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s) + && (airspeed_validated.calibrated_airspeed_m_s > FLT_EPSILON)) { + cas2tas = airspeed_validated.true_airspeed_m_s / airspeed_validated.calibrated_airspeed_m_s; + } + airspeedSample airspeed_sample { .time_us = airspeed_validated.timestamp, .true_airspeed = airspeed_validated.true_airspeed_m_s, - .eas2tas = airspeed_validated.true_airspeed_m_s / airspeed_validated.calibrated_airspeed_m_s, + .eas2tas = cas2tas, }; _ekf.setAirspeedData(airspeed_sample); } From e2c0e5c88ab68cafbee9b4fe4562882858706a63 Mon Sep 17 00:00:00 2001 From: Stefano Colli <45536733+StefanoColli@users.noreply.github.com> Date: Tue, 13 Aug 2024 22:20:28 +0200 Subject: [PATCH 51/90] MissionBase: replay the gimbal and trigger cached items only upon reaching resume waypoint (#23484) * Fix: replay the mission cached items only upon reaching resume waypoint * Refactoring Split camera mode mission items from gimbal ones so to have a finer control over the relative replays * Chore: fix formatting --------- Co-authored-by: Silvan Fuhrer --- src/modules/navigator/mission_base.cpp | 42 ++++++++++++++++++-------- src/modules/navigator/mission_base.h | 21 ++++++++++--- 2 files changed, 47 insertions(+), 16 deletions(-) diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index d41456ccce66..6a64f46044e8 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -235,6 +235,7 @@ MissionBase::on_activation() checkClimbRequired(_mission.current_seq); set_mission_items(); + _mission_activation_index = _mission.current_seq; _inactivation_index = -1; // reset // reset cruise speed @@ -293,17 +294,27 @@ MissionBase::on_active() _align_heading_necessary = false; } - // replay gimbal and camera commands immediately after resuming mission - if (haveCachedGimbalOrCameraItems()) { - replayCachedGimbalCameraItems(); + // Replay camera mode commands immediately upon mission resume + if (haveCachedCameraModeItems()) { + replayCachedCameraModeItems(); } - // replay trigger commands upon raching the resume waypoint if the trigger relay flag is set - if (cameraWasTriggering() && is_mission_item_reached_or_completed()) { - replayCachedTriggerItems(); - } - replayCachedSpeedChangeItems(); + // Replay cached mission commands once the last mission waypoint is re-reached after the mission interruption. + // Each replay function also clears the cached items afterwards + if (_mission.current_seq > _mission_activation_index) { + // replay gimbal commands + if (haveCachedGimbalItems()) { + replayCachedGimbalItems(); + } + + // replay trigger commands + if (cameraWasTriggering()) { + replayCachedTriggerItems(); + } + + replayCachedSpeedChangeItems(); + } /* lets check if we reached the current mission item */ if (_mission_type != MissionType::MISSION_TYPE_NONE && is_mission_item_reached_or_completed()) { @@ -1255,7 +1266,7 @@ void MissionBase::cacheItem(const mission_item_s &mission_item) } } -void MissionBase::replayCachedGimbalCameraItems() +void MissionBase::replayCachedGimbalItems() { if (_last_gimbal_configure_item.nav_cmd > 0) { issue_command(_last_gimbal_configure_item); @@ -1266,7 +1277,10 @@ void MissionBase::replayCachedGimbalCameraItems() issue_command(_last_gimbal_control_item); _last_gimbal_control_item = {}; // delete cached item } +} +void MissionBase::replayCachedCameraModeItems() +{ if (_last_camera_mode_item.nav_cmd > 0) { issue_command(_last_camera_mode_item); _last_camera_mode_item = {}; // delete cached item @@ -1297,11 +1311,15 @@ void MissionBase::resetItemCache() _last_camera_trigger_item = {}; } -bool MissionBase::haveCachedGimbalOrCameraItems() +bool MissionBase::haveCachedGimbalItems() { return _last_gimbal_configure_item.nav_cmd > 0 || - _last_gimbal_control_item.nav_cmd > 0 || - _last_camera_mode_item.nav_cmd > 0; + _last_gimbal_control_item.nav_cmd > 0; +} + +bool MissionBase::haveCachedCameraModeItems() +{ + return _last_camera_mode_item.nav_cmd > 0; } bool MissionBase::cameraWasTriggering() diff --git a/src/modules/navigator/mission_base.h b/src/modules/navigator/mission_base.h index 2f416f29e206..35d4d37030de 100644 --- a/src/modules/navigator/mission_base.h +++ b/src/modules/navigator/mission_base.h @@ -328,6 +328,7 @@ class MissionBase : public MissionBlock, public ModuleParams mission_s _mission; /**< Currently active mission*/ float _mission_init_climb_altitude_amsl{NAN}; /**< altitude AMSL the vehicle will climb to when mission starts */ int _inactivation_index{-1}; // index of mission item at which the mission was paused. Used to resume survey missions at previous waypoint to not lose images. + int _mission_activation_index{-1}; /**< Index of the mission item that will bring the vehicle back to a mission waypoint */ int32_t _load_mission_index{-1}; /**< Mission inted of loaded mission items in dataman cache*/ int32_t _dataman_cache_size_signed; /**< Size of the dataman cache. A negativ value indicates that previous mission items should be loaded, a positiv value the next mission items*/ @@ -398,9 +399,14 @@ class MissionBase : public MissionBlock, public ModuleParams void updateCachedItemsUpToIndex(int end_index); /** - * @brief Replay the cached gimbal and camera mode items + * @brief Replay the cached gimbal items */ - void replayCachedGimbalCameraItems(); + void replayCachedGimbalItems(); + + /** + * @brief Replay the cached camera mode items + */ + void replayCachedCameraModeItems(); /** * @brief Replay the cached trigger items @@ -415,11 +421,18 @@ class MissionBase : public MissionBlock, public ModuleParams void replayCachedSpeedChangeItems(); /** - * @brief Check if there are cached gimbal or camera mode items to be replayed + * @brief Check if there are cached gimbal items to be replayed + * + * @return true if there are cached items + */ + bool haveCachedGimbalItems(); + + /** + * @brief Check if there are cached camera mode items to be replayed * * @return true if there are cached items */ - bool haveCachedGimbalOrCameraItems(); + bool haveCachedCameraModeItems(); /** * @brief Check if the camera was triggering From fd062d00859fccce7f25de8dacae9d67c5440545 Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Tue, 13 Aug 2024 13:30:33 +0300 Subject: [PATCH 52/90] icm40609d: Clear interrupt status at FIFO reset If DRDY signal is used, the interrupt status needs to be cleared at FIFO reset in order to make DRDY go back inactive. Otherwise there won't be a falling edge interrupt at the next sample. Signed-off-by: Jukka Laitinen --- src/drivers/imu/invensense/icm40609d/ICM40609D.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp index 8dc0f4b79512..55db372d5d39 100644 --- a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp +++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp @@ -599,6 +599,9 @@ void ICM40609D::FIFOReset() // SIGNAL_PATH_RESET: FIFO flush RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH); + // Read INT_STATUS to clear + RegisterRead(Register::BANK_0::INT_STATUS); + // reset while FIFO is disabled _drdy_timestamp_sample.store(0); } From cc4d5bd2a66f5a68c8e1b5850545376449e41dd4 Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Tue, 13 Aug 2024 13:40:49 +0300 Subject: [PATCH 53/90] icm40609d: Add INTF register definition and disable I2C interface Disable I2C to make sure that the sensor doesn't switch to that by accident Signed-off-by: Jukka Laitinen --- src/drivers/imu/invensense/icm40609d/ICM40609D.hpp | 3 ++- .../icm40609d/InvenSense_ICM40609D_registers.hpp | 12 ++++++++++++ 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp index ab6fcfa1f27b..882e11a91ee8 100644 --- a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp +++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp @@ -160,10 +160,11 @@ class ICM40609D : public device::SPI, public I2CSPIDriver int32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint8_t _checked_register_bank0{0}; - static constexpr uint8_t size_register_bank0_cfg{10}; + static constexpr uint8_t size_register_bank0_cfg{11}; register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] { // Register | Set bits, Clear bits { Register::BANK_0::INT_CONFIG, INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY }, + { Register::BANK_0::INTF_CONFIG0, INTF_CONFIG0_BIT::UI_SIFS_CFG_DISABLE_I2C, 0}, { Register::BANK_0::FIFO_CONFIG, FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 0 }, { Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 0 }, { Register::BANK_0::GYRO_CONFIG0, GYRO_CONFIG0_BIT::GYRO_ODR_8kHz, Bit7 | Bit6 | Bit5 | Bit3 | Bit2 }, diff --git a/src/drivers/imu/invensense/icm40609d/InvenSense_ICM40609D_registers.hpp b/src/drivers/imu/invensense/icm40609d/InvenSense_ICM40609D_registers.hpp index 4d359526ba92..e9880ec8cdd0 100644 --- a/src/drivers/imu/invensense/icm40609d/InvenSense_ICM40609D_registers.hpp +++ b/src/drivers/imu/invensense/icm40609d/InvenSense_ICM40609D_registers.hpp @@ -82,6 +82,8 @@ enum class BANK_0 : uint8_t { SIGNAL_PATH_RESET = 0x4B, + INTF_CONFIG0 = 0x4C, + PWR_MGMT0 = 0x4E, GYRO_CONFIG0 = 0x4F, ACCEL_CONFIG0 = 0x50, @@ -132,6 +134,16 @@ enum SIGNAL_PATH_RESET_BIT : uint8_t { FIFO_FLUSH = Bit1, }; +// INTF_CONFIG0 +enum INTF_CONFIG0_BIT : uint8_t { + FIFO_HOLD_LAST_DATA_EN = Bit7, + FIFO_COUNT_REC = Bit6, + FIFO_COUNT_ENDIAN = Bit5, + SENSOR_DATA_ENDIAN = Bit4, + UI_SIFS_CFG_DISABLE_SPI = Bit1, + UI_SIFS_CFG_DISABLE_I2C = Bit1 | Bit0 +}; + // PWR_MGMT0 enum PWR_MGMT0_BIT : uint8_t { GYRO_MODE_LOW_NOISE = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode From 0459481cb4e4e22912fa9d29bea2a1385bf05ea6 Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Mon, 12 Aug 2024 16:08:10 +0300 Subject: [PATCH 54/90] icm40609d: Change FIFO count to samples instead of bytes As the sensor can directly report the amount of samples in the fifo, use it to simplify the logic. Also combine the fifo empty/fifo overflow checks for interrupt and polling modes. Signed-off-by: Jukka Laitinen --- .../imu/invensense/icm40609d/ICM40609D.cpp | 89 ++++++------------- .../imu/invensense/icm40609d/ICM40609D.hpp | 2 +- 2 files changed, 29 insertions(+), 62 deletions(-) diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp index 55db372d5d39..28fce0691ce9 100644 --- a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp +++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp @@ -203,57 +203,45 @@ void ICM40609D::RunImpl() case STATE::FIFO_READ: { hrt_abstime timestamp_sample = now; - uint8_t samples = 0; + uint8_t samples = FIFOReadCount(); - if (_data_ready_interrupt_enabled) { - // scheduled from interrupt if _drdy_timestamp_sample was set as expected - const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); - - if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) { - timestamp_sample = drdy_timestamp_sample; - samples = _fifo_gyro_samples; + if (samples == 0) { + perf_count(_fifo_empty_perf); - } else { - perf_count(_drdy_missed_perf); + } else { + // tolerate minor jitter, leave sample to next iteration if behind by only 1 + if (samples == _fifo_gyro_samples + 1) { + timestamp_sample -= static_cast(FIFO_SAMPLE_DT); + samples--; } - // push backup schedule back - ScheduleDelayed(_fifo_empty_interval_us * 2); - } - - if (samples == 0) { - // check current FIFO count - const uint16_t fifo_count = FIFOReadCount(); - - if (fifo_count >= FIFO::SIZE) { + if (samples > FIFO_MAX_SAMPLES) { + // not technically an overflow, but more samples than we expected or can publish FIFOReset(); perf_count(_fifo_overflow_perf); + samples = 0; + } + } - } else if (fifo_count == 0) { - perf_count(_fifo_empty_perf); + bool success = false; - } else { - // FIFO count (size in bytes) - samples = (fifo_count / sizeof(FIFO::DATA)); + if (samples > 0) { + if (_data_ready_interrupt_enabled) { + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); - // tolerate minor jitter, leave sample to next iteration if behind by only 1 - if (samples == _fifo_gyro_samples + 1) { - timestamp_sample -= static_cast(FIFO_SAMPLE_DT); - samples--; - } + if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) { + timestamp_sample = drdy_timestamp_sample; + samples = _fifo_gyro_samples; - if (samples > FIFO_MAX_SAMPLES) { - // not technically an overflow, but more samples than we expected or can publish - FIFOReset(); - perf_count(_fifo_overflow_perf); - samples = 0; + } else { + perf_count(_drdy_missed_perf); } - } - } - bool success = false; + // push backup schedule back + ScheduleDelayed(_fifo_empty_interval_us * 2); + } - if (samples >= 1) { if (FIFORead(timestamp_sample, samples)) { success = true; @@ -374,17 +362,11 @@ void ICM40609D::ConfigureSampleRate(int sample_rate) void ICM40609D::ConfigureFIFOWatermark(uint8_t samples) { - // FIFO watermark threshold in number of bytes - const uint16_t fifo_watermark_threshold = samples * sizeof(FIFO::DATA); - for (auto &r : _register_bank0_cfg) { if (r.reg == Register::BANK_0::FIFO_CONFIG2) { // FIFO_WM[7:0] FIFO_CONFIG2 - r.set_bits = fifo_watermark_threshold & 0xFF; + r.set_bits = samples & 0xFF; - } else if (r.reg == Register::BANK_0::FIFO_CONFIG3) { - // FIFO_WM[11:8] FIFO_CONFIG3 - r.set_bits = (fifo_watermark_threshold >> 8) & 0x0F; } } } @@ -537,25 +519,10 @@ bool ICM40609D::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples) return false; } - const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL); - - if (fifo_count_bytes >= FIFO::SIZE) { - perf_count(_fifo_overflow_perf); - FIFOReset(); - return false; - } - - const uint8_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA); - - if (fifo_count_samples == 0) { - perf_count(_fifo_empty_perf); - return false; - } - // check FIFO header in every sample uint8_t valid_samples = 0; - for (int i = 0; i < math::min(samples, fifo_count_samples); i++) { + for (int i = 0; i < samples; i++) { bool valid = true; // With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp index 882e11a91ee8..8dee6c2fb793 100644 --- a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp +++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp @@ -164,7 +164,7 @@ class ICM40609D : public device::SPI, public I2CSPIDriver register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] { // Register | Set bits, Clear bits { Register::BANK_0::INT_CONFIG, INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY }, - { Register::BANK_0::INTF_CONFIG0, INTF_CONFIG0_BIT::UI_SIFS_CFG_DISABLE_I2C, 0}, + { Register::BANK_0::INTF_CONFIG0, INTF_CONFIG0_BIT::FIFO_COUNT_REC | INTF_CONFIG0_BIT::UI_SIFS_CFG_DISABLE_I2C, 0}, { Register::BANK_0::FIFO_CONFIG, FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 0 }, { Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 0 }, { Register::BANK_0::GYRO_CONFIG0, GYRO_CONFIG0_BIT::GYRO_ODR_8kHz, Bit7 | Bit6 | Bit5 | Bit3 | Bit2 }, From a327b14cefbbe1f61b56a2e1ec3b71894e920f19 Mon Sep 17 00:00:00 2001 From: "murata,katsutoshi" Date: Wed, 14 Aug 2024 10:33:36 +0900 Subject: [PATCH 55/90] navigator: always fully initialize geofence msg --- src/modules/navigator/geofence.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index ea2c30a18eee..2c317bb79d9b 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -104,7 +104,7 @@ void Geofence::run() _initiate_fence_updated = false; _dataman_state = DatamanState::Read; - geofence_status_s status; + geofence_status_s status{}; status.timestamp = hrt_absolute_time(); status.geofence_id = _opaque_id; status.status = geofence_status_s::GF_STATUS_LOADING; @@ -159,7 +159,7 @@ void Geofence::run() _dataman_state = DatamanState::UpdateRequestWait; _fence_updated = true; - geofence_status_s status; + geofence_status_s status{}; status.timestamp = hrt_absolute_time(); status.geofence_id = _opaque_id; status.status = geofence_status_s::GF_STATUS_READY; @@ -179,7 +179,7 @@ void Geofence::run() _updateFence(); _fence_updated = true; - geofence_status_s status; + geofence_status_s status{}; status.timestamp = hrt_absolute_time(); status.geofence_id = _opaque_id; status.status = geofence_status_s::GF_STATUS_READY; From 5121358e879d9a61bb9e4565206e2705be2e650e Mon Sep 17 00:00:00 2001 From: mirusu400 Date: Tue, 13 Aug 2024 21:34:39 -0400 Subject: [PATCH 56/90] Makefile: Fix error message when cannot find target board not $(MAKE) help|list_config_targets, we should use $(MAKE) list_config_targets for the desired results. --- Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile b/Makefile index fe8580402dab..7d4283a95690 100644 --- a/Makefile +++ b/Makefile @@ -549,7 +549,7 @@ distclean: # All other targets are handled by PX4_MAKE. Add a rule here to avoid printing an error. %: $(if $(filter $(FIRST_ARG),$@), \ - $(error "Make target $@ not found. It either does not exist or $@ cannot be the first argument. Use '$(MAKE) help|list_config_targets' to get a list of all possible [configuration] targets."),@#) + $(error "Make target $@ not found. It either does not exist or $@ cannot be the first argument. Use '$(MAKE) list_config_targets' to get a list of all possible [configuration] targets."),@#) # Print a list of non-config targets (based on http://stackoverflow.com/a/26339924/1487069) help: From f3a8d05f8c2ad79c8ea22ea70328aebddc3371b7 Mon Sep 17 00:00:00 2001 From: Hamish Willee Date: Wed, 14 Aug 2024 12:18:14 +1000 Subject: [PATCH 57/90] MPC_ACC_DECOUPLE - better description (#23518) --- .../mc_pos_control/multicopter_position_control_limits_params.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/mc_pos_control/multicopter_position_control_limits_params.c b/src/modules/mc_pos_control/multicopter_position_control_limits_params.c index 91039f04ea6d..8307b82d7259 100644 --- a/src/modules/mc_pos_control/multicopter_position_control_limits_params.c +++ b/src/modules/mc_pos_control/multicopter_position_control_limits_params.c @@ -143,6 +143,8 @@ PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.f); * Acceleration to tilt coupling * * Set to decouple tilt from vertical acceleration. + * This provides smoother flight but slightly worse tracking in position and auto modes. + * Unset if accurate position tracking during dynamic maneuvers is more important than a smooth flight. * * @boolean * @group Multicopter Position Control From dec550dcb92039d5fc377c989d06a7d3f2bd7f3b Mon Sep 17 00:00:00 2001 From: "murata,katsutoshi" Date: Wed, 14 Aug 2024 16:40:36 +0900 Subject: [PATCH 58/90] navigator: Change IF statement to SWITCH statement (#23534) --- src/modules/navigator/geofence.cpp | 62 +++++++++++++++++++++--------- 1 file changed, 44 insertions(+), 18 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 2c317bb79d9b..00a1e3f94011 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -406,17 +406,25 @@ bool Geofence::checkPointAgainstPolygonCircle(const PolygonInfo &polygon, double { bool checksPass = true; - if (polygon.fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION) { + switch (polygon.fence_type) { + case NAV_CMD_FENCE_CIRCLE_INCLUSION: checksPass &= insideCircle(polygon, lat, lon, altitude); + break; - } else if (polygon.fence_type == NAV_CMD_FENCE_CIRCLE_EXCLUSION) { + case NAV_CMD_FENCE_CIRCLE_EXCLUSION: checksPass &= !insideCircle(polygon, lat, lon, altitude); + break; - } else if (polygon.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION) { + case NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION: checksPass &= insidePolygon(polygon, lat, lon, altitude); + break; - } else if (polygon.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION) { + case NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION: checksPass &= !insidePolygon(polygon, lat, lon, altitude); + break; + + default: // unknown fence type + break; } return checksPass; @@ -452,12 +460,18 @@ bool Geofence::insidePolygon(const PolygonInfo &polygon, double lat, double lon, break; } - if (temp_vertex_i.frame != NAV_FRAME_GLOBAL && temp_vertex_i.frame != NAV_FRAME_GLOBAL_INT - && temp_vertex_i.frame != NAV_FRAME_GLOBAL_RELATIVE_ALT - && temp_vertex_i.frame != NAV_FRAME_GLOBAL_RELATIVE_ALT_INT) { + switch (temp_vertex_i.frame) { + case NAV_FRAME_GLOBAL: + case NAV_FRAME_GLOBAL_INT: + case NAV_FRAME_GLOBAL_RELATIVE_ALT: + case NAV_FRAME_GLOBAL_RELATIVE_ALT_INT: + break; + + default: // TODO: handle different frames PX4_ERR("Frame type %i not supported", (int)temp_vertex_i.frame); - break; + return c; + } if (((double)temp_vertex_i.lon >= lon) != ((double)temp_vertex_j.lon >= lon) && @@ -482,12 +496,18 @@ bool Geofence::insideCircle(const PolygonInfo &polygon, double lat, double lon, return false; } - if (circle_point.frame != NAV_FRAME_GLOBAL && circle_point.frame != NAV_FRAME_GLOBAL_INT - && circle_point.frame != NAV_FRAME_GLOBAL_RELATIVE_ALT - && circle_point.frame != NAV_FRAME_GLOBAL_RELATIVE_ALT_INT) { + switch (circle_point.frame) { + case NAV_FRAME_GLOBAL: + case NAV_FRAME_GLOBAL_INT: + case NAV_FRAME_GLOBAL_RELATIVE_ALT: + case NAV_FRAME_GLOBAL_RELATIVE_ALT_INT: + break; + + default: // TODO: handle different frames PX4_ERR("Frame type %i not supported", (int)circle_point.frame); return false; + } if (!_projection_reference.isInitialized()) { @@ -675,20 +695,26 @@ void Geofence::printStatus() for (int i = 0; i < _num_polygons; ++i) { total_num_vertices += _polygons[i].vertex_count; - if (_polygons[i].fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION) { + switch (_polygons[i].fence_type) { + case NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION: ++num_inclusion_polygons; - } + break; - if (_polygons[i].fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION) { + case NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION: ++num_exclusion_polygons; - } + break; - if (_polygons[i].fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION) { + case NAV_CMD_FENCE_CIRCLE_INCLUSION: ++num_inclusion_circles; - } + break; - if (_polygons[i].fence_type == NAV_CMD_FENCE_CIRCLE_EXCLUSION) { + case NAV_CMD_FENCE_CIRCLE_EXCLUSION: ++num_exclusion_circles; + break; + + default: // unknown fence type + break; + } } From 1fa878ad88d80e084f0b332b76677321376ff0ff Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 9 Aug 2024 11:15:15 +0200 Subject: [PATCH 59/90] navigator: add navigation state ID to every mode class --- src/modules/navigator/land.cpp | 4 ++-- src/modules/navigator/loiter.cpp | 2 +- src/modules/navigator/mission.cpp | 2 +- src/modules/navigator/mission_base.cpp | 6 +++--- src/modules/navigator/mission_base.h | 2 +- src/modules/navigator/mission_block.cpp | 4 ++-- src/modules/navigator/mission_block.h | 2 +- src/modules/navigator/navigator_mode.cpp | 5 +++-- src/modules/navigator/navigator_mode.h | 7 ++++++- src/modules/navigator/precland.cpp | 2 +- src/modules/navigator/rtl.cpp | 2 +- src/modules/navigator/rtl_base.h | 2 +- src/modules/navigator/rtl_direct.cpp | 4 ++-- src/modules/navigator/takeoff.cpp | 4 ++-- src/modules/navigator/vtol_takeoff.cpp | 4 ++-- 15 files changed, 29 insertions(+), 23 deletions(-) diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index b6a951d1b766..74f54fbb079a 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -42,7 +42,7 @@ #include "navigator.h" Land::Land(Navigator *navigator) : - MissionBlock(navigator) + MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) { } @@ -93,7 +93,7 @@ Land::on_active() if (_navigator->get_land_detected()->landed) { _navigator->get_mission_result()->finished = true; _navigator->set_mission_result_updated(); - _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND); + _navigator->mode_completed(getNavigatorStateId()); set_idle_item(&_mission_item); struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index 15408baaff05..db61030528c1 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -43,7 +43,7 @@ #include "navigator.h" Loiter::Loiter(Navigator *navigator) : - MissionBlock(navigator), + MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER), ModuleParams(navigator) { } diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 2949c88d62b3..ea7dae675217 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -65,7 +65,7 @@ using namespace time_literals; static constexpr int32_t DEFAULT_MISSION_CACHE_SIZE = 10; Mission::Mission(Navigator *navigator) : - MissionBase(navigator, DEFAULT_MISSION_CACHE_SIZE) + MissionBase(navigator, DEFAULT_MISSION_CACHE_SIZE, vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) { } diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index 6a64f46044e8..8963a0685d08 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -46,8 +46,8 @@ #include "mission_feasibility_checker.h" #include "navigator.h" -MissionBase::MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed) : - MissionBlock(navigator), +MissionBase::MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed, uint8_t navigator_state_id) : + MissionBlock(navigator, navigator_state_id), ModuleParams(navigator), _dataman_cache_size_signed(dataman_cache_size_signed) { @@ -476,7 +476,7 @@ MissionBase::set_mission_items() if (set_end_of_mission) { setEndOfMissionItems(); - _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION); + _navigator->mode_completed(getNavigatorStateId()); } } diff --git a/src/modules/navigator/mission_base.h b/src/modules/navigator/mission_base.h index 35d4d37030de..f819b5c0e51a 100644 --- a/src/modules/navigator/mission_base.h +++ b/src/modules/navigator/mission_base.h @@ -65,7 +65,7 @@ class Navigator; class MissionBase : public MissionBlock, public ModuleParams { public: - MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed); + MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed, uint8_t navigator_state_id); ~MissionBase() override = default; virtual void on_inactive() override; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 1f0c9f0dd021..f1f202231853 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -55,8 +55,8 @@ using matrix::wrap_pi; -MissionBlock::MissionBlock(Navigator *navigator) : - NavigatorMode(navigator) +MissionBlock::MissionBlock(Navigator *navigator, uint8_t navigator_state_id) : + NavigatorMode(navigator, navigator_state_id) { } diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 13815b860211..edc1b0e843f5 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -64,7 +64,7 @@ class MissionBlock : public NavigatorMode /** * Constructor */ - MissionBlock(Navigator *navigator); + MissionBlock(Navigator *navigator, uint8_t navigator_state_id); virtual ~MissionBlock() = default; MissionBlock(const MissionBlock &) = delete; diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index b0ced9006c56..392ca6abb458 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -42,8 +42,9 @@ #include "navigator_mode.h" #include "navigator.h" -NavigatorMode::NavigatorMode(Navigator *navigator) : - _navigator(navigator) +NavigatorMode::NavigatorMode(Navigator *navigator, uint8_t navigator_state_id) : + _navigator(navigator), + _navigator_state_id(navigator_state_id) { /* set initial mission items */ on_inactivation(); diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h index 60abdcd165e0..ca418e5b2d9a 100644 --- a/src/modules/navigator/navigator_mode.h +++ b/src/modules/navigator/navigator_mode.h @@ -41,12 +41,14 @@ #pragma once +#include + class Navigator; class NavigatorMode { public: - NavigatorMode(Navigator *navigator); + NavigatorMode(Navigator *navigator, uint8_t navigator_state_id); virtual ~NavigatorMode() = default; NavigatorMode(const NavigatorMode &) = delete; NavigatorMode &operator=(const NavigatorMode &) = delete; @@ -56,6 +58,8 @@ class NavigatorMode bool isActive() {return _active;}; + uint8_t getNavigatorStateId() const { return _navigator_state_id; } + /** * This function is called while the mode is inactive */ @@ -81,4 +85,5 @@ class NavigatorMode private: bool _active{false}; + uint8_t _navigator_state_id{0}; }; diff --git a/src/modules/navigator/precland.cpp b/src/modules/navigator/precland.cpp index 080491d37ca7..74c81569cf3d 100644 --- a/src/modules/navigator/precland.cpp +++ b/src/modules/navigator/precland.cpp @@ -62,7 +62,7 @@ static constexpr const char *LOST_TARGET_ERROR_MESSAGE = "Lost landing target while landing"; PrecLand::PrecLand(Navigator *navigator) : - MissionBlock(navigator), + MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND), ModuleParams(navigator) { _handle_param_acceleration_hor = param_find("MPC_ACC_HOR"); diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 2b6bd55b3f91..6fef45f9df6e 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -55,7 +55,7 @@ static constexpr float MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES{10.0f}; // [m] We static constexpr float MIN_DIST_THRESHOLD = 2.f; RTL::RTL(Navigator *navigator) : - NavigatorMode(navigator), + NavigatorMode(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_RTL), ModuleParams(navigator), _rtl_direct(navigator) { diff --git a/src/modules/navigator/rtl_base.h b/src/modules/navigator/rtl_base.h index dc698687fce6..a4c88e06b026 100644 --- a/src/modules/navigator/rtl_base.h +++ b/src/modules/navigator/rtl_base.h @@ -46,7 +46,7 @@ class RtlBase : public MissionBase { public: RtlBase(Navigator *navigator, int32_t dataman_cache_size_signed): - MissionBase(navigator, dataman_cache_size_signed) {}; + MissionBase(navigator, dataman_cache_size_signed, vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {}; virtual ~RtlBase() = default; virtual rtl_time_estimate_s calc_rtl_time_estimate() = 0; diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index 62a940f6c0cf..145b99d0d1db 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -51,7 +51,7 @@ using namespace math; RtlDirect::RtlDirect(Navigator *navigator) : - MissionBlock(navigator), + MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_RTL), ModuleParams(navigator) { _destination.lat = static_cast(NAN); @@ -318,7 +318,7 @@ void RtlDirect::set_rtl_item() case RTLState::IDLE: { set_idle_item(&_mission_item); - _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL); + _navigator->mode_completed(getNavigatorStateId()); break; } diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index c9e59fec1e20..d9b22720f34a 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -43,7 +43,7 @@ #include Takeoff::Takeoff(Navigator *navigator) : - MissionBlock(navigator) + MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF) { } @@ -68,7 +68,7 @@ Takeoff::on_active() } else if (is_mission_item_reached_or_completed() && !_navigator->get_mission_result()->finished) { _navigator->get_mission_result()->finished = true; _navigator->set_mission_result_updated(); - _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF); + _navigator->mode_completed(getNavigatorStateId()); position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); diff --git a/src/modules/navigator/vtol_takeoff.cpp b/src/modules/navigator/vtol_takeoff.cpp index 7b36201856d3..f9a552ef58fc 100644 --- a/src/modules/navigator/vtol_takeoff.cpp +++ b/src/modules/navigator/vtol_takeoff.cpp @@ -43,7 +43,7 @@ using matrix::wrap_pi; VtolTakeoff::VtolTakeoff(Navigator *navigator) : - MissionBlock(navigator), + MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF), ModuleParams(navigator) { } @@ -151,7 +151,7 @@ VtolTakeoff::on_active() // the VTOL takeoff is done _navigator->get_mission_result()->finished = true; _navigator->set_mission_result_updated(); - _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF); + _navigator->mode_completed(getNavigatorStateId()); break; } From 9f69e9ee6ca3aaf5091866b202671311885fc10e Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 9 Aug 2024 11:19:08 +0200 Subject: [PATCH 60/90] navigator: publish navigator_state feedback to commander --- msg/CMakeLists.txt | 1 + msg/NavigatorStatus.msg | 6 ++++ src/modules/logger/logged_topics.cpp | 1 + src/modules/navigator/navigator.h | 14 ++++++++- src/modules/navigator/navigator_main.cpp | 37 ++++++++++++++++++++++++ src/modules/navigator/rtl_direct.cpp | 10 +++++++ 6 files changed, 68 insertions(+), 1 deletion(-) create mode 100644 msg/NavigatorStatus.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index f0f6d520c701..3b31b7de554d 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -146,6 +146,7 @@ set(msg_files MountOrientation.msg ModeCompleted.msg NavigatorMissionItem.msg + NavigatorStatus.msg NormalizedUnsignedSetpoint.msg NpfgStatus.msg ObstacleDistance.msg diff --git a/msg/NavigatorStatus.msg b/msg/NavigatorStatus.msg new file mode 100644 index 000000000000..21517d61e7bb --- /dev/null +++ b/msg/NavigatorStatus.msg @@ -0,0 +1,6 @@ +# Current status of a Navigator mode +# The possible values of nav_state are defined in the VehicleStatus msg. +uint64 timestamp # time since system start (microseconds) + +uint8 nav_state # Source mode (values in VehicleStatus) +bool failure # True when the current mode cannot continue diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 24b5004ee9af..5be39234e34f 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -91,6 +91,7 @@ void LoggedTopics::add_default_topics() add_topic("manual_control_switches"); add_topic("mission_result"); add_topic("navigator_mission_item"); + add_topic("navigator_status"); add_topic("npfg_status", 100); add_topic("offboard_control_mode", 100); add_topic("onboard_computer_status", 10); diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 45c4f158b7c4..70068a9c515b 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -69,6 +69,7 @@ #include #include #include +#include #include #include #include @@ -283,8 +284,12 @@ class Navigator : public ModuleBase, public ModuleParams void mode_completed(uint8_t nav_state, uint8_t result = mode_completed_s::RESULT_SUCCESS); + void set_failsafe_status(uint8_t nav_state, bool failsafe); + void sendWarningDescentStoppedDueToTerrain(); + void trigger_failsafe(uint8_t nav_state); + private: int _local_pos_sub{-1}; @@ -305,6 +310,7 @@ class Navigator : public ModuleBase, public ModuleParams uORB::Publication _geofence_result_pub{ORB_ID(geofence_result)}; uORB::Publication _mission_result_pub{ORB_ID(mission_result)}; + uORB::Publication _navigator_status_pub{ORB_ID(navigator_status)}; uORB::Publication _pos_sp_triplet_pub{ORB_ID(position_setpoint_triplet)}; uORB::Publication _vehicle_cmd_ack_pub{ORB_ID(vehicle_command_ack)}; uORB::Publication _vehicle_cmd_pub{ORB_ID(vehicle_command)}; @@ -324,6 +330,7 @@ class Navigator : public ModuleBase, public ModuleParams // Publications geofence_result_s _geofence_result{}; + navigator_status_s _navigator_status{}; position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of position setpoints */ position_setpoint_triplet_s _reposition_triplet{}; /**< triplet for non-mission direct position command */ position_setpoint_triplet_s _takeoff_triplet{}; /**< triplet for non-mission direct takeoff command */ @@ -333,7 +340,10 @@ class Navigator : public ModuleBase, public ModuleParams Geofence _geofence; /**< class that handles the geofence */ GeofenceBreachAvoidance _gf_breach_avoidance; - hrt_abstime _last_geofence_check = 0; + hrt_abstime _last_geofence_check{0}; + + bool _navigator_status_updated{false}; + hrt_abstime _last_navigator_status_publication{0}; hrt_abstime _wait_for_vehicle_status_timestamp{0}; /**< If non-zero, wait for vehicle_status update before processing next cmd */ @@ -386,6 +396,8 @@ class Navigator : public ModuleBase, public ModuleParams */ void publish_mission_result(); + void publish_navigator_status(); + void publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result); bool geofence_allows_position(const vehicle_global_position_s &pos); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index f2694b595ab7..a00ca0e79c49 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -41,6 +41,7 @@ * @author Julian Oes * @author Anton Babushkin * @author Thomas Gubler + * and many more... */ #include "navigator.h" @@ -897,6 +898,8 @@ void Navigator::run() publish_mission_result(); } + publish_navigator_status(); + _geofence.run(); perf_end(_loop_perf); @@ -1355,6 +1358,40 @@ void Navigator::set_mission_failure_heading_timeout() } } +void Navigator::trigger_failsafe(const uint8_t nav_state) +{ + if (!_navigator_status.failure || _navigator_status.nav_state != nav_state) { + _navigator_status.failure = true; + _navigator_status.nav_state = nav_state; + + _navigator_status_updated = true; + } +} + +void Navigator::publish_navigator_status() +{ + uint8_t current_nav_state = _vstatus.nav_state; + + if (_navigation_mode != nullptr) { + current_nav_state = _navigation_mode->getNavigatorStateId(); + } + + if (_navigator_status.nav_state != current_nav_state) { + _navigator_status.nav_state = current_nav_state; + _navigator_status.failure = false; + _navigator_status_updated = true; + } + + if (_navigator_status_updated + || (hrt_elapsed_time(&_last_navigator_status_publication) > 500_ms)) { + _navigator_status.timestamp = hrt_absolute_time(); + _navigator_status_pub.publish(_navigator_status); + + _navigator_status_updated = false; + _last_navigator_status_publication = hrt_absolute_time(); + } +} + void Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd) { vcmd->timestamp = hrt_absolute_time(); diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index 145b99d0d1db..216a94c858b4 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -158,6 +158,16 @@ void RtlDirect::set_rtl_item() { position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + if (_global_pos_sub.get().terrain_alt_valid + && ((_rtl_alt - _global_pos_sub.get().terrain_alt) > _navigator->get_local_position()->hagl_max)) { + // Handle case where the RTL altidude is above the maximum HAGL and land in place instead of RTL + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return alt higher than max HAGL, landing\t"); + events::send(events::ID("rtl_fail_max_hagl"), events::Log::Warning, "RTL: return alt higher than max HAGL, landing"); + + _navigator->trigger_failsafe(getNavigatorStateId()); + _rtl_state = RTLState::IDLE; + } + const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, _global_pos_sub.get().lat, _global_pos_sub.get().lon); const float loiter_altitude = math::min(_land_approach.height_m, _rtl_alt); From 25fcb3c91338d62c3c2cfeae2851c9f768c98ff2 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 9 Aug 2024 14:12:23 +0200 Subject: [PATCH 61/90] comander: trigger failsafe when navigator reports failure --- msg/FailsafeFlags.msg | 1 + .../HealthAndArmingChecks/CMakeLists.txt | 1 + .../HealthAndArmingChecks.hpp | 3 ++ .../checks/navigatorCheck.cpp | 47 +++++++++++++++++ .../checks/navigatorCheck.hpp | 51 +++++++++++++++++++ src/modules/commander/failsafe/failsafe.cpp | 10 ++++ src/modules/navigator/rtl_direct.cpp | 4 +- 7 files changed, 115 insertions(+), 2 deletions(-) create mode 100644 src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp create mode 100644 src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.hpp diff --git a/msg/FailsafeFlags.msg b/msg/FailsafeFlags.msg index 2cd31bf83598..de514fb2db4b 100644 --- a/msg/FailsafeFlags.msg +++ b/msg/FailsafeFlags.msg @@ -49,6 +49,7 @@ bool vtol_fixed_wing_system_failure # vehicle in fixed-wing system failure fai bool wind_limit_exceeded # Wind limit exceeded bool flight_time_limit_exceeded # Maximum flight time exceeded bool local_position_accuracy_low # Local position estimate has dropped below threshold, but is currently still declared valid +bool navigator_failure # Navigator failed to execute a mode # Failure detector bool fd_critical_failure # Critical failure (attitude/altitude limit exceeded, or external ATS) diff --git a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt index 7c78c47c8534..7265e042b4c0 100644 --- a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt +++ b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt @@ -55,6 +55,7 @@ px4_add_library(health_and_arming_checks checks/manualControlCheck.cpp checks/missionCheck.cpp checks/modeCheck.cpp + checks/navigatorCheck.cpp checks/offboardCheck.cpp checks/openDroneIDCheck.cpp checks/parachuteCheck.cpp diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp index af0fd9aec5f0..fdf38f5d5d77 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp @@ -49,6 +49,7 @@ #include "checks/escCheck.hpp" #include "checks/estimatorCheck.hpp" #include "checks/failureDetectorCheck.hpp" +#include "checks/navigatorCheck.hpp" #include "checks/gyroCheck.hpp" #include "checks/imuConsistencyCheck.hpp" #include "checks/loggerCheck.hpp" @@ -129,6 +130,7 @@ class HealthAndArmingChecks : public ModuleParams EscChecks _esc_checks; EstimatorChecks _estimator_checks; FailureDetectorChecks _failure_detector_checks; + NavigatorChecks _navigator_checks; GyroChecks _gyro_checks; ImuConsistencyChecks _imu_consistency_checks; LoggerChecks _logger_checks; @@ -167,6 +169,7 @@ class HealthAndArmingChecks : public ModuleParams &_esc_checks, &_estimator_checks, &_failure_detector_checks, + &_navigator_checks, &_gyro_checks, &_imu_consistency_checks, &_logger_checks, diff --git a/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp new file mode 100644 index 000000000000..8c7d0e314af4 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * 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 "navigatorCheck.hpp" + +void NavigatorChecks::checkAndReport(const Context &context, Report &reporter) +{ + navigator_status_s status; + + if (!_navigator_status_sub.copy(&status)) { + status = {}; + } + + if (context.status().nav_state == status.nav_state) { + reporter.failsafeFlags().navigator_failure = status.failure; + } +} diff --git a/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.hpp new file mode 100644 index 000000000000..48c6965deaf8 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.hpp @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * 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 + +#include "../Common.hpp" +#include +#include + + +class NavigatorChecks : public HealthAndArmingCheckBase +{ +public: + NavigatorChecks() = default; + ~NavigatorChecks() = default; + + void checkAndReport(const Context &context, Report &reporter) override; + +private: + uORB::Subscription _navigator_status_sub{ORB_ID(navigator_status)}; +}; diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index fef6e2136bf3..11285af40352 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -472,6 +472,16 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, CHECK_FAILSAFE(status_flags, local_position_accuracy_low, ActionOptions(Action::RTL)); } + if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF || + state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { + CHECK_FAILSAFE(status_flags, navigator_failure, + ActionOptions(Action::Land).clearOn(ClearCondition::OnModeChangeOrDisarm)); + + } else { + CHECK_FAILSAFE(status_flags, navigator_failure, + ActionOptions(Action::Hold).clearOn(ClearCondition::OnModeChangeOrDisarm)); + } + CHECK_FAILSAFE(status_flags, geofence_breached, fromGfActParam(_param_gf_action.get()).cannotBeDeferred()); // Battery flight time remaining failsafe diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index 216a94c858b4..9be8ce24a937 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -161,8 +161,8 @@ void RtlDirect::set_rtl_item() if (_global_pos_sub.get().terrain_alt_valid && ((_rtl_alt - _global_pos_sub.get().terrain_alt) > _navigator->get_local_position()->hagl_max)) { // Handle case where the RTL altidude is above the maximum HAGL and land in place instead of RTL - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return alt higher than max HAGL, landing\t"); - events::send(events::ID("rtl_fail_max_hagl"), events::Log::Warning, "RTL: return alt higher than max HAGL, landing"); + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return alt higher than max HAGL\t"); + events::send(events::ID("rtl_fail_max_hagl"), events::Log::Error, "RTL: return alt higher than max HAGL"); _navigator->trigger_failsafe(getNavigatorStateId()); _rtl_state = RTLState::IDLE; From 4ed3e9e2103e75c1e9d52fe240d4f0c2cf6266c8 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 13 Aug 2024 17:57:29 +0200 Subject: [PATCH 62/90] navigator: add failure enum --- msg/NavigatorStatus.msg | 5 ++++- .../HealthAndArmingChecks/checks/navigatorCheck.cpp | 13 ++++++++++++- src/modules/navigator/navigator.h | 2 +- src/modules/navigator/navigator_main.cpp | 8 ++++---- src/modules/navigator/rtl_direct.cpp | 2 +- 5 files changed, 22 insertions(+), 8 deletions(-) diff --git a/msg/NavigatorStatus.msg b/msg/NavigatorStatus.msg index 21517d61e7bb..da666533574f 100644 --- a/msg/NavigatorStatus.msg +++ b/msg/NavigatorStatus.msg @@ -3,4 +3,7 @@ uint64 timestamp # time since system start (microseconds) uint8 nav_state # Source mode (values in VehicleStatus) -bool failure # True when the current mode cannot continue +uint8 failure # Navigator failure enum + +uint8 FAILURE_NONE = 0 +uint8 FAILURE_HAGL = 1 # Target altitude exceeds maximum height above ground diff --git a/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp index 8c7d0e314af4..2a0245bb1fbd 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp @@ -42,6 +42,17 @@ void NavigatorChecks::checkAndReport(const Context &context, Report &reporter) } if (context.status().nav_state == status.nav_state) { - reporter.failsafeFlags().navigator_failure = status.failure; + + reporter.failsafeFlags().navigator_failure = (status.failure != navigator_status_s::FAILURE_NONE); + + if (status.failure == navigator_status_s::FAILURE_HAGL) { + /* EVENT + */ + reporter.armingCheckFailure(NavModes::All, + health_component_t::system, + events::ID("check_navigator_failure_hagl"), + events::Log::Error, + "Waypoint above maximum height"); + } } } diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 70068a9c515b..ec72881fc527 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -288,7 +288,7 @@ class Navigator : public ModuleBase, public ModuleParams void sendWarningDescentStoppedDueToTerrain(); - void trigger_failsafe(uint8_t nav_state); + void trigger_hagl_failsafe(uint8_t nav_state); private: diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a00ca0e79c49..15db1abdc44d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1358,10 +1358,10 @@ void Navigator::set_mission_failure_heading_timeout() } } -void Navigator::trigger_failsafe(const uint8_t nav_state) +void Navigator::trigger_hagl_failsafe(const uint8_t nav_state) { - if (!_navigator_status.failure || _navigator_status.nav_state != nav_state) { - _navigator_status.failure = true; + if ((_navigator_status.failure != navigator_status_s::FAILURE_HAGL) || _navigator_status.nav_state != nav_state) { + _navigator_status.failure = navigator_status_s::FAILURE_HAGL; _navigator_status.nav_state = nav_state; _navigator_status_updated = true; @@ -1378,7 +1378,7 @@ void Navigator::publish_navigator_status() if (_navigator_status.nav_state != current_nav_state) { _navigator_status.nav_state = current_nav_state; - _navigator_status.failure = false; + _navigator_status.failure = navigator_status_s::FAILURE_NONE; _navigator_status_updated = true; } diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index 9be8ce24a937..f26df56530d2 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -164,7 +164,7 @@ void RtlDirect::set_rtl_item() mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return alt higher than max HAGL\t"); events::send(events::ID("rtl_fail_max_hagl"), events::Log::Error, "RTL: return alt higher than max HAGL"); - _navigator->trigger_failsafe(getNavigatorStateId()); + _navigator->trigger_hagl_failsafe(getNavigatorStateId()); _rtl_state = RTLState::IDLE; } From c8501cc1d02021e85fcc020bab3287d9e0ae08e8 Mon Sep 17 00:00:00 2001 From: Alexis Guijarro Date: Wed, 14 Aug 2024 17:52:42 -0600 Subject: [PATCH 63/90] boards: Support for 3DR Control Zero H7 OEM Rev G board --- .ci/Jenkinsfile-compile | 1 + .github/workflows/compile_nuttx.yml | 1 + .vscode/cmake-variants.yaml | 5 + Makefile | 1 + .../ctrl-zero-h7-oem-revg/bootloader.px4board | 3 + .../ctrl-zero-h7-oem-revg/default.px4board | 96 ++++++ .../3dr_ctrl-zero-h7-oem-revg_bootloader.bin | Bin 0 -> 42864 bytes .../ctrl-zero-h7-oem-revg/firmware.prototype | 13 + .../init/rc.board_defaults | 9 + .../init/rc.board_sensors | 19 ++ .../nuttx-config/bootloader/defconfig | 91 ++++++ .../nuttx-config/include/board.h | 288 ++++++++++++++++++ .../nuttx-config/include/board_dma_map.h | 44 +++ .../nuttx-config/nsh/defconfig | 260 ++++++++++++++++ .../nuttx-config/scripts/bootloader_script.ld | 221 ++++++++++++++ .../nuttx-config/scripts/script.ld | 228 ++++++++++++++ .../ctrl-zero-h7-oem-revg/src/CMakeLists.txt | 65 ++++ .../ctrl-zero-h7-oem-revg/src/board_config.h | 187 ++++++++++++ .../src/bootloader_main.c | 75 +++++ .../3dr/ctrl-zero-h7-oem-revg/src/hw_config.h | 135 ++++++++ boards/3dr/ctrl-zero-h7-oem-revg/src/i2c.cpp | 40 +++ boards/3dr/ctrl-zero-h7-oem-revg/src/init.c | 204 +++++++++++++ boards/3dr/ctrl-zero-h7-oem-revg/src/led.c | 111 +++++++ boards/3dr/ctrl-zero-h7-oem-revg/src/spi.cpp | 53 ++++ .../src/timer_config.cpp | 54 ++++ boards/3dr/ctrl-zero-h7-oem-revg/src/usb.c | 82 +++++ 26 files changed, 2286 insertions(+) create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/bootloader.px4board create mode 100644 boards/3dr/ctrl-zero-h7-oem-revg/default.px4board create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/extras/3dr_ctrl-zero-h7-oem-revg_bootloader.bin create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/firmware.prototype create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/init/rc.board_defaults create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/init/rc.board_sensors create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/bootloader/defconfig create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/include/board.h create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/include/board_dma_map.h create mode 100644 boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/nsh/defconfig create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/scripts/bootloader_script.ld create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/scripts/script.ld create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/src/CMakeLists.txt create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/src/board_config.h create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/src/bootloader_main.c create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/src/hw_config.h create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/src/i2c.cpp create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/src/init.c create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/src/led.c create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/src/spi.cpp create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/src/timer_config.cpp create mode 100755 boards/3dr/ctrl-zero-h7-oem-revg/src/usb.c diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile index c482b14d459d..fcf70a5a2fdc 100644 --- a/.ci/Jenkinsfile-compile +++ b/.ci/Jenkinsfile-compile @@ -35,6 +35,7 @@ pipeline { def nuttx_builds_archive = [ target: [ + "3dr_ctrl-zero-h7-oem-revg_default", "airmind_mindpx-v2_default", "ark_can-flow_canbootloader", "ark_can-flow_default", diff --git a/.github/workflows/compile_nuttx.yml b/.github/workflows/compile_nuttx.yml index 5468e51d2049..cc44c55ea252 100644 --- a/.github/workflows/compile_nuttx.yml +++ b/.github/workflows/compile_nuttx.yml @@ -19,6 +19,7 @@ jobs: fail-fast: false matrix: config: [ + 3dr_ctrl-zero-h7-oem-revg, airmind_mindpx-v2, ark_can-flow, ark_can-gps, diff --git a/.vscode/cmake-variants.yaml b/.vscode/cmake-variants.yaml index 1311079ff988..d55b170485e2 100644 --- a/.vscode/cmake-variants.yaml +++ b/.vscode/cmake-variants.yaml @@ -111,6 +111,11 @@ CONFIG: buildType: MinSizeRel settings: CONFIG: px4_fmu-v6xrt_bootloader + 3dr_ctrl-zero-h7-oem-revg_default: + short: 3dr_ctrl-zero-h7-oem-revg + buildType: MinSizeRel + settings: + CONFIG: 3dr_ctrl-zero-h7-oem-revg_default airmind_mindpx-v2_default: short: airmind_mindpx-v2 buildType: MinSizeRel diff --git a/Makefile b/Makefile index 7d4283a95690..37c61db1c0b3 100644 --- a/Makefile +++ b/Makefile @@ -324,6 +324,7 @@ px4io_update: git status bootloaders_update: \ + 3dr_ctrl-zero-h7-oem-revg_bootloader \ ark_fmu-v6x_bootloader \ ark_pi6x_bootloader \ cuav_nora_bootloader \ diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/bootloader.px4board b/boards/3dr/ctrl-zero-h7-oem-revg/bootloader.px4board new file mode 100755 index 000000000000..19b6e662be69 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/bootloader.px4board @@ -0,0 +1,3 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="" diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/default.px4board b/boards/3dr/ctrl-zero-h7-oem-revg/default.px4board new file mode 100644 index 000000000000..549ba99b7331 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/default.px4board @@ -0,0 +1,96 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS4" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_DPS310=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_BOSCH_BMI088=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_ESC_BATTERY=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_GYRO_FFT=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_POS_CONTROL=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UUV_ATT_CONTROL=y +CONFIG_MODULES_UUV_POS_CONTROL=y +CONFIG_MODULES_UXRCE_DDS_CLIENT=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BSONDUMP=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=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_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=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_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_USB_CONNECTED=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/extras/3dr_ctrl-zero-h7-oem-revg_bootloader.bin b/boards/3dr/ctrl-zero-h7-oem-revg/extras/3dr_ctrl-zero-h7-oem-revg_bootloader.bin new file mode 100755 index 0000000000000000000000000000000000000000..86dc50de7b9c4140d7356a8214ecd9a4b48857a7 GIT binary patch literal 42864 zcmeFZd3;k<{y%)~lH|52O}YT-0%T$72Bs}&1sy|DZkK>%aU2#$14X^H3RTdl%9NC% zBB%qDae(S~kUEN@LJFP7!qis58J*GjCM-^&FjheVEy@jT$?clweG+hH)OlXN=fCHV zC$HDZz4zR6&+<8+^V!eo98Jbg;E3=R`~Ro^9|r&5!2xpp7rOk>)?fZw_3@Q%m3yqr z>sarV?x^Y)#fL;TAhB1v<7y^!Y_^j3JCQ0XJ^T1r=eXi=MVst59Vc>1-<9sf8i&ktHQk9DQi>C#-MwEtW|3`< zkCb<^6C_7Z1()C=@-O>Zx=Vi3(!KIG2`FQc8RRYqUl+J;ydknUNaW~#BF`OkD16E% z{`m^8#hvyO|6n2+-$BTDfMopUH;6#*r(Pi)n~@ed*X>W1%spX2=m_QrjWyHx9<{i{G38_Lvv ze_!izIP;6sX6#(%C{8YQ6z?o$dr406a(nUVWyz9V-ZMYhl`LUnSdi+ntICqf2 zlQVF2$ZFJes}tpVZ1!+De(>bYMbXk`T&Lk`msvL2H8p(4;0`t6O}5i)PIg)Gomtkf zc9~=8x2&AU8Z!EQnT1)!9L8W<;OHR>INI+X+bZ0zN^nhdCP}8Aj+rxi-1CxL;d>MA zvy)tLl1WCs?4B3r0ygy6;%4@cIELo;*s|kLzgf0nEbRV@f@En6cSr9p_c1$}>38`_ zKD$u4X$vvnnu?T$WVYEz3Cn55u?(|8Xdy9$>_P()kJN+rSv*-tjAXtZ_je&JM4F2P zd~z}-Vx*8`FsjtzI3zt%k`!%AEwamc{_4*Sq`MQ;&^AAq;({?;==ihrNyqD(oVeR@t)5*EuD4=&7_5nMW`n8Xulr3GTcc> zh{?iz8`2r14x|u6^cQihCh9ZPb_Zpm2Q2_V}D$Sl~L2J-#~G9q;phpiWIkj z>v2C`>0z?t-0>S8_Z;#N3w?XOVLi(3_3%dPpRf+zFY++Ae(NV0SG z9h1w{_WC2a%-Q96-(1hT7S21+yJ9WV9-VjgKT6)4$(&{KzWBc6yQ>&-gGQ*G1SDopcHEIA zw586r=B4Jxg!754cU|@0m&LakX3q`hcKg@jY2@sR@Y7}gZ?o7_C348BZ;qS2Ih^;k zU+?78v@Xt9(&I*7j`>w-Ebd9J;#Y_7+?H;?F72F8S6ZCJBeZV)`Il9O&JIPfdV-+toxppjFz>F-8 zjBdHZ4tiOOQgMJ!T#?q8j2VBepO|$;oRP?{^jkp3);b(=hr{BMEp$eg{ZS7b;L*05 z_6hv}Z52}K^*rgIxbfR4B0P$e(CD@>ZI~AWrRMEP` zq_LJX(z36cxJe^(kjT8k{E0M2p7{({JCV8ArQJ2LPdSfYjFoBrjTEOaGYHb|0K@*b z`^lH&acLQvcF#p++%BZufiyNPBY65t<(02WI=5>7c~L9RQ}`J+q>13R2C@sugu6W4 z<&jj}RpPFajK-aUJB7s4I|g^m2ztlijuYse26r0DvlSigbj=s#aV&TP$p8UvzR^Qw z`hS0ljQju1_|h__wbL=8&2dJBol(-6;8@;0&Qa2tn16rwxcrjNk+UA?9yhC`bCmdC z_c*bnb98~!J+7dn(@_!k<9{PF;$`Ai`rzjlbUM8&X#7(ro7g2>2)hj{r!^MX4W3mLy) zPHsg>&%wv$J5{}}SJpJnD=~>_;%%-qu2k1Ot~;Io`kxcNkNk7;{|s{$gSoiKChmQr z*!fQN$CvnAwPkM&Cx1O$;Jg)eJfpZ~eyl9dK5~hy=j7Xm>`vw_NgU&1Ei-55Rxt9k zeyl+TZ~A$Q#buG0%(-WcQY+!)VyyaGoh8>oYetI(mr+WRHrr>NF-v6p2v>?UvG5gl z`dV;4HQExN+o3L?)RP|4GWiF`;=;cFt#R*3wO!pQ!?LBB;> zLf-VxRB|ekB&O>lhRJzN;alGUU6gn-Ud9@J7}rXq7m&6gtw&mj^bTmkv=|~>L>V`p zmm{r4av_-#iBM)F!r>YsoJ^(k3bY`Y7E(D&dh(VnH<4{T#`O=yC{lVwT0Y%D1O@fc zaI8$pC2l=(wiMWafL5tG|Mvz`Ub`T-YCZ3+cf4w*&GY@zU4gH4%c zBV8X8yX#p{Sl~?aN_iw1!1&&ZCBiG{>mSmD#+bIK;`xfzy2QhFwX*Z2?FDMcDo9i^ zF{_-{{K+EyhL)$WpyQ?A|2Mjs3%VH!zA`MlMg)~p-rb?gT-Nb?YO&HHJKc>M4`^H*HX4DyCIad?qOpz8eecM1~0%FW-W@gkiptj6j z-xWV2#|#9}M_MnPvx>%N_wypwLPf0j`6WJwlSlQb#7{giJD#m!w`g}TTcUTIMLyYL zf($ZM;m4{@r-_{01&FKnS2U-IjeRjTBG>m_^RJY(e7ApGqxu!HH5#=sQ;8)Byyx=} zkNF$dsCk9mrhet~OJr(-7%j4%fL4+Ml$J95Fgz zU}#<+zP@sLZ#j1$J6yiLKc!=@R3z$)=^9ECwRPlWB9FxwN3_T6oQX0-{+mMoclYt4 z&7&!zBfocm+;qlEbAR7QF%wkpInvS>PXvnZptU8>0HPRuJXd=i_kJLIeyQYbL8|_% z*aW^vjBN_p&*T)8#5de7?Xiz&&yy&Huq*Z=l}OU&_^vfjti))Dt%J$9U3$NTF*CQ1 zw3T#z9HZ^}IChCLZ7!KS&G&bO*se98HtXA{_m%v26yCVIntsdHamH7bJfmePS4TO{ zs|v3#U0T1%^n^t|!YzFwT}*KBfqb^U25TOp`oj~9+}alk9vn6o88PNlpjP!ygo1~L z(?k^=@o$(M#v0%7hJU(50(=apR@Hnm-9^l!ML}9;*R?Mai7~!px+G+?UB@2V-?dDq zZC@y|^8bcDQOMkN{#l?USxQa^lXC%eFxKoo{xfF!JVm%Irjun=T_44u1kKH&qd4uS zbc4c|(hW*KraC8I3YL)c#Fh=>c2Oge@gv2VqQOjhU$5ZgltG*L94Ij-#}10Avkqs8 z?9@C_-L;-!s_73o(Cx52MOr49T3j=rM zpPH5IBgSHdkNJzTd?^o)z$|l-1;t z->L8=KUes1;6Gb9`I7;+$m}qRLVyuCxoIFj--k7`n4F|5haB@8K?n3V>TRt2?FY0EDo~AQD_D7Wm>2UrQ;n?LH@wT0)?~zr?+DQmsX7=lF>&%xA2Gh; z%4^==7h|lEpgX!5^WsM<3i2A)ODb`yr}RlClRw4GD1DSz_M#je+5LTdz(~~c#eRx^ z!=N^yU}NYzKkXNKWmU`nz#G~hBLDI5zj8nOK6me7Vo?{X3zI}9_l?Ytz;)Sxz$(5_ zZ~|llN~0-lu0VZr51?nH!-LgI^|c>pR5~Y_m?AMVINOLql{8DqbrFel9pQlU72B_C zZ}sH~9tyjs!u_lbfiB34hbbSq!SAddZbJ&soPaLlrZNfTFBJv)#!&ETh|<*Mc21TQ z9$G?Zs!`N8D~+SYB&Ql2Y>X5N%7Z#j`NK0@CukpQh&jy7);;z|s#C6mHp@*^OGCx zY9AL!WUNgBX8up|CtN@jwT?zAmL#qArT#6!x%&P}oyzNNHJu$MEC^yWl(4 zKrJM>o($Mu9)kU)!=EC_7{{ey#dB97EANATxB%C^A(D2?n<+V5er|xIIlZgUeuK2m)h*Sj`4j1tjAfH8!BOMNl#ZZZ+|r+Jkq^vfXq)NRp%V(; zIe?NIehlMea6iz$;rc2y=;;rkYYE-gzd_kRzr0ECGN-Yg)K<_Wxf;|@ecWGPrOM=d zyouJ)+gD$u$-LsrtQ0aY`)JOGeHXDNZs1(NKm<)AvxUeH_oaE59o+t3DtuQc*!#gs zh}Yq#0V-SY;xqeUNr(alMw7r@0{UIU{2sIU++%-y9zA&uv$=Z{^Sdt_;(N$ry!Sce zML0IN*;er1VQ!}yQpHv8Yn!;OupmDATFp#QtNP&+O8)*v$=Rt=-M^fEwMtS4Vi<0x z_37;ev=w$Im1pRRrR{{N&eoaY?oC$FjFxuSEPMFpwsoCacBJi@PA%%doDOq!5gHT< z{_l_#WB#K54cHMPvTi8&3}`ixQ~T%&3o#df?rO7Gh+Kkhv{FalDeNuLi|nL6H|$wmM)z2VG@ywV45Myo!P>@YKALt_Cq}rYoF0Bfrl6#~-g=xr-XP&-24;=Z+Ftq}b@d+KY&VTy+6pMcX*ID@w!~ddA zq=mMN`Eoo9{Y^ioL}sH>Z{CHuvjWy{50^`;JU3M`ZxQq7O3dawDlO(gFO@6%ZWU>X zwZqI*VVX9p6LLp*_G)N-`o@|~yqKaId188#TT1gdz&BZ68lbWpyqU<_F!#9&v^o@g zXQ+M?EN!LrHG0rWP%PPqw-J8%^7|M=^^eC~v+2`@1es^mUWcsbYHBEVp)jOZy;xB& z-b!T4Ax$F-4$J_TMh?Mlw39IzAdmO?9us3v>z`i%8 z9c@zz^aFh?+Nmu{k)pzUmgzdeAME38>+LZu^Dtw?7_pH;2Q&_1&D`Lrl&=2tY}8co zGq_AxJLLpOt{SXB%X$L&KQcpk%^DMt-yaAt$V)DkI9LdN^YPlM~;N;U;EgJqs!w8jQ4+ z2V1QI-8Rjcey7U_3qpRHlm1V-{{Ls((`Qzm+OHA|JtW}LWIdiH8m46fXFeL-XAk;X zK|Lq*Q`wCKT2<_%(RWUI+$b8R(h{D*KcIvSB@(Zfu&Kx-{hgCmLE;FP*cC3}MhRG4 z!*#e(hwyJDeiJUCG=By@c>?JS(hV9`(Kr`hUVyGd`!-j_-)R&R!}YEne8;Z%D!^mu zcT5p0m;WhpKlzg47%ARTJPw@hm_ppKVw)mt=1x)hsQIE&GqdSdu@Kn%YseS(H;p<> zjNN{c+2hl~auV={fC&>zpxve2G|C$dVWQS)NiU(^W5(yUt11 z{K(IL$JOqiCNY!ad>POi=~q{VRN^yDB+KhHicz3>tUNKihW3X3;)hKpT;}Z1xBeHJ zV!`>Q4DzBCGSP-mG@d5mX`zRHWfUi^X64Yp{jOC~z3YH<&b22j*Vlz*Z8~mZxutQP z#89p9nIGc+U1eF(}T3n@5j6ogL_C(iKMJntvSrX$)5vQtI}}0HtneO$N+cbf4e> z$9-Jid}CfAWBvNS$yltsap1-Z`9GDheD9N%8+HHpwJUoZ(m9XbNsK8{yQki+^I4=6 z&razzDLQ|rYoR9^*z?2yF}hqgRB6K zww~)c%B4KnuHjA+d5rST6M4<7ymX-7|32(RD(AW?ed7WpN6%xWJUjsT=?Q9cqVHJP z=CSU~)K@|4)HiA+w(5WmY&*9!#?Q%u%_j~$e-9Q+I`2B%?-_tTmry2_T#KoA7ux)b3TtD2 z%gNVbI}qnO63{Q;dWl8r zqW#~D@w9ex2|{Lcn;Ohe%F1V*M{p}T-`U>sz{ z-JRG|QF64KyDiac)Rf3N!P)XgK;Ux4@kXPuj?7h59YQ~+#PoCBQ#yrMM`tYOm{-y% z3Y;ZP;?lW(q1)KWk(HfXEQ5PTC*d+(E4_S=Ef@W`<6$p)cSlSM3mZ$9MEV6ewv*!w zj*G581&B`Xv(Pr9e$-}b6xz!|Nyogxu}qq?4fU0xrj>=L&$hC_Ti@$`_)LIsj;RgM9+b=h#ORKZP))(mf?>GI+hDm)$)po(l=P`j39J~jao9yt{+|=iz zym*9IQ_hQVqGs9zS)rzMvO-V095q{Lv``K9C;VV)d3m2vS`8^_c^@4wF>8y2MlI~a zZcB7AGlTm227FPly;Bd43VfQ#n+7Pf82JeJ;gt{G{U;quM4TOs8l`bmQLNAtRvlFa ztt;hGcz(r7EJrWR&6O?n59T#fI>1s%JI~NUWjxw$MWYtA zi<5d!SAB>)S{ktgEnY=2&U|3q1M60P^nh0xG_ns0#jVJ5o7X*vvX8YQkCvr)eWRb# zCK6#J>>R1m|FxfyJt38ZF*3{^*sF$b_*P3-7{!Rb^JDp#k$)Fbx$0Lid(5@xR10jhI@VE`V~x1+p1m4Ts(3N zEwDU&`Y9$Gx~Hv3Frv)P6?)gs6+Tz3a<|@6%%roOgw6HYds>&(D!(96X{=XBv-+Mq zfLNpXsMoc9MXj>XHIqiq%!~Cv6P`CSoVUnTP4gZd>$!I&&38Xq?aFY}tvKJ1?4r_= z-27p;oi)ii#)h_|_^6mTN2{Mr7g|vBvQPX6(7va)VT$?M<~*qRq7ZFe-)!mG{HO^q@9`fg zuUpYp*4k~;V@`3uV_D=%^IDNdWvp=BKcTPEx)*sIohdu5D$;q#VZb(!X+0D?A zg@R*Ym!dYiUzSmsWQYB7xytM5tzMh%T`3y~FgRnjcbjszb-=@9BAVjSn3fX6F+tjJ zY*QBM^&+*<+f2knpBL%r6L{E6`s|RcT&?S?M_Bn6z{^wqB=NjI=RmE*$yUaiZt{^# z=Je8)mplomb+k93hpbI(VIbA7M{Bi`UOdw6PFSxj#C^MNy|M^e@B*dd{xooue++-& zM=5<*m><3aE$?XgHpNATf=`5qncl^PHDSopO?izUm#d31#NQ%!ICvhqi=m0$4+U3; zrR(Q2sNQ{~JW;Gg8=0P(^13p!hnTh(SVRNtw2Y|_^(Ts3R~#>^YuK)SU72r9IC&83 z;(4|1B!y~P=pgcv)oPk+O;NMqTtBSC`sM^VR(Hi4Ye`sk8FU~I{Uj!pSid64Me${< zbEloBF)HRUg+oF0aK{R)>jC4+P%vk}wNe0Oo-(j|1%>khL=^pcL6ip?|AGbZWbIxN zN29512`>8n=X$J3M9k53>FAtUTI&i0XASROL5wuQjVy4;BM^_pSYB7=lJ&}6R?I45 zk%*1PP;noGa-ow>JX)?2@s3*ho^zXXp?Cj#G*z}pZ4Ny{p$2L>>O$W~pzkVQ4txY| zlNP;yb7=Pp#JPoAqT|>X9>+Tvhrt6W2lk&g4lDoG?%GCekpzqS#1OGW%}EvEMM=5U zb8o6OR}x=TqF^0eH<$|_jgS@a@$f@dKz3P!M6n)}sth(%kF$(h$~8Hb5{q+bX_c|+ zNO!Bs*wi7BHLmU>T)&1e;JV@kNF;cYX_Hfccpfr+owG+bFUsUcpV-Ukoso7jVe|aHoz539pOLl>|3_P-im|AQ zYiz_AiL>iyCQ-Eh+(BBu^U3HZKdrQPa%M*r%v?UaLsbyvrTlF_}NvA!2 zc8lPQaz3v~$fwbLB)%|AEr9kzO!sI*G$ONhRs7)rmU7H(%%6y~2K81ku&_`@|?$C1@HtV&c4 zYV95omVpJpf?Kn+dnU)NrlUb^uA>Y zjqx(W=ejWM%GB%nIh8p`3xz9%i`uCvMvQ8}Wk04|oX%dmXhsY&qCTmdrU%Zz4DON} zRaM54N0_cfoJ<@fBiH-q4R?*5ntyQMT8~(j;Go`rN9R!oGsSSH(BkOKU)42lRux~R zk~!6KhM8QyNaKs2Gg1u|*UzKtE`{R)$m?Y+>+DRIn`h6+ z&CK|}`^Uo@o$BhrJnHHCK%#wD8e_{e@W$QSq^-KCXbH1;pSwuGFm!4PEs*;q=I2#-f)t**HMzQ~wz8TYv8J8Dci* zS5kf`_+W^B$)gvRDofMTYgFcC-I|1_SLZz6y?x@ z1JYEC%i73^+S$otQj&Mr$r)?2MLN>0nZPRiZLGrGgZo)XrL4le30QmhUJtmW zt=q3Y3#(@lpBpW0w=-M&&(Jth`YsZAV=i0VWT{FnT*4$RoGFu~Nr2U|Fsv$?eH)U( ze&Y0@F8{d=@pmpB8<%gc@(gqb<}*&m=0%Rq4;?W@@S$PEweu`h5qR@<3hx^uC7Gvv zztj(C!Gfv;#F3BjC6+mt-qu8)c=bn@__!$8 z|G191Wiyu=#Cc02vQPzhw4RR>IcjShwu;^GNBt($3CTi7;NNQ@mj3IEfADA?R-mx5v5uOvg^w z1`_2T6!roVF;i_YPIOsA!Ia_BHGFt}N>@jGJmdrVHWYjSJ%~0&7ttPIg*qHPBw@ka zq2Swn>f*3e5^j~wwE(Q9b~su;g{i%Y6OXBx6QSVg7Pd-VIG>4INaNt+<`U!LD!VMO zi=UwIr+xcxRFLYlzrAjA2aOA!3@MP<0(jxa%WT7Dgkm8H`E~+AU&AGcEpDbTPBx$sT_5aiLf6;m&mD6 z=T=)Dj~Y<-=81@d*+pj!*WJv`pI7FVX;VwPvK ziz`CGfx+@8HB&6|Y_9y#(}*pm9%o`oEKG)m%)~EtX)V~N;ew^%w)slpZI+(lQai3T zvt?ss4+YOz*0C1LW1-*|12#~40TF1g!UiE1go4?CGlel7+j%4d?h}#r#KJEJc+!|G z(!2F>UR)`Qh^Rt*&>$ah5&FbzroOsu%I(UKx`-EA=?7WI+d3Xj11$hZ6Lp+zG|D2+ObU;-8!jiY1P+NvpY4DW_EI>ZOW+4 z_Rd(duKzhj+o0=TH{2A6(WSNNuJX}~m2`RL&9HgiLWE!7T6X=a%_G8`>*?pXu0r~B zS|r_va@!PrYby55sJv_j=Nl!vb;LXFzzrGCvZh@#>f|=1(wZi2jZl0@Jd5Yj@ld>^ zGXf-r!wxqL$o898drBaS1e+iXaxzb<3BK<2!qTu=~2miIM zLczNS-||2Bt2cio`;BY#7|s89NyaNa+B!RBl#krBwx?uYq>VeS74M_1Y$sYZ>bGJp zD~#YDI@pjyLG$o4ellfkPu@NT-%Q)biOahA1k36h_1@9**glf6wrAzONZs+M`)9sL z&C|oZkA;j_L;k(yv|*qBfy2yAoX;lfiE;H`{p?NSeQ|O~XYxfw*RNr2<a{(V zeG6cDUE4Em-`bn*xR$!FbPbKiXEX2U8MV&>d5x|GSYi_rTt=sPV}gy^fEn!NShvcu zuFBty8rXeJ7ffqPQ0LHlGM@Y2TL>%g+Ma9gF&KCEdv9MbubGFp-@QlM>v}KJp8vfY z_}kG(($@Bzd(XVlfDsB#4wN~u#nHoJZ;uQ9QpS0<7ZB4U@Gu}u}#CA)Jt)$ zC4w<$v7b!*QfYO6skH3^t=@k{bIVEi7?Kt#Ld9u#U&h%KP0C5eGD;4SB1N#)DRI_J z;JwVbGIhR5pr2n2)}b9p5sU`WPA~P-5=$Y0ZNjxl!=7X<^tKgzjqG_ z;dhS+>Hwqu&^H=UP84U)`RF*ve!47LA7Rr}PPRo4?pG*{ru>)k zUP_~H&~GE&6TO>I+D&PAiR{wC@*;evS#|OeMDZ6UKjOrwx1>m;5H-G7iK<}xg{wJs zveiIkW^SBqG$=Jwdz+WkA^C8`SY9zf@*_xUzdz+|D&;6awp> zE*2}BMQjYPoaZbl*P zTb`4?9}6Cw-tP{VctoI)mfAq7w#K*S0$O3&j1%ZjQoqknx!0@x{tJ1}4{w+u-JER5 zf6u)M`^?~S87nPe&({ry79 z9Koatj3lAp!v5d(jYJ%FbbH3yuBru{tVQK5Uqh*yCZT+F(}nUitXc8b^J{xm@99y) z?rS})r6&Z-jE19{!$(R_-Z4eFh0=?UaHUkM0(`P{L4vF68!E5sktlx-+XgV-H|9LA zQ9HZCWfD2ik?jR4=y8Yyhb1Vl`Elc5Fm{+h3`8inU7@@m9`qJkM+IatDkHil5=%r| zrcq*4TLv#1h{%z#a}YZrrHe~YE6O5P;y+71Zam*G82sxH^+JS#5-fQa!&XwN^F`W7 zn{!d3wyIA;`2Hv2*f18;IK+U3fjQb>+5=l|?VrK9NZ4vu@4opJ31ZsZ~sa+XrQW;r-X`D!}eie}nR3P%cCrEwdi zd9zF&gY`Bn(a*!4(9MOz!OFhh;Q#UP|6_eGl>kPem#_<==P_#E{~2NwelxVGh!r^z zavnT^wivtON+Bk@YuN05;G@;-y&$J%jLDaCVib!XnVMFWjBL2yOrYBb3W|hn`E2dYdE5eUwTBbGO4*WiZQN6iHXw; zu-K8YMY(;ZCWC%ajk|g!cHTvN^I#F3|4KwnaoC?j?QA!exHy#S9GC$g9@Ea;7W6TeT-QC$9O)QnB{PHCF39*nNW=>dmhSXZS?8|~jKV-}-LKJl_$qJXsC*x5H zCG=S+6|)JY#urT}jD}nt${7y}4J%okW_hjM+=E5jv&L?gLu`Vh+RVAeI%5j8*v%P> zQp3Ru!-(nadz|`iZG*vi@axgNwc2+C5`z@|UKF_uJ0-%9Ys(tS$*c_d2z^?5M_`#UXBMWmMOgeBuR)xw!tX6HsbcZ%#}a>&-J zene^IFk7ijgYL{8cparq@Z zyB{+^&;Kl$n8t`)@9rmPJUj<)sgWhUyB^&g(371@^*Q?7wxt^p_e3L;HQ3Ez#(t7e zaBnD0tZ1h3>F^~VqFU*2@K=a{p*AsIe!ZWh^|kYIM&De-b@TF{`&u2XI_#T~U+&|u zLin>Z_c_RWz3q(g8$T}(z>}cD-Us+N<&la#Rnomu>C`2@7k~Ykob&1gDkB2oeVnulilyiS*JB?EOiMa2R zip|vCV0L}1*k*?J{5RhP&F}Sqz+fdh@^OC`c2xY;FjpdJ3GIJdc ze|E)7cpIb7&&U5_iIs_+bu4xzi?oivB`?G``u5|OSbeoC%F`x&0y`!xFXS6s^CSjd z9L6!vm4qniP_P=%q@LkW@TnFB-b@|kD?ia+V)mk^?G3HyH|;OUnhq`EX0IkNS~Wq| zsrK2eZC2EGOQ;cW$vsBvUJ+6Qmg~#-17@jSdfT3H{A;Ne5nWo(*Vq$okrt-t(qc|o z@|PyL=FM?UaNQV*Ra^{uKQ&Y;2~!={p+W0ThhIl&o&XKqDrVu?89EGjf96jWKZjL^ zICj{nPVW{+itKS3%OF4ZVSq$ucSdv3os6c}r@x7Bqy8hRF+bnJ0wWZevG9Q`G69hcGjoZY ztEwE zFTbUz#Djg&)*5>hY!U)uxjFf_;GF&KjOG4nrsJH1U3spG<|BwbWY01)j=*Y7YZ0st z_y`;>PF7lA5p`*$geC#;Ulj#T$>8euBU(({7mhU zwf24=;&4bT z*($N7QGoT%EEPA6xN`(-S9#Ej@-}|}KO|$Zxb_qb2G0-Ojs3ZfSv|*{v#uV)Za!vv z0o~Jj*e8gtU>>{ z^xwnROZ`Heq;>3U;atQR`SRc2Z2FHk6bcj?pSKI;}-5PL{g-YS`nIMhEsX=|qzGv-bVrveV%|J3gFMHt2b;?4Vr( z{u2s*J^W5tdfD=1N`bn{Ua?Q~-JFsmnnVd+?*i<37;*ZyiXGM^mU{-*Q1HHCURrLa zUw%A%-p}XXoUHSZDc_vQESs3Wcj-a<3Gd#r#i?&EO;ni74BGTMw+S2fqph4a1zoXUblM#z8rO=VhfH55@{_SdV+H#woNUh>kE;vDv_6rviE7 z=S8r=Azdmag50?Az+W_+m)5}h`DC`n0!*J>z&8$w9 zII1OaT%w3blybAkb@4HpjD)tYg_W{K19{hen~lq` zVl~3_zg!{@obz)TU$n`7Cgan#fZslr?m~wr;|`KZqr*5c8N0*cDoJ|3PoG>-z~(cz za-zPm)~@lQWC?odB-r~?YbQQ^GkxmA*kd6*S{sjFB6orI`95A+11-(HfbxJOM>R0j z+WZzGX+B(P5(9oFN5~zQaS>0Dh38%J>mg^gmH|r}!SUb1UOdDvzth5Qr*Zy>R~n#_ z2#p$8g#7+?v?>1`QSTK6;7Qo~@#6kkkxbYE%Q3a1AfmnIP+)>i@H$fwjU>&(m&~+N zh+oS3YKy|`+LTJ)vb6y(nG|jQnfSi=e&7zBv+YnIQ+K;J5m%#bhWAjwqWhWm?SPeQ zZ-~k)X)|Ch<6GDP%`JtpTVMy&H)}c$1t#m#QPQgG^cBjCz}6)w%wD#Z_Tq%^?Z7P9 zEGpZg1IFmW9#7NJEw=}_Q)U0;Ju-sons3wzv!pEeNlrjA0H6$)iuIPEW# z1>qm~*9#~7@As0mhmIc#Jf)kAwpQyh@c)o*yf3ZCEseuYo5CKW^r8PyAOo?RwEiWa zL#)@*dG1$#L`?XbEwsO5#e40A@&@5ul$)Y!3zxIW_t&89%6jBItkWWotTnWtr_#S_ zCY9UpRtmp$ue|Y=R7To$OV8kKg^q1WLmwh}q;j4A?ZEvQ`G!-60;QOZ_o}1iFS!M< zXPv4(CGK)AQuw_TLPE<OJYQCMvB(XKK34YKeTyz^h6s>_$;6lVSH7WL9VQ8N%m1fg8Z9V2neA3;=PW^}9nebmNy zjkkW@;+8x@2F1sD6vj)#fuhl03kjBx`!QHzmO9GTUos zrJp{G8Yl$VcBZaw*7(!f0Ha~*zD&{eZ|EkOTxo>)&N=?W= zU_aP$EtyO-$Wgi!U#^(g!R#m;`(XpK<*zt}=DxB2^pizElhSt0WppfOhbAv7rDVtfrE-w_=MO}B<2XOUJS*DwsxrnnEhD*2g}YQ(()7ThY>Qb+3a|0a1vevJ$_T6N zp+M*km2Yn<{hkL6*p3>S{Mi=jr`5ks1OX`qiL{17s8eqgv(v}6(D}AtkI#$rif_nG z7U}l0>@@h0qG?Ur3w|*RC(oS5O1Y2DX=2MP)BT8fqVvSJ=g>Y`+tpJ}{w+YXlTDFX zx0((IqI46_M4l{?HehAYr)k>@Ixa9fDP@{JYhz2y+g8*3{OAA^y`d#uNl2x%Vtc_~ z!gIvuc7A<0@m-(8i(<_ zukLo|U)8e%4O;Ad*TWZ;DaS?AH_4cHBERa6dE1^JuOwwzGi>rp?u?$+;tW}Ty@UXJ z`m63_nSmecfZcudkM3Y{cSAvbeV0mmANuut$0<-BvX;WlfSvE>9^FZ);>zE7k-IXc z#n7;@;lt6J{dBy^|1sV)S*<-3Sg50;y9FF#C3m_ZDr0(ERAwH0Hk>?j;w|`%zU7uQ zTE8~HN43Lp%Xo?DD?e?a|9T7VhSr82f0{f>8x7lj1X7%LG=&s9&P(A`?so#BDHtu| z1x%fK%jF10;Hec>B!1O-Kb^4&CW;vmy0yKa=^Me)=6_#Y;CG0pMG5~8_>+6wCBUOx z(!u7oH>~u}ysBoCJ6YWwJrC{crgZ>I7}TNdQi0zcu=lp#pdl32<3ST6>z&pU4}3f% zdS_*}zu6^__h)^AeTS-iO6}5pOkHOF7;)7%1!6JUB=@NTtjZ!Kwk$MNhVk%g+A^06 z+IQBQ42rD^FyZ%^CZh>;-)Cr#A9|mc`{)`gDMLww!H^AnS+G0WEC01}$s(5yE9!^6riWnpK?<_T>MWJQ)tjovfZdK(sO&Ag5y(K5$|3%If$T!p{rT=bo zu||v)6RF16;`z>|WyUw+Kk%Kte-utP;2Fo5e3|8=lC8$S8eX>J))~e$cJp&>EHid>(jsgHoyGfcjD8uKK5+B%=7>B|Nh&VPapj5 zFX1+RdD31aSacH@Ym>I&Y2S5kA zd^w&8o!_ita+C9427MhR<~X^oKXbL;lBccTX|%upHuL-X`SynI_@|j^ZscnMD&b4z zb$EYX-#_oNUdv-|Hb%%jFOtpeDX^v0IdHPWZ_dsamxyG05u~%PLnL60StQOC$<`fW zv>7{573Oc8{6%P;xJkU=A9=m+=y$pCJ2gi7mvTB%zCF@!QG0*>jb2~&)q_^k9RZ>6 z@5)+2rcUfFk8u!p$1P7Dnd#^N{Wc(NM7%pyeN>-VqPZ57AyJmw|-WH4X$ycJwarAhGcznY9;K1S=W3D z^cWsm%qVd>cDNZ8T2~bGDypwEgtZzqA}v?R5>lqw|a)ajS6YdFv1dd&aDB~?yc^1QK!?P)DD#8C1@!ybFA zFCqW2vYqw=OI2;eM8BMiez&)ioBY14lvDnZfHk8q|9iM)=YYW4M}6^III=n(nbae&%5&eBE?7Vj-Cm-UCA9PY=@=VBH*E%&3f)hjdZCkDzf9 zxd?mRx%Ox$Yfi4Bsvfyua2C%dPo4|KrfX*Wk=eeWNhj<-$H5 z_&ZLb_k6nw{w5|j0oIOENIn5yzu%2=l_)RZ6bgT6SHQ?Ev@e_~z#0+-OF*n>u4FT4 ze{UMFpiPyJo|;3ge6WHZrv4q;mcB85wgvT$_0n@2RWo+llTPWJI8P@)O0nxKcc(q> z7%#&T39A&{Gs(-T3MY5K2FD{_JF+90$8M5vj1OT29%&=?zr++13yrJBt|h(6L;smu z?essF$L>$;Hmb~Rs+TNHtdW=hqU2cWF)liu)#wxJY;&E)Zo5Hfcr=gdPG2heniuzz z35V{~;G|WdegjUS;eUG&dUr#Ja^(G@&huA=R2-mQg`5AX2y=IcV=5Tb{*p4uL;b)c z6EnAt80mf()n*lBz^kzR^73j$RcD%^&%D^i>_$Ij(sFho5xS9(4JP zCPL8?BGe*XLAw1OBIuB4xi@h=fO0I_$;G^3*Ty=&jz%5xwfA+D^gIQBa9l_=w)JYT znwBwz%LJ;Wv3wf2`fc@>7x9HQHZGCyrfT@6lS1@smiyAro1QMbhW}p|cH@6dq3lnc zkOujT!wo$l#b$DBEA+dwJ~)ml_^c|9q&x3OsJT7 zHJ)QlnmuIFyO1YC!6U;W{IU#sG(UvXa8UEI5M#B;KdiM~(p#g_0WmR65%vWNXeV{zu_1jLC> z$bZK^(nnz4t6Ifjh4yz!VlR7(laN=hxba1 z3_<(M+-$eWYV@ux46!BQwlb^9gqy@E9c1#YzDHAU@oHWkUp!CxuxXp9#m+NMW@48J zAAsX(yfyM8j+&l4uBL}`)W}AT$-^l<*|Q<1c`z2g50EkGVXt7apSJPo?G1DXUrp!e zNj*M(8iny*`)6&KxjmZS;Y^!hIu>kky+CK=&%>I{NBndh(e;q_2O_+L`>jZnHmpfN zLOxx;bbV;{LXXsr5tkd;TG*&yd1$@*i5ohYmaRJs9gT8v*{7*v!|yNRJ$xo^w$6Z- z>V_AIn$6Tgb$&?a|IeD5!!_0a*P4!mYpO*}J2f`>H*O)5Tz$b!Yd!jXtxZF8TsCg) z1&WhzDD<82eWEy_;JZe!mm%AM$G6N(7wm9#)CGNxfMOyncBoU8MNEm6nEY zec^@WsO=GyUeNw=!`R7l+i2}iG4)@@`T|U>|Hp9JLB*~XWgtjd=s79?YH^9 zRJQG>C1!PMwTsYG?Qo?0h~b5^?&$f=d%)=giqjE{UOaU6o5-4-GbDRfrH=4XoY(AO zO_BG-L$n`n4n-t7y3$g(l*}klT12W~A07Sg7<$%z&U*~3>0FUCnchOi$~JI7%%NEt3xehnN|)t=@uVn<(| zZ8*ps!kIeoM{WZ?WC33`<5~%P0e78kgtuo>2y`*m0xFCBbjVL=)Pe4kOx@{NC#{pk z*%g$NQVvMH3sFv^%ODL0|Ix>oA)hctlz6R=N&61EqXv<)v<}(^ZI5+Ee2T36aTCoS z>#S&ujAFxnPQIfrrAUqO6ysdFyaR?J3X}Xk>j9iSgfjsg;oXnZ`?#?Q?eTNU#7vC1 zypOGeO;xn?yuXU>GGc3ou|6K6-Urb8rLgA;kf19t>J3dLJzq^#&1nY(_zJ5SIx}EE z$G-;sn1bt#@xM9zukq7&QBF-Gyr5~?_Q?2HIR~TT!=sCwcR?Z1=ci?Wx@uw>Q?ytuz_ zqIH8fxdHnuwKySo+FpBG3z<;=LJA_yqrIgsOm5iy5aR0_5JB#~e|m6Lt@o6{+Z0uZ zeVD8JRb$7slL^q2v`pQrcK+D8%689)CVZo+FK3{S)s<~8`1(c5OYsdU4LfnROnrIE zLn$6&Ho`Nqy@1y7SwE<3SPQef^!1BL4e<>d?PI;m%9-5x_oOr&?90*bpfIsmo~@S> zdOFssywMpNUxI9d9cqM`nMO6RR{w~MJ|DH58(N|f<47jA_$XB;nNBax@S8hQwyBEf z*-snYH0A~{qaITVv)$!%rw4_^*0Rk7U%hC4DGEK==%cw*t~^&s5F`22K6+i-vt}P4s(LC6!EgPv%)emrAadub9;Pr>m4#2M zs*&z3QwK(==nmvhaJr(Uy&JXM?8~i$q$!6qQ@zYhlj#_K;%uedC#glGyy-x>6&^6y zt;T9j)3cOlU8((4GL)<7ySnhZ1LfT5C6X2JzwECoW1tmmFQBDwsklJ<%AdpkXuiWA zY>Y(&fK5ZsANd9L|Gwa{Jh|*ihiu?-u}iA-QKyG_s{=}ChP2WEIMIYT^ zPES0jkz(QdrTCI6*wB}I`ZQzjY$uaT;1jg5#DcT!mypR*efnHJV2sLFEH1vQhAR|v zm*QNCzS0*C_DuxtGmv~K2JC(z9ym0?S13~a!}(c#6n~cWQT!_WzYF^sxTdbV|8wt6 zLVzd{v?3rT1T+Xp6s-7GLx3xa9aimX)viIrUaFP&Qj6Q38ilUZ+RhfYTJ2Vp*4dYB z1a*V97T0cFU)C+r_IXrvrFCtpv@VExF9gW*{oO?Cv$_92pU=&?FXxeb|aGwT$JAlb#o|&rtfseMI{Q9uN11dPo=cVU|$G0&L`gQ)%9Z@d<8P&S#w~ zcQ)gzY3%rYr@$lTVla}UdsQln54_cP8uVjl@?l_S(EO6T2C1w8??-6CjoObe2fu_h zfyz%~m-eFfRS(FofU4`ReGT*2R!v41AGi-YuxmaoBn}Hvx*fpmOzP~W@8F(1Om~9H z1wv|qzS_UpZguPQZ6&&h-B>cjw_k^Z@jhwHjIdi@k^FRF#F`WMb#JG0GQ^K7%*d48TaOZ20y3}}I!|1k(RPW($ z-Va_4$)8bIg->qRim_V^VwNYinsy7Z6vc9L7e11W-g$54bqOB56D3azNh?3=LUWr>aUOs z-1qjZ@OmlIes|_jbL~v%AkvJ}?4tF94@|}`k9O-CJkJ`cj66?dm67LWJcliNWc*>) z#XM~683X?2Z{5GU`qb9gGrzmKW9y{OgI9NK)=ruT9;7a?ZdYdJb1(kxs$*-G*M2!V z@0X4ugi#2SXx@Q*|6kib?dkJRg?ner+4Gn?lQDPhOZrg7y@TEwtku$i_lde$uTj)zpg@r(r@BC9(Td*!Act)THW4)fun05Ckr@6 zYozneZpl)y(LdGG3?!xVKvJ?)cRSZ$w2op@ghz7gl5RskeAo3vdm`F9Z?Lk~;-MA2 z3Mkmwq2}7G8@+=Iq=&*7x2`Z&nK{wM1?~?~Url0MU>Q(ks26jAyVbL7oGpaDM+&%J z)U&w2ybkI&T6NaG4SE|ucNij`g$}5q78hOjs%V_dOf!zzMJs%kv>cd$ptvfC>+`@M zzTp;jjugKWE0ASNa=S_m;XJGvOQ{;5V-u2J4t)&18OzX!x#G&5ziGWb`x$-TwY3IR z1S8r4qxTYY%EG#swQsBW;~ukq?QKTu?SC-2^}w1BRxASj)2yx~gfF(&(Ol2Y{;Hmv z`d7a~*vN%{dWubGK@Zh{4!Z+;<_HZ&rNy@`J$yv5xgkAM4wWQRl|W%gdz^J)_R;*3;h6D0@gY z4Bg?G3|*O#Xo*&}4lF=zrWM;#-5KTmRIqBjN!Gvk*d%>N)cC{xnDHLWRbgyDvW^0O zi0lhs2U`#N7-J4PEV0|{HS?r*8$NPFhq*d&E@2ro!Wx}r89FfQGm3?H_^11KGr_uB z#Q`L}4M8IO3zbq3ot{6rt>U13AP5Zy;L+wm(<>XgUet;vXmYrvUC1@TL$g%V+sM)E zmO}Erw~0!>bsuz}JY}f#Fl^~<`_uRd+{}8M6-u#ElFO9#;`(JSmbTY*#+9^w z+4?imBbCVu4REV1kJ@J)N>;)(6sR^q1+Vgcd8DX=4tWtY|UT7Ca5 z)@Ro9W4&Dr%2oZlmHSX1RX|)=4jiTmpQdMb^e4)0C!4guyFp^+yeEuOtD>o)K}FDs#E zU7W%DWV^5p-^~rm^I)SKl2_h9tsoo>_Sppy1l5P#;k7Jy8R7?(saZW&W?O zy;pJVB2JC)^L9mNb7mEEI4xY`C(2GY$)qM?*PwCQ2%Bh!un#F83-1^n!;XQOk0@!! zf7*IrXI%fzf%w+lCCR=DtXbiGg9iHsnwx0P6&~N&1K0OipS3d}hwE*$_t2_49>}L1 zU8jGw^T301T2I{PSaN`7DHkCcKIb@4&z*fqNpImMzvtKV>;#I&bFc}#+ca4$6}cH0 zGvK+)?)Ma%F?P+my6*|Wf|j9nu0j5Kw`Y!UBJBH%kdvMHgcgJ?Y%7RN92Y z|Bjrp638ihrXhly5=Kg)zI!u%N~7}=AXE&>ZG*puF2H&9&A)IbV9j2PHTxvi>_!P` zOs(A~w;uGeti(L$R3Sd$f-JWIgD#_sC>Cf!{wubj85CDE_5;s5^q{!Rn0KOEmnZ$9 zm2gGZ_{~p$iKm!BY&os-k}v;lW{>z+NPr zj;O5)q@#CpGduhhJ72z$dS3DsqGz<*)qY5bj(2(Vuv!_EA5qBqg=hfkV_20yq0suF z5gUO5<5rd!L-KzGyVeQPa#~+%!l4r(Q6|f!#JNP%8pOwFT;8H*gC~V~YDJL#PlTA| zTI-tn*XpTn!{-Kc$1yL1#tFZf==^$Vn8>o{d1-cgBQymzGEDh=T_$KXt7@aeEazA6 zuQ7WhtZC09zvc(VgR-|pVcyXHxAHY)DNf+W>}*xFiT~9b3Lv9cB~n{=r4puvENHbHTQ(n5v#73e^+J8N;!Rl zTQpadNp|xKPAX~YK+^-vy2X13WLWRQYAFjnx5bz$36?@25w*Y^*?ax&{o??FZ1qN_064xQ}mv1Q*cIqAHF_4SWBaJgcfX8M+o5VPR0uHv@&8=-lQ zdU`U_#vhz6q-?@9=eIu7@N@l>bFP}ah|MdSCM3wWc0m-;eI$=d=lEZ z46>Xs1qLg~AvWn6u;2?}|D)-7LFigH0=h-eMuprcuk(v_yqVhvnjww8=>y!$>`bef zEYiIxBGpI-?v3lLtO@s=&y-^K3r^}ge^3l=>e)oeXhl~$t+$5TxxBX>wB~;6Bi

zf-oYk^!*-lA1ncf4y)s;#ZTWx+?6=-S$K}QYs=5KEV#&J`?h#tTlcIY>=G|><9F4L zsI^~=o^(p7T{t4()Wr2aJ+#%g7&>@QU(wCfLOSrV`!ndB2p<*As9Mn8!^e&Hw)Y5e z+4$$MOMKg%Ex~729|N9_1sdR*lDh@k)0~hg9_p}(S?j-c*FvwF zzJ*z%&oIlHZ8~TYy}((K#}V+d{bVmymx-(CeX^frlIG3*{ZIGU@m+ST|8CrQ%$+6e zQI?pJASwKtpUeEQ4_7uWs}`KRCff@oJT%o_C|U1g*)`zQz*2M&&y938F2qc~^KZ+n zR8z35`~OdhyOClsdP|B6aCAq#L=#{^({J^xg;gGV@v~Np;vdVT!kw8zj zdzop^+}|NZN+j1r-*9-yo$A5?UCsp(_IxJ%dg@df=GA76;sM>d4O zgTyG0jnaAYodsf?N^KNfUSNgQLWTq5>a)IKJ;vea(XgDq$MyEtaqN#5EO-C$LdM;% zjIE5G6>N}azGXJn7Bht+_y$!M;}7nfhTZDT!La%sBtF_k~TY=EnKTO_fv z(s8Y;@_fDK>hg`vD~)BqEzFNA17B$utYd65MOAtkXbh5EVMs0xMFEM`gnh+;yn67U zf1$VpXS0J*ViWjRE5HY-$FUZN?dW4Ik4drfjpm;z_h8kKK>xae5_BWJ1ocX3w6I_3 zmme9h;>tcqUKS|5f;B^9B_0P0K$?}3%0}KvyPAW2;3=kjo{}ooIYxHcnPA~DuxLzWu4s$i^`z^4mvT&h=a+u{o#%0Y7Xkd11Q(|O=PQp|Y!tiX{{!ArdXVYc z5FW$9N?mjRj*tURcf6Okj`K=yMp?A!v91NKMb)M2R|dB7J6uBAhc0_s%=(zL9qSEU z*PoxLZdkv=#Ydj8wE61^9qszlXRF_Lg;P^Xddj7J=4a6IqBV`sjY+2XxEnkE@#z!L zOZ0k-k)Dv-!kPKB@<8=Wvh30?I{x~pEX zI@cl9MB8wr%0gMP5hK%CujC;1BpkUoY{%=B$?OvH%R`JQc$$xtQ}N_>W4$sBPYc+g zYA1HL^~!V{Gfef$OvIRl7_*IckoD0v+VL7;*PuT--xIrKJ~+i%A)%;jeS+tG%uSDj zuL0|cZy-L(XJe=gCB}GtSzP!PcS=VMSoRvqU3qG)@>S)H3OoO!CdX!?B?2E zIJ9SM=(}G8fiucty1tJm19E2&I`>nLIs7MAJAdV3CX#tE^MEX?`|QVRx!F-CoYH{& zXJAhdFQiXickH*!HNH|hb+4`1cn1rZ`fuwoLgSY149JIrU!t!l<>UvdK^!fPh2;_p zd^NBQ|8Ih0dPgVe=iVhgukg0Eju=QBKY(7qlhAmB-Z}cxbMUevoGC@@OM}*~Gtl9T z0XF7otVIHBua|bLaI8QpmS21lm^+&IM#;pTY=@<>Thsj!#WF2kDiq)E#F=e5&VG60 ztW-Sb#O?uO8DH_!lh7LC@{LpU+2^21$;D7<;BBtt^4ZuPo}+RsMmgT>qT)a zo}tc_YQ<l^FFR$+(*d;^XBjWjHK<*y+&hrgnnMW<4 z-a_d<8zdfOB`Eu5H&N8^OV_ps@&U4UX8|vEFGu=#SZsm62482eRB?y>2YFyka{x9( zRqkf@RrG606II&Mr@`t>wp(@XuP2;5^0*#R29yW}`$AI_g;y+gWl7?9QI* zPi!|H%bnQkgN02`qQK3+)M@9`rMQ;wYizl;Kj_NfWM{{lWKD( z>b(=@kF-sg#|1cL4Wvzl-YjQEnN>=WgxcJ!**-RYnz*1nO{@W(HL(I{`e|6Gy{J@(&@Q_D#?ECj?l~2w5XxaWo$Oykfi3@SOMV zF43O+;xtRAJk!LJ{z_?rm?C}z9mbDwFw2Mdg4i(jweW$T>fDAkjp`k~?NIJVJ^7%y zS4jp>(_9VsfGlt?{8ON8c_>>RaRl?>wJ!kv!(%OEtvSd0xWsaSdyzTA+u*p}OKmY0 zp5_ue^CZ@|Q9`>-gRNQP%<7Lk{ZA08gfMZKud}+*Q;pn-n$(DfGn6U=?L?#FvLrm4 zEG2j(JpCs$oHVZk(<8J7^u2zn@ZkYR39M>MFlrLSt3H-a@_4=GbL>QaB59yAG)la~ zb9?n99I2ibgTn`dQc=zg@S7 zy-B+zwa-6Jsi>bM_%bi*g+-$Fd3861TE7S?zCe5pbDY$NG}bS@1M+jq7pU>3xtII{ z@-s>&>PD+kabeNtNx$&*+G+PfPSZD4{Ox!J9mMPhp)l` zO)7teKSWl2Vc&*THKP^^k6KI#;hd%S4hRtucC&jr?{6NtcLT4A_YCulxhfGKynfz*}rQ&?WON!n7 z#|v7E0URuc?L`mEm<3LPx+i}814UE#nZGn$6V&a_&i?|vK(D2pX{@%zKU?fpS3ugW zjD@C1NcN$n<2!{{x#tN}$OP&lx)U}(>1UoGD{Du9@RE~=J11Rtu#M@H3}))LGKqF(_-c!+yKxgNE?H+A-NRq+u!k^ z)QFgki20J53&~C2M=o_}uOMW22Kf3r|3s%Q7FMQ>h!I{Ji`%%F$&l|fZws8GJMgje z7L9cc#;Ml>&np&g7k5avJG}nz#Tbu-|25d;i0VWycT{4`FLn}YJIkZBCMe73>w3%= zn!SzivrV(csgUFWF1K-hgnt=Y zlT@Oh{8xAnWPzuk>kOaiszx$oFsjU-2b$;}iFr&m*W6lQ%g4bdYBJ`)!+ohHHZkU0 z(xbwo(8oCuGp|pc4L&_Hk&g}?e0256Q-^qq$s?p4*qkOcZtnFrZ|?C^9^~T`IE=_; zdAlG9Xj zYZ&EyRjPv>xBw2dfkky$#iBJ9XWg1gAV#A;oCTolIC!q5{0ffV20xMDQ{6(YdCCKS zgy<cnDNr3D3Msqd35p(A}JMmidH$0fAyDAuj!Pi;(G zo{vJp*nV$#P5BdM3>|dRK=DBuK{{|~ZFp@OfwmkYo%#>xyy3D$pA#CW{|avGX(HZ~ zj6|t2MSUyT6VrE5FZ`d#%oHT=vNzT))enw!66u-ULi0mtn0qa>fuvsAi1tTKUxD+vG$$#!lPtw}$WR;(mC?>7ymvbT>1(}KTjYcu>+dFnf%0k~v9vp-6<2RKh zjNhTi@dGH$EjYhf4N02lPqN?@a`>lVo~k0p0QJ{(jAi$p<7UxZuqTw>deNWlzT^i_ zFcm9R=eW$#rxf|?Al4?x?5be{!WKTIaO5M##5MNG=P>gq@>keZg)yq{(ezPyHJ}H- zi8?BI^z)gyC3v})4<<0y2qRRiFg!L9#huske-xF~QGQeLA$kg9fz|^?xn;bb+VtQ2_{TTw@vFm{eSE(alnw5UFjwy8dmiX_o_9MXqw^hwGCdXM0< z(s$4FI;3$xVxM;13--VGko-(pg|@%e3R#>x2rLcqRY85BKr!~j#%V;Ik0cR9d#T!v zb&o@Pj!J$))#2jyOc$HG_53c!j*jC=%wz{|;rD%%LLDQ}$zvU%5BAO^eXi|~>L$iO z$0U^21ljARG&eYrERA2?C%Yko4}CL({P`A<6#p4fowIJCF&G8@cb~kXzrYU7L7Cgt z;^7V{vUP}Ly^4HdFvr8gUPLnovoN$&bXyeo10y_o;CRq(sV+}yUPT&Od0yIYGjnRG zrYBvGZ|^#u_?c3BJb8X$H@HleZ-T=)76=SjVAnKyK5=VL5yfZjlYcp2M+x>r4!^?4 zEaNhpSH%ObCJ1zf2FS-&I9bkn-X+Qkl%NtNIPTY>xAtMpruv9cwNnM%H|pFu2V;!J z*{`1f-|fS_GYKhWo{ITRdJ_8iaVCA4eqIu|^vj%zyr&Pi*=cd6Bqw=`k>&9np+!+_ zWEboDcn8xP5nk7!W&ys9aG8i#LKrr2qPd;WYLxPzJmszU_Mm(+nApYlj23`b5WB2| zT|VaC-=o(AA;r2!(XTyRVmPHau?(0Lq;c@=z-%@ADdbbc-+)GCil$o!1vplC26b4G z#Ii~y!p~ySOBCHMaMHVV!s{J+c=;8ImzVtb+~Av!O%g=k z1^%6P;6I6WgS_`c=`V`Gw8uf?WbWx0=|oaqag5UfR1;v~I959GKFl)Ao>=rD&Zu-^ z5GXCNz|`TVi%UgW{^!tm3HX=ZEb)FZraM}D26X!~&JnQQF_=DdrWQY6GTxbW`ju`$ z*gvNEE?}HM_Cz@fMRswGGbpD9ANNxk>me=O?tQ-G?1{tO(Sp%?r}&EBB3(|b1kM>N zN^@3HS!@E%6R0e6lO9=liondu@R7tFb`;I}N}5%3Ry)6s92^v4LdMQATo5nBXEf<}n(QoM@)!uhb` zy+sHyiB1|fn87#fip=M+PQI+Dh?&apg!-22Xfbr@c<6=b%f1|~FFhaROQPnR646>h zNjHZ+J}GEZHQs zuM;(gglEBrQqo{or#Tdq>xXneE;E8E56W*~MQ~D%dtnz77^4V#$)+PHZy3sV8sGtf zXpMei4Y zq)v@X$R()#GKy1&?`Iu%MV`aX(~SPoHVr##ZWnuY*J1j)^9nSH_l~X2$>F?NIj|Yd z;~LE(w+H)wB_}7&*V4nvqfZCrEx|MkSxqwgkK0Fev5as0{h+5n(^$1-y6o6qf|uRrJ4 zfH%W3gFb`!PqgkZ(ID+=x76BT&|g`*Iz<3V#?q{@K3+7MoA90#rOm_V8%^23Rx(&}SQ9PK({5O>FV7N}@rY4*#bkoSr3wD0+zy^& z9VoQP-nh&uYE9{5$4<_lGvc}u_xX^()p=pVRezZ^Fi`X-oz^}<<&WL zU|Gp{;=Jie`2HeCWp|O_ni~#Tp#0xreD|#Irf1jFEpDHW@`X!<5={=5>gn%F6&Qb0 zD%ac>zl~i}_|0hG2*fU*0IUADF1@#=?UGpb(xs9g4P5&3k8ZnUOd5S@R}8HgWY5pk zUto45EF_Mp{udf?Cs4xdfjajN$7pfx>DK=3mmQvWpx>O{&jQcbpR?O9a)#aZ@{6NO zErz#(%C&L*j}PgCxBa2$B3Gmf4)1y1f%PO&*Aue217o&5@33FKZ5w~b%;M!qFSQ;H zDgk}am{)vJC}zRoLkCNuPtO{|u*Y`?wA&7r*e~#IlJ{U8mvnNp_dd_z?&16c z-CR^uzhU6AeF4y9WIO6<0vjK$(@C(|hy`BVg|8CD!LN!+zC6*^%@41}u9PeI%&!+u z_?_ZY{%vBrf3^6q-v(@x*L!%w^x_0R^_g_cbJVw^uyPoWMw|2t6zmW%wkwlO7VJ{| z1IZ>u{s+7u2=gsKYzF72!wnx|r*J^`+XTrp_A#JXU7ga>A_lu97B~v;?|Q5e`+y%y z_R2&9AIANCeGKv4_zyp?fehO~epawfke`)B@QD%)Ud9NV>*u8f4lPQ%Vgz2IZa&?? zJc@HFE4*bdHR50_#AU1HKzW_CKQ9k(>TZe!Qs1Dr?g4E>Pc%0@8EPkVSn%c4y!79) zt~h9&!^548r1C|n!?TD77Ig}K(fdu%)eCcBr@VnL??GNy5iij2W?hul_uchz_^?LF za9rp2F(X@HDNCOpP`RC>8{X}WW#5;Q4WoY8=E(9s_e2}`Rh-F?p$)RM?@Ob8Sa=yG ztqzmdahXGyijb~TyoKtuzGGR&%G6kKQ)*0cqhs_5$SP}=raao7oxa_X;^I#4Nx9Eu zPYQr5_o@Hhj0^sy8GirwGrk1H!4z)omq5PwC)%0H^a9$0D?7eat;er>#=%FuRRAIf zSE|V8fL)Ost6|}LoO`NY*7fQejB|(l z`9Sz)HsvV%sC=*Jic3gvPAQu*KUos4ZcPxbKDu}x--zDT7Mz6iM}s-|Zw+#$YjQcX z7xJZRa@kO3*$WP6yQS0J;-Lwkf%2W#rNC;$-phhe6HJ2z_i$eZaGTBGiA4Lp zh2^*I6!k|op`_?_M?pun7>yoW1v&WA_Itqn-ZexyCJ&7$%f`sbL!P66q#j=yl#_;d z^9Y2ufiu9;u_qe>g+HXgO1mMSoA8dm0exkmoh@~v-l$*GH%GNZwKRe3c?rjT|E2wt zYO@WxVLlE4Jhr7pLYXPB)lR+b2-490o=?@5X-f?qdNXI{0~&seQ)9j+v$yFv?fXei z&Xr!1WsEhthEI930Bq-L@})r@IE6`0_!`6gM9M1?_wyTZp$Ukzrz6)IYj{OZI7MH* za!|;L24(_dhK_XD)iLv8Xu^ys(w2=uJuDt1%@JBLgZ@n@KYc4~nMM6k9<(St=YZcY z%LNMdONwTmwk$^CuXL?UfPcG=-twOg`3n=7LBXvo!^hbxCAVe1Ee)gDnu9mZfG6}zK zyF>9Mih6Lwhk?Vd9F4Ogr_~x@dWqr>RNUkTQ4ZMlK}IxBEo(z43VesTD)#97kj64Y zqMWL~(^RY`UfZuL3wd{?0K(B6bh&h{qsAy9jCeOJhW{Q{+< ztNCi2yVUP29x%K~lD~e4WCX^=8q7&zJDy)2I$Z1!TSVSsz#b$dUmQABGQ~@M zp`*9vr}rwRS@6VUyH`m;s6m*BP>*mVLIXl0LL)*0!jTB|2on)%5T@9!=R-MvJfwfK zQi_IkF!iwGHKV=E!lMr9*>8t1X7X6tNcHLvW)Hf@atg7J9+I0Pd^SBScYjN@p}j;q z^c(Sa4NZ~mK}q*I@_ZX`mV&cMzE+$i;w;m58D}GLmhPKUeGf{!*D=nw0cQrBCHY!$ zrpMVR-({R>aAt&dm1$bIlrcWj?LR2_v+w5jEmie*HCob+*JyvOw$g9>*0hkhUrvNv zQM-W3FT)nk%0hB4G+!#Q>c37pFTn9(K)W`8xNm-n(6s&_3(UQDxqhO~C30Cckl;dy91E!26VfbBr<{8`B4`L;;4 zk4XDU)p4%HP-PvI_r=jHy<)>do2TVGl#B3wsba&b)fEq~-889^&0Dp$%Cw|n;~EBC zy~vN`BOK027NWf=T#9RoLh>bgzB_VV9|^11Rc~U2E|+6N^|}Y{cCqRW>vM73|M0r1 zT31!&rm9ud8#mycv)0wDt9YpD4!rUm%IDKDz!i#wLL}7TK|Ah8;hI7{u78g(Ch|NQ z*OV?cat%q3dOa)>Zmil+S+j1&3{%OXMdgcTmMPzze3nE1$q)tKoz=;cL|KstX z1rWM37^C*57r+a>nqeG;pX0qogy?NNh4lgxC`3jYgr@ybOiR}v>Y0v0<8T&*ka*R4 zy52p4MI&^chaM?HcYP|0Mc9-vo*57xe=mm(L)i7eR5lzTi=NHm5LT~S&DK?{W7c(O z@JzIgikmmND2$^w;9zW5>L1^F525|b7i`ZWWCwO9-ACcqn@cB9Xu7}Yav?%i_3<$^ ztpA_K)zBNiSPfAYHDnKdZKrsw;cz&#Zw-fRUL+i8-}^lM+UNXennX^ z`?6Ub8@HY-AN4S`bvC=vdu`NLwpO73N$5XLraxC1?qi=;G5z9|;q&XUF-S93A;Ho0 zOyA>Tde26te+A*(2Vtjzueb%~g`sZNJN#)Lpgrg$&`w-9Z@Y*%( zp+z-p;nIAQV}S{$rlOj4n>N&}T~@WBrgWOAyyQ+6k50s+$zC6y4#$%mFK}v^sEtt` z%A0-w64H0$2wySw&&YYk7N)0z?&n0pjE%V&8(Hp6HIY4e^3*9d+l@Q;S{qxu + * + * 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 NuttX 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 "board_dma_map.h" + +#include + +#ifndef __ASSEMBLY__ +# include +#endif + +#include "stm32_rcc.h" +#include "stm32_sdmmc.h" + +/* Clocking *************************************************************************/ +/* The board provides the following clock sources: + * + * X1: 24 MHz crystal for HSE + * + * So we have these clock source available within the STM32 + * + * HSI: 16 MHz RC factory-trimmed internal oscillator + * HSE: 24 MHz crystal for HSE + */ +#define STM32_BOARD_XTAL 24000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL +#define STM32_LSE_FREQUENCY 0 + +#define STM32_HSEBYP_ENABLE 1 + +/* Main PLL Configuration. + * + * PLL source is HSE = 24,000,000 + * + * PLL_VCOx = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * Subject to: + * + * 1 <= PLLM <= 63 + * 4 <= PLLN <= 512 + * 150 MHz <= PLL_VCOL <= 420MHz + * 192 MHz <= PLL_VCOH <= 836MHz + * + * SYSCLK = PLL_VCO / PLLP + * CPUCLK = SYSCLK / D1CPRE + * Subject to + * + * PLLP1 = {2, 4, 6, 8, ..., 128} + * PLLP2,3 = {2, 3, 4, ..., 128} + * CPUCLK <= 480 MHz + */ +#define STM32_PLLCFG_PLLSRC RCC_PLLCKSELR_PLLSRC_HSE + +/* PLL1, wide 4 - 8 MHz input, enable DIVP, DIVQ, DIVR + * + * PLL1_VCO = (24,000,000 / 2) * 80 = 960 MHz + * + * PLL1P = PLL1_VCO/2 = 960 MHz / 2 = 480 MHz + * PLL1Q = PLL1_VCO/4 = 960 MHz / 4 = 240 MHz + * PLL1R = PLL1_VCO/8 = 960 MHz / 8 = 120 MHz + */ +#define STM32_PLLCFG_PLL1CFG (RCC_PLLCFGR_PLL1VCOSEL_WIDE|RCC_PLLCFGR_PLL1RGE_4_8_MHZ|RCC_PLLCFGR_DIVP1EN|RCC_PLLCFGR_DIVQ1EN|RCC_PLLCFGR_DIVR1EN) +#define STM32_PLLCFG_PLL1M RCC_PLLCKSELR_DIVM1(2) +#define STM32_PLLCFG_PLL1N RCC_PLL1DIVR_N1(80) +#define STM32_PLLCFG_PLL1P RCC_PLL1DIVR_P1(2) +#define STM32_PLLCFG_PLL1Q RCC_PLL1DIVR_Q1(4) +#define STM32_PLLCFG_PLL1R RCC_PLL1DIVR_R1(8) + +#define STM32_VCO1_FREQUENCY ((STM32_HSE_FREQUENCY / 2) * 80) +#define STM32_PLL1P_FREQUENCY (STM32_VCO1_FREQUENCY / 2) +#define STM32_PLL1Q_FREQUENCY (STM32_VCO1_FREQUENCY / 4) +#define STM32_PLL1R_FREQUENCY (STM32_VCO1_FREQUENCY / 8) + +/* PLL2 */ +#define STM32_PLLCFG_PLL2CFG (RCC_PLLCFGR_PLL2VCOSEL_WIDE|RCC_PLLCFGR_PLL2RGE_4_8_MHZ|RCC_PLLCFGR_DIVP2EN|RCC_PLLCFGR_DIVQ2EN|RCC_PLLCFGR_DIVR2EN) +#define STM32_PLLCFG_PLL2M RCC_PLLCKSELR_DIVM2(4) +#define STM32_PLLCFG_PLL2N RCC_PLL2DIVR_N2(32) +#define STM32_PLLCFG_PLL2P RCC_PLL2DIVR_P2(2) +#define STM32_PLLCFG_PLL2Q RCC_PLL2DIVR_Q2(2) +#define STM32_PLLCFG_PLL2R RCC_PLL2DIVR_R2(2) + +#define STM32_VCO2_FREQUENCY ((STM32_HSE_FREQUENCY / 4) * 32) +#define STM32_PLL2P_FREQUENCY (STM32_VCO2_FREQUENCY / 2) +#define STM32_PLL2Q_FREQUENCY (STM32_VCO2_FREQUENCY / 2) +#define STM32_PLL2R_FREQUENCY (STM32_VCO2_FREQUENCY / 2) + +/* PLL3 */ +#define STM32_PLLCFG_PLL3CFG (RCC_PLLCFGR_PLL3VCOSEL_WIDE|RCC_PLLCFGR_PLL3RGE_4_8_MHZ|RCC_PLLCFGR_DIVQ3EN) +#define STM32_PLLCFG_PLL3M RCC_PLLCKSELR_DIVM3(4) +#define STM32_PLLCFG_PLL3N RCC_PLL3DIVR_N3(32) +#define STM32_PLLCFG_PLL3P RCC_PLL3DIVR_P3(2) +#define STM32_PLLCFG_PLL3Q RCC_PLL3DIVR_Q3(4) +#define STM32_PLLCFG_PLL3R RCC_PLL3DIVR_R3(2) + +#define STM32_VCO3_FREQUENCY ((STM32_HSE_FREQUENCY / 4) * 32) +#define STM32_PLL3P_FREQUENCY (STM32_VCO3_FREQUENCY / 2) +#define STM32_PLL3Q_FREQUENCY (STM32_VCO3_FREQUENCY / 4) +#define STM32_PLL3R_FREQUENCY (STM32_VCO3_FREQUENCY / 2) + +/* SYSCLK = PLL1P = 480MHz + * CPUCLK = SYSCLK / 1 = 480 MHz + */ +#define STM32_RCC_D1CFGR_D1CPRE (RCC_D1CFGR_D1CPRE_SYSCLK) +#define STM32_SYSCLK_FREQUENCY (STM32_PLL1P_FREQUENCY) +#define STM32_CPUCLK_FREQUENCY (STM32_SYSCLK_FREQUENCY / 1) + +/* Configure Clock Assignments */ + +/* AHB clock (HCLK) is SYSCLK/2 (240 MHz max) + * HCLK1 = HCLK2 = HCLK3 = HCLK4 = 240 + */ +#define STM32_RCC_D1CFGR_HPRE RCC_D1CFGR_HPRE_SYSCLKd2 /* HCLK = SYSCLK / 2 */ +#define STM32_ACLK_FREQUENCY (STM32_CPUCLK_FREQUENCY / 2) /* ACLK in D1, HCLK3 in D1 */ +#define STM32_HCLK_FREQUENCY (STM32_CPUCLK_FREQUENCY / 2) /* HCLK in D2, HCLK4 in D3 */ +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/2 (120 MHz) */ +#define STM32_RCC_D2CFGR_D2PPRE1 RCC_D2CFGR_D2PPRE1_HCLKd2 /* PCLK1 = HCLK / 2 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB2 clock (PCLK2) is HCLK/2 (120 MHz) */ +#define STM32_RCC_D2CFGR_D2PPRE2 RCC_D2CFGR_D2PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB3 clock (PCLK3) is HCLK/2 (120 MHz) */ +#define STM32_RCC_D1CFGR_D1PPRE RCC_D1CFGR_D1PPRE_HCLKd2 /* PCLK3 = HCLK / 2 */ +#define STM32_PCLK3_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB4 clock (PCLK4) is HCLK/4 (120 MHz) */ +#define STM32_RCC_D3CFGR_D3PPRE RCC_D3CFGR_D3PPRE_HCLKd2 /* PCLK4 = HCLK / 2 */ +#define STM32_PCLK4_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timer clock frequencies */ + +/* Timers driven from APB1 will be twice PCLK1 */ +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* Timers driven from APB2 will be twice PCLK2 */ +#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM15_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM16_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM17_CLKIN (2*STM32_PCLK2_FREQUENCY) + +/* Kernel Clock Configuration + * Note: look at Table 54 in ST Manual + */ +#define STM32_RCC_D2CCIP2R_I2C123SRC RCC_D2CCIP2R_I2C123SEL_HSI /* I2C123 clock source */ +#define STM32_RCC_D3CCIPR_I2C4SRC RCC_D3CCIPR_I2C4SEL_HSI /* I2C4 clock source */ +#define STM32_RCC_D2CCIP1R_SPI123SRC RCC_D2CCIP1R_SPI123SEL_PLL2 /* SPI123 clock source */ +#define STM32_RCC_D2CCIP1R_SPI45SRC RCC_D2CCIP1R_SPI45SEL_PLL2 /* SPI45 clock source */ +#define STM32_RCC_D3CCIPR_SPI6SRC RCC_D3CCIPR_SPI6SEL_PLL2 /* SPI6 clock source */ +#define STM32_RCC_D2CCIP2R_USBSRC RCC_D2CCIP2R_USBSEL_PLL3 /* USB 1 and 2 clock source */ +#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */ +#define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */ + +#define STM32_FDCANCLK STM32_HSE_FREQUENCY + +/* UART clock selection */ +/* reset to default to overwrite any changes done by any bootloader */ + +#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC +#define STM32_RCC_D2CCIP2R_USART16_SEL RCC_D2CCIP2R_USART16SEL_RCC + +/* FLASH wait states */ +#define BOARD_FLASH_WAITSTATES 2 + +/* SDMMC definitions ********************************************************/ +/* Init 400kHz, freq = PLL1Q/(2*div) div = PLL1Q/(2*freq) */ +#define STM32_SDMMC_INIT_CLKDIV (300 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) + +/* 25 MHz Max for now, 25 mHZ = PLL1Q/(2*div), div = PLL1Q/(2*freq) + * div = 4.8 = 240 / 50, So round up to 5 for default speed 24 MB/s + */ +#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA) +# define STM32_SDMMC_MMCXFR_CLKDIV (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_MMCXFR_CLKDIV (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif +#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA) +# define STM32_SDMMC_SDXFR_CLKDIV (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_SDXFR_CLKDIV (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif + +#define STM32_SDMMC_CLKCR_EDGE STM32_SDMMC_CLKCR_NEGEDGE + + +/* UART/USART */ +#define GPIO_USART2_TX GPIO_USART2_TX_2 /* PD5 */ +#define GPIO_USART2_RX GPIO_USART2_RX_2 /* PD6 */ +#define GPIO_USART2_CTS GPIO_USART2_CTS_NSS_2 /* PD3 */ +#define GPIO_USART2_RTS GPIO_USART2_RTS_2 /* PD4 */ + +#define GPIO_USART3_TX GPIO_USART3_TX_3 /* PD8 */ +#define GPIO_USART3_RX GPIO_USART3_RX_3 /* PD9 */ +#define GPIO_USART3_CTS GPIO_USART3_CTS_NSS_2 /* PD11 */ +#define GPIO_USART3_RTS GPIO_USART3_RTS_2 /* PD12 */ + +#define GPIO_UART4_TX GPIO_UART4_TX_2 /* PA0 */ +#define GPIO_UART4_RX GPIO_UART4_RX_2 /* PA1 */ + +#define GPIO_USART6_TX 0 /* USART6 is RX-only */ +#define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC7 */ + +#define GPIO_UART7_TX GPIO_UART7_TX_3 /* PE8 */ +#define GPIO_UART7_RX GPIO_UART7_RX_3 /* PE7 */ + +#define GPIO_UART8_TX GPIO_UART8_TX_1 /* PE1 */ +#define GPIO_UART8_RX GPIO_UART8_RX_1 /* PE0 */ + + +/* CAN */ +#define GPIO_CAN1_RX GPIO_CAN1_RX_3 /* PD0 */ +#define GPIO_CAN1_TX GPIO_CAN1_TX_3 /* PD1 */ + +#define GPIO_CAN2_RX GPIO_CAN2_RX_1 /* PB12 */ +#define GPIO_CAN2_TX GPIO_CAN2_TX_1 /* PB13 */ + + +/* SPI */ +#define ADJ_SLEW_RATE(p) (((p) & ~GPIO_SPEED_MASK) | (GPIO_SPEED_2MHz)) + +#define GPIO_SPI1_SCK ADJ_SLEW_RATE(GPIO_SPI1_SCK_1) /* PA5 */ +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 /* PA6 */ +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 /* PA7 */ + +#define GPIO_SPI2_SCK ADJ_SLEW_RATE(GPIO_SPI2_SCK_3) /* PB10 */ +#define GPIO_SPI2_MISO GPIO_SPI2_MISO_1 /* PB14 */ +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1 /* PB15 */ + +#define GPIO_SPI5_SCK ADJ_SLEW_RATE(GPIO_SPI5_SCK_1) /* PF7 */ +#define GPIO_SPI5_MISO GPIO_SPI5_MISO_1 /* PF8 */ +#define GPIO_SPI5_MOSI GPIO_SPI5_MOSI_2 /* PF9 */ + + +/* I2C */ +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 /* PB8 */ +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 /* PB9 */ + +#define GPIO_I2C3_SCL GPIO_I2C3_SCL_2 /* PH7 */ +#define GPIO_I2C3_SDA GPIO_I2C3_SDA_2 /* PH8 */ + +#define GPIO_I2C4_SCL GPIO_I2C4_SCL_4 /* PB6 */ +#define GPIO_I2C4_SDA GPIO_I2C4_SDA_4 /* PB7 */ diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/include/board_dma_map.h b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/include/board_dma_map.h new file mode 100755 index 000000000000..99a2fe3ba9b9 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/include/board_dma_map.h @@ -0,0 +1,44 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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 + +// DMAMUX1 +#define DMAMAP_SPI1_RX DMAMAP_DMA12_SPI1RX_0 /* DMA1:37 */ +#define DMAMAP_SPI1_TX DMAMAP_DMA12_SPI1TX_0 /* DMA1:38 */ + +#define DMAMAP_USART6_RX DMAMAP_DMA12_USART6RX_1 /* DMA1:71 */ +#define DMAMAP_USART6_TX DMAMAP_DMA12_USART6TX_1 /* DMA1:72 */ + +#define DMAMAP_SPI5_RX DMAMAP_DMA12_SPI5RX_0 /* DMA1:83 */ +#define DMAMAP_SPI5_TX DMAMAP_DMA12_SPI5TX_0 /* DMA1:84 */ diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/nsh/defconfig b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/nsh/defconfig new file mode 100644 index 000000000000..c9ec46ed878e --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/nsh/defconfig @@ -0,0 +1,260 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_DISABLE_ENVIRON is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_DISABLE_PTHREAD is not set +# CONFIG_MMCSD_HAVE_CARDDETECT is not set +# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_MMCSD_SPI is not set +# CONFIG_NSH_DISABLEBG is not set +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_DATE is not set +# CONFIG_NSH_DISABLE_DF is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_ENV is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXPORT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MV is not set +# CONFIG_NSH_DISABLE_PS is not set +# CONFIG_NSH_DISABLE_PSSTACKUSAGE is not set +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_RM is not set +# CONFIG_NSH_DISABLE_RMDIR is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_SET is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_SOURCE is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_TIME is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H743II=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=768 +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_DCACHE=y +CONFIG_ARMV7M_DTCM=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARM_MPU_EARLY_RESET=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=95150 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x1124 +CONFIG_CDCACM_PRODUCTSTR="3DRControlZeroH7OEM_revG" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_VENDORSTR="3DR" +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_MEMFAULT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DEV_FIFO_SIZE=0 +CONFIG_DEV_PIPE_MAXSIZE=1024 +CONFIG_DEV_PIPE_SIZE=70 +CONFIG_EXPERIMENTAL=y +CONFIG_STM32H7_PWR_EXTERNAL_SOURCE_SUPPLY=y +CONFIG_FAT_DMAMEMORY=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_LFN_ALIAS_HASH=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FS_BINFS=y +CONFIG_FS_CROMFS=y +CONFIG_FS_FAT=y +CONFIG_FS_FATTIME=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y +CONFIG_FS_PROCFS_MAX_TASKS=64 +CONFIG_FS_PROCFS_REGISTER=y +CONFIG_FS_ROMFS=y +CONFIG_GRAN=y +CONFIG_GRAN_INTR=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_I2C=y +CONFIG_I2C_RESET=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_INIT_ENTRYPOINT="nsh_main" +CONFIG_INIT_STACKSIZE=3194 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_MAX_EXITFUNS=1 +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SDIO=y +CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y +CONFIG_MM_REGIONS=4 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=y +CONFIG_MTD_PROGMEM=y +CONFIG_MTD_RAMTRON=y +CONFIG_NAME_MAX=40 +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=15 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_QUOTE=y +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_VARS=y +CONFIG_OTG_ID_GPIO_DISABLE=y +CONFIG_PIPES=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 +CONFIG_RAMTRON_SETSPEED=y +CONFIG_RAM_SIZE=245760 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=y +CONFIG_READLINE_CMD_HISTORY=y +CONFIG_READLINE_TABCOMPLETION=y +CONFIG_RTC_DATETIME=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_INSTRUMENTATION_SWITCH=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDMMC1_SDIO_PULLUP=y +CONFIG_SEM_PREALLOCHOLDERS=32 +CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SIG_SIGWORK=4 +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_STM32H7_ADC1=y +CONFIG_STM32H7_ADC3=y +CONFIG_STM32H7_BBSRAM=y +CONFIG_STM32H7_BBSRAM_FILES=5 +CONFIG_STM32H7_BDMA=y +CONFIG_STM32H7_BKPSRAM=y +CONFIG_STM32H7_DMA1=y +CONFIG_STM32H7_DMA2=y +CONFIG_STM32H7_DMACAPABLE=y +CONFIG_STM32H7_FLOWCONTROL_BROKEN=y +CONFIG_STM32H7_I2C1=y +CONFIG_STM32H7_I2C3=y +CONFIG_STM32H7_I2C4=y +CONFIG_STM32H7_I2C_DYNTIMEO=y +CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10 +CONFIG_STM32H7_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_RTC=y +CONFIG_STM32H7_RTC_HSECLOCK=y +CONFIG_STM32H7_RTC_MAGIC_REG=1 +CONFIG_STM32H7_SAVE_CRASHDUMP=y +CONFIG_STM32H7_SDMMC1=y +CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_SPI1=y +CONFIG_STM32H7_SPI1_DMA=y +CONFIG_STM32H7_SPI1_DMA_BUFFER=1024 +CONFIG_STM32H7_SPI2=y +CONFIG_STM32H7_SPI5=y +CONFIG_STM32H7_SPI5_DMA=y +CONFIG_STM32H7_SPI5_DMA_BUFFER=1024 +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_TIM2=y +CONFIG_STM32H7_TIM3=y +CONFIG_STM32H7_TIM4=y +CONFIG_STM32H7_TIM8=y +CONFIG_STM32H7_UART4=y +CONFIG_STM32H7_UART7=y +CONFIG_STM32H7_UART8=y +CONFIG_STM32H7_USART2=y +CONFIG_STM32H7_USART3=y +CONFIG_STM32H7_USART6=y +CONFIG_STM32H7_USART_BREAKS=y +CONFIG_STM32H7_USART_INVERT=y +CONFIG_STM32H7_USART_SINGLEWIRE=y +CONFIG_STM32H7_USART_SWAP=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_TTY_SIGINT=y +CONFIG_TTY_SIGTSTP=y +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_RXBUFSIZE=600 +CONFIG_UART4_TXBUFSIZE=1500 +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_RXBUFSIZE=600 +CONFIG_UART7_TXBUFSIZE=1500 +CONFIG_UART8_BAUD=57600 +CONFIG_UART8_RXBUFSIZE=600 +CONFIG_UART8_SERIAL_CONSOLE=y +CONFIG_UART8_TXBUFSIZE=1500 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_TXBUFSIZE=1500 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_IFLOWCONTROL=y +CONFIG_USART3_OFLOWCONTROL=y +CONFIG_USART3_RXBUFSIZE=600 +CONFIG_USART3_TXBUFSIZE=3000 +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_RXBUFSIZE=600 +CONFIG_USART6_TXBUFSIZE=1500 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/scripts/bootloader_script.ld b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/scripts/bootloader_script.ld new file mode 100755 index 000000000000..3fb4cc1f33ce --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/scripts/bootloader_script.ld @@ -0,0 +1,221 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2020 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/* The board uses an STM32H743II has 2048Kb of main FLASH memory. + * The flash memory is partitioned into a User Flash memory and a System + * Flash memory. Each of these memories has two banks: + * + * 1) User Flash memory: + * + * Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each + * Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each + * + * 2) System Flash memory: + * + * Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector + * Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector + * + * 3) User option bytes for user configuration, only in Bank 1. + * + * In the STM32H743II, two different boot spaces can be selected through + * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and + * BOOT_ADD1 option bytes: + * + * 1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0]. + * ST programmed value: Flash memory at 0x0800:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x1FF0:0000 + * + * There's a switch on board, the BOOT0 pin is at ground so by default, + * the STM32 will boot to address 0x0800:0000 in FLASH unless the switch is + * drepresed, then the boot will be from 0x1FF0:0000 + * + * The STM32H743ZI also has 1024Kb of data SRAM. + * SRAM is split up into several blocks and into three power domains: + * + * 1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with + * 0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus + * + * 1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000 + * + * The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit + * DTCM ports. The DTCM-RAM could be used for critical real-time + * data, such as interrupt service routines or stack / heap memory. + * Both DTCM-RAMs can be used in parallel (for load/store operations) + * thanks to the Cortex-M7 dual issue capability. + * + * 1.2) 64Kb of ITCM-RAM beginning at address 0x0000:0000 + * + * This RAM is connected to ITCM 64-bit interface designed for + * execution of critical real-times routines by the CPU. + * + * 2) AXI SRAM (D1 domain) accessible by all system masters except BDMA + * through D1 domain AXI bus matrix + * + * 2.1) 512Kb of SRAM beginning at address 0x2400:0000 + * + * 3) AHB SRAM (D2 domain) accessible by all system masters except BDMA + * through D2 domain AHB bus matrix + * + * 3.1) 128Kb of SRAM1 beginning at address 0x3000:0000 + * 3.2) 128Kb of SRAM2 beginning at address 0x3002:0000 + * 3.3) 32Kb of SRAM3 beginning at address 0x3004:0000 + * + * SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000 + * + * 4) AHB SRAM (D3 domain) accessible by most of system masters + * through D3 domain AHB bus matrix + * + * 4.1) 64Kb of SRAM4 beginning at address 0x3800:0000 + * 4.1) 4Kb of backup RAM beginning at address 0x3880:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + */ + +MEMORY +{ + ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 128K + DTCM1_RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + DTCM2_RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 64K + AXI_SRAM (rwx) : ORIGIN = 0x24000000, LENGTH = 512K /* D1 domain AXI bus */ + SRAM1 (rwx) : ORIGIN = 0x30000000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM2 (rwx) : ORIGIN = 0x30020000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM3 (rwx) : ORIGIN = 0x30040000, LENGTH = 32K /* D2 domain AHB bus */ + SRAM4 (rwx) : ORIGIN = 0x38000000, LENGTH = 64K /* D3 domain */ + BKPRAM (rwx) : ORIGIN = 0x38800000, LENGTH = 4K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + } > FLASH + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > FLASH + + .ARM.extab : { + *(.ARM.extab*) + } > FLASH + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > FLASH + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > AXI_SRAM AT > FLASH + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > AXI_SRAM + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } + + .ramfunc : { + _sramfuncs = .; + *(.ramfunc .ramfunc.*) + . = ALIGN(4); + _eramfuncs = .; + } > ITCM_RAM AT > FLASH + + _framfuncs = LOADADDR(.ramfunc); +} diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/scripts/script.ld b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/scripts/script.ld new file mode 100755 index 000000000000..02e763a790fb --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/scripts/script.ld @@ -0,0 +1,228 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2021 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/* The board uses an STM32H743XIH6 and has 2048Kb of main FLASH memory. + * The flash memory is partitioned into a User Flash memory and a System + * Flash memory. Each of these memories has two banks: + * + * 1) User Flash memory: + * + * Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each + * Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each + * + * 2) System Flash memory: + * + * Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector + * Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector + * + * 3) User option bytes for user configuration, only in Bank 1. + * + * In the STM32H743II, two different boot spaces can be selected through + * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and + * BOOT_ADD1 option bytes: + * + * 1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0]. + * ST programmed value: Flash memory at 0x0800:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x1FF0:0000 + * + * There's a switch on board, the BOOT0 pin is at ground so by default, + * the STM32 will boot to address 0x0800:0000 in FLASH unless the switch is + * drepresed, then the boot will be from 0x1FF0:0000 + * + * The STM32H743ZI also has 1024Kb of data SRAM. + * SRAM is split up into several blocks and into three power domains: + * + * 1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with + * 0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus + * + * 1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000 + * + * The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit + * DTCM ports. The DTCM-RAM could be used for critical real-time + * data, such as interrupt service routines or stack / heap memory. + * Both DTCM-RAMs can be used in parallel (for load/store operations) + * thanks to the Cortex-M7 dual issue capability. + * + * 1.2) 64Kb of ITCM-RAM beginning at address 0x0000:0000 + * + * This RAM is connected to ITCM 64-bit interface designed for + * execution of critical real-times routines by the CPU. + * + * 2) AXI SRAM (D1 domain) accessible by all system masters except BDMA + * through D1 domain AXI bus matrix + * + * 2.1) 512Kb of SRAM beginning at address 0x2400:0000 + * + * 3) AHB SRAM (D2 domain) accessible by all system masters except BDMA + * through D2 domain AHB bus matrix + * + * 3.1) 128Kb of SRAM1 beginning at address 0x3000:0000 + * 3.2) 128Kb of SRAM2 beginning at address 0x3002:0000 + * 3.3) 32Kb of SRAM3 beginning at address 0x3004:0000 + * + * SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000 + * + * 4) AHB SRAM (D3 domain) accessible by most of system masters + * through D3 domain AHB bus matrix + * + * 4.1) 64Kb of SRAM4 beginning at address 0x3800:0000 + * 4.1) 4Kb of backup RAM beginning at address 0x3880:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + */ + +MEMORY +{ + ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + FLASH (rx) : ORIGIN = 0x08020000, LENGTH = 1920K + + DTCM1_RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + DTCM2_RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 64K + AXI_SRAM (rwx) : ORIGIN = 0x24000000, LENGTH = 512K /* D1 domain AXI bus */ + SRAM1 (rwx) : ORIGIN = 0x30000000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM2 (rwx) : ORIGIN = 0x30020000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM3 (rwx) : ORIGIN = 0x30040000, LENGTH = 32K /* D2 domain AHB bus */ + SRAM4 (rwx) : ORIGIN = 0x38000000, LENGTH = 64K /* D3 domain */ + BKPRAM (rwx) : ORIGIN = 0x38800000, LENGTH = 4K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + } > FLASH + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > FLASH + + + .ARM.extab : { + *(.ARM.extab*) + } > FLASH + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > FLASH + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + + /* Pad out last section as the STM32H7 Flash write size is 256 bits. 32 bytes */ + . = ALIGN(16); + FILL(0xffff) + . += 16; + } > AXI_SRAM AT > FLASH = 0xffff + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > AXI_SRAM + + /* Emit the the D3 power domain section for locating BDMA data */ + + .sram4_reserve (NOLOAD) : + { + *(.sram4) + . = ALIGN(4); + _sram4_heap_start = ABSOLUTE(.); + } > SRAM4 + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/CMakeLists.txt b/boards/3dr/ctrl-zero-h7-oem-revg/src/CMakeLists.txt new file mode 100755 index 000000000000..6a7d2ce306f7 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/CMakeLists.txt @@ -0,0 +1,65 @@ +############################################################################ +# +# Copyright (c) 2021 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. +# +############################################################################ +if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") + add_library(drivers_board + bootloader_main.c + usb.c + ) + target_link_libraries(drivers_board + PRIVATE + nuttx_arch + nuttx_drivers + bootloader + ) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common) + +else() + add_library(drivers_board + i2c.cpp + init.c + led.c + spi.cpp + timer_config.cpp + usb.c + ) + + target_link_libraries(drivers_board + PRIVATE + arch_io_pins + arch_spi + drivers__led + nuttx_arch + nuttx_drivers + px4_layer + ) +endif() diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/board_config.h b/boards/3dr/ctrl-zero-h7-oem-revg/src/board_config.h new file mode 100755 index 000000000000..bcc8bba1a619 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/board_config.h @@ -0,0 +1,187 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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 board_config.h + * + * Board internal definitions + */ + +#pragma once + +#include +#include +#include +#include + +/* LEDs are driven with push open drain to support Anode to 5V or 3.3V */ +#define GPIO_nLED_RED /* PB11 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11) +#define GPIO_nLED_GREEN /* PB1 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN1) +#define GPIO_nLED_BLUE /* PB3 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN3) + +#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +#define BOARD_OVERLOAD_LED LED_RED +#define BOARD_ARMED_STATE_LED LED_BLUE + +/* ADC channels */ +#define PX4_ADC_GPIO \ + /* PA2 */ GPIO_ADC12_INP14, \ + /* PA3 */ GPIO_ADC12_INP15, \ + /* PA4 */ GPIO_ADC12_INP18, \ + /* PC1 */ GPIO_ADC123_INP11 + +/* Define Channel numbers must match above GPIO pins */ +#define ADC_BATTERY_VOLTAGE_CHANNEL 14 /* PA2 BATT_VOLT_SENS */ +#define ADC_BATTERY_CURRENT_CHANNEL 15 /* PA3 BATT_CURRENT_SENS */ +#define ADC_SCALED_V5_CHANNEL 18 /* PA4 VDD_5V_SENS */ +#define ADC_RC_RSSI_CHANNEL 11 /* PC1 */ + +#define ADC_CHANNELS \ + ((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \ + (1 << ADC_BATTERY_CURRENT_CHANNEL) | \ + (1 << ADC_SCALED_V5_CHANNEL) | \ + (1 << ADC_RC_RSSI_CHANNEL)) + +/* HW has to large of R termination on ADC todo:change when HW value is chosen */ +#define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) + +/* CAN Silence: Silent mode control */ +#define GPIO_CAN1_SILENT_S0 /* PF5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5) + +/* PWM */ +#define DIRECT_PWM_OUTPUT_CHANNELS 8 + +/* Power supply control and monitoring GPIOs */ +#define GPIO_nPOWER_IN_A /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) + +#define GPIO_VDD_BRICK1_VALID GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */ +#define BOARD_NUMBER_BRICKS 1 + +#define GPIO_VDD_3V3_SPEKTRUM_POWER_EN /* PE4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) +#define GPIO_VDD_1V2_CORE_POWER_EN /* PH5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTH|GPIO_PIN5) + +/* Define True logic Power Control in arch agnostic form */ +#define VDD_3V3_SPEKTRUM_POWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SPEKTRUM_POWER_EN, (!on_true)) +#define READ_VDD_3V3_SPEKTRUM_POWER_EN() (px4_arch_gpioread(GPIO_VDD_3V3_SPEKTRUM_POWER_EN) == 0) + +/* Tone alarm output */ +#define TONE_ALARM_TIMER 2 /* timer 2 */ +#define TONE_ALARM_CHANNEL 1 /* PA15 TIM2_CH1 */ + +#define GPIO_BUZZER_1 /* PA15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) + +#define GPIO_TONE_ALARM_IDLE GPIO_BUZZER_1 +#define GPIO_TONE_ALARM GPIO_TIM2_CH1OUT_2 + +/* USB OTG FS */ +#define GPIO_OTGFS_VBUS /* PA9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN9) + +/* High-resolution timer */ +#define HRT_TIMER 3 /* use timer3 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 1 */ + +#define HRT_PPM_CHANNEL /* T3C3 */ 3 /* use capture/compare channel 3 */ +#define GPIO_PPM_IN /* PB0 T3C3 */ GPIO_TIM3_CH3IN_1 + +/* RC Serial port */ +#define RC_SERIAL_PORT "/dev/ttyS3" + +#define GPIO_RSSI_IN /* PC1 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1) + +/* Safety Switch: Enable the FMU to control it if there is no px4io fixme:This should be BOARD_SAFETY_LED(__ontrue) */ +#define GPIO_SAFETY_SWITCH_IN /* PC4 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4) +/* Enable the FMU to use the switch it if there is no px4io fixme:This should be BOARD_SAFTY_BUTTON() */ +#define GPIO_BTN_SAFETY GPIO_SAFETY_SWITCH_IN /* Enable the FMU to control it if there is no px4io */ + +/* Power switch controls ******************************************************/ +#define SPEKTRUM_POWER(_on_true) VDD_3V3_SPEKTRUM_POWER_EN(_on_true) + +/* + * Board has a separate RC_IN + * + * GPIO PPM_IN on PB0 T3CH3 + * SPEKTRUM_RX (it's TX or RX in Bind) on UART6 PC7 + * Inversion is possible in the UART and can drive GPIO_PPM_IN as an output + */ +#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0) +#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT) +#define SPEKTRUM_RX_AS_UART() /* Can be left as uart */ +#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true)) + +/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) + * this board support the ADC system_power interface, and therefore + * provides the true logic GPIO BOARD_ADC_xxxx macros. + */ +#define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) +#define BOARD_ADC_USB_VALID BOARD_ADC_USB_CONNECTED +#define BOARD_ADC_SERVO_VALID (1) /* never powers off the Servo rail */ +#define BOARD_ADC_BRICK_VALID (px4_arch_gpioread(GPIO_VDD_BRICK1_VALID)) + +/* This board provides a DMA pool and APIs */ +#define BOARD_DMA_ALLOC_POOL_SIZE 5120 + +/* This board provides the board_on_reset interface */ +#define BOARD_HAS_ON_RESET 1 + +#define BOARD_HAS_STATIC_MANIFEST 1 + + +#define BOARD_NUM_IO_TIMERS 3 + + +#define BOARD_ENABLE_CONSOLE_BUFFER + +#define PX4_GPIO_INIT_LIST { \ + PX4_ADC_GPIO, \ + GPIO_CAN1_TX, \ + GPIO_CAN1_RX, \ + GPIO_CAN2_TX, \ + GPIO_CAN2_RX, \ + GPIO_CAN1_SILENT_S0, \ + GPIO_nPOWER_IN_A, \ + GPIO_VDD_3V3_SPEKTRUM_POWER_EN, \ + GPIO_VDD_1V2_CORE_POWER_EN, \ + GPIO_TONE_ALARM_IDLE, \ + GPIO_SAFETY_SWITCH_IN, \ + GPIO_OTGFS_VBUS, \ + } + +__BEGIN_DECLS +#ifndef __ASSEMBLY__ + +extern void stm32_spiinitialize(void); +extern void board_peripheral_reset(int ms); + +#include +#endif /* __ASSEMBLY__ */ +__END_DECLS diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/bootloader_main.c b/boards/3dr/ctrl-zero-h7-oem-revg/src/bootloader_main.c new file mode 100755 index 000000000000..bb6b4dc23aad --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/bootloader_main.c @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (c) 2020, 2021 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 bootloader_main.c + * + * FMU-specific early startup code for bootloader +*/ + +#include "board_config.h" +#include "bl.h" + +#include +#include +#include +#include +#include +#include "arm_internal.h" +#include + +extern int sercon_main(int c, char **argv); + +__EXPORT void board_on_reset(int status) {} + +__EXPORT void stm32_boardinitialize(void) +{ + /* configure USB interfaces */ + stm32_configgpio(GPIO_OTGFS_VBUS); +} + +__EXPORT int board_app_initialize(uintptr_t arg) +{ + return 0; +} + +void board_late_initialize(void) +{ + sercon_main(0, NULL); +} + +extern void sys_tick_handler(void); +void board_timerhook(void) +{ + sys_tick_handler(); +} diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/hw_config.h b/boards/3dr/ctrl-zero-h7-oem-revg/src/hw_config.h new file mode 100755 index 000000000000..b212ccd60273 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/hw_config.h @@ -0,0 +1,135 @@ +/**************************************************************************** + * + * 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 + +/**************************************************************************** + * 10-8--2016: + * To simplify the ripple effect on the tools, we will be using + * /dev/serial/by-id/PX4 to locate PX4 devices. Therefore + * moving forward all Bootloaders must contain the prefix "PX4 BL " + * in the USBDEVICESTRING + * This Change will be made in an upcoming BL release + ****************************************************************************/ +/* + * Define usage to configure a bootloader + * + * + * Constant example Usage + * APP_LOAD_ADDRESS 0x08004000 - The address in Linker Script, where the app fw is org-ed + * BOOTLOADER_DELAY 5000 - Ms to wait while under USB pwr or bootloader request + * BOARD_FMUV2 + * INTERFACE_USB 1 - (Optional) Scan and use the USB interface for bootloading + * INTERFACE_USART 1 - (Optional) Scan and use the Serial interface for bootloading + * USBDEVICESTRING "PX4 BL FMU v2.x" - USB id string + * USBPRODUCTID 0x0011 - PID Should match defconfig + * BOOT_DELAY_ADDRESS 0x000001a0 - (Optional) From the linker script from Linker Script to get a custom + * delay provided by an APP FW + * BOARD_TYPE 9 - Must match .prototype boad_id + * _FLASH_KBYTES (*(uint16_t *)0x1fff7a22) - Run time flash size detection + * BOARD_FLASH_SECTORS ((_FLASH_KBYTES == 0x400) ? 11 : 23) - Run time determine the physical last sector + * BOARD_FLASH_SECTORS 11 - Hard coded zero based last sector + * BOARD_FLASH_SIZE (_FLASH_KBYTES*1024)- Total Flash size of device, determined at run time. + * (1024 * 1024) - Hard coded Total Flash of device - The bootloader and app reserved will be deducted + * programmatically + * + * BOARD_FIRST_FLASH_SECTOR_TO_ERASE 2 - Optional sectors index in the flash_sectors table (F4 only), to begin erasing. + * This is to allow sectors to be reserved for app fw usage. That will NOT be erased + * during a FW upgrade. + * The default is 0, and selects the first sector to be erased, as the 0th entry in the + * flash_sectors table. Which is the second physical sector of FLASH in the device. + * The first physical sector of FLASH is used by the bootloader, and is not defined + * in the table. + * + * APP_RESERVATION_SIZE (BOARD_FIRST_FLASH_SECTOR_TO_ERASE * 16 * 1024) - Number of bytes reserved by the APP FW. This number plus + * BOOTLOADER_RESERVATION_SIZE will be deducted from + * BOARD_FLASH_SIZE to determine the size of the App FW + * and hence the address space of FLASH to erase and program. + * USBMFGSTRING "PX4 AP" - Optional USB MFG string (default is '3D Robotics' if not defined.) + * SERIAL_BREAK_DETECT_DISABLED - Optional prevent break selection on Serial port from entering or staying in BL + * + * * Other defines are somewhat self explanatory. + */ + +/* Boot device selection list*/ +#define USB0_DEV 0x01 +#define SERIAL0_DEV 0x02 +#define SERIAL1_DEV 0x04 + +#define APP_LOAD_ADDRESS 0x08020000 +#define BOOTLOADER_DELAY 5000 +#define INTERFACE_USB 1 +#define INTERFACE_USB_CONFIG "/dev/ttyACM0" +#define BOARD_VBUS MK_GPIO_INPUT(GPIO_OTGFS_VBUS) + +//#define USE_VBUS_PULL_DOWN +#define INTERFACE_USART 1 +#define INTERFACE_USART_CONFIG "/dev/ttyS0,115200" +#define BOOT_DELAY_ADDRESS 0x000001a0 +#define BOARD_TYPE 1124 +#define _FLASH_KBYTES (*(uint32_t *)0x1FF1E880) +#define BOARD_FLASH_SECTORS (15) +#define BOARD_FLASH_SIZE (_FLASH_KBYTES * 1024) + +#define OSC_FREQ 24 + +#define BOARD_PIN_LED_ACTIVITY GPIO_nLED_RED +#define BOARD_PIN_LED_BOOTLOADER GPIO_nLED_RED +#define BOARD_LED_ON 0 +#define BOARD_LED_OFF 1 + +#define SERIAL_BREAK_DETECT_DISABLED 1 + +#if !defined(ARCH_SN_MAX_LENGTH) +# define ARCH_SN_MAX_LENGTH 12 +#endif + +#if !defined(APP_RESERVATION_SIZE) +# define APP_RESERVATION_SIZE 0 +#endif + +#if !defined(BOARD_FIRST_FLASH_SECTOR_TO_ERASE) +# define BOARD_FIRST_FLASH_SECTOR_TO_ERASE 1 +#endif + +#if !defined(USB_DATA_ALIGN) +# define USB_DATA_ALIGN +#endif + +#ifndef BOOT_DEVICES_SELECTION +# define BOOT_DEVICES_SELECTION USB0_DEV|SERIAL0_DEV|SERIAL1_DEV +#endif + +#ifndef BOOT_DEVICES_FILTER_ONUSB +# define BOOT_DEVICES_FILTER_ONUSB USB0_DEV|SERIAL0_DEV|SERIAL1_DEV +#endif diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/i2c.cpp b/boards/3dr/ctrl-zero-h7-oem-revg/src/i2c.cpp new file mode 100755 index 000000000000..1b8927c69939 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/i2c.cpp @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * 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 + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), + initI2CBusExternal(3), + initI2CBusExternal(4), +}; diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/init.c b/boards/3dr/ctrl-zero-h7-oem-revg/src/init.c new file mode 100755 index 000000000000..daaace48fb2f --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/init.c @@ -0,0 +1,204 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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 init.c + * + * board-specific early startup code. This file implements the + * board_app_initialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +#include "board_config.h" + +#include + +#include +#include +#include +#include +#include +#include "arm_internal.h" + +#include +#include +#include +#include +#include +#include +#include + +#include + +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +__END_DECLS + +/************************************************************************************ + * Name: board_peripheral_reset + * + * Description: + * + ************************************************************************************/ +__EXPORT void board_peripheral_reset(int ms) +{ + bool last = READ_VDD_3V3_SPEKTRUM_POWER_EN(); + /* Keep Spektum on to discharge rail*/ + VDD_3V3_SPEKTRUM_POWER_EN(false); + + /* wait for the peripheral rail to reach GND */ + usleep(ms * 1000); + syslog(LOG_DEBUG, "reset done, %d ms\n", ms); + + /* re-enable power */ + + /* switch the peripheral rail back on */ + VDD_3V3_SPEKTRUM_POWER_EN(last); +} + +/************************************************************************************ + * Name: board_on_reset + * + * Description: + * Optionally provided function called on entry to board_system_reset + * It should perform any house keeping prior to the rest. + * + * status - 1 if resetting to boot loader + * 0 if just resetting + * + ************************************************************************************/ +__EXPORT void board_on_reset(int status) +{ + for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) { + px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); + } + + if (status >= 0) { + up_mdelay(6); + } +} + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the initialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ +__EXPORT void stm32_boardinitialize(void) +{ + /* Reset PWM first thing */ + board_on_reset(-1); + + /* configure pins */ + const uint32_t gpio[] = PX4_GPIO_INIT_LIST; + px4_gpio_init(gpio, arraySize(gpio)); + board_control_spi_sensors_power_configgpio(); + + /* configure LEDs */ + board_autoled_initialize(); +} + +/**************************************************************************** + * Name: board_app_initialize + * + * Description: + * Perform application specific initialization. This function is never + * called directly from application code, but only indirectly via the + * (non-standard) boardctl() interface using the command BOARDIOC_INIT. + * + * Input Parameters: + * arg - The boardctl() argument is passed to the board_app_initialize() + * implementation without modification. The argument has no + * meaning to NuttX; + * + * Returned Value: + * Zero (OK) is returned on success; a negated errno value is returned on + * any failure to indicate the nature of the failure. + * + ****************************************************************************/ +__EXPORT int board_app_initialize(uintptr_t arg) +{ + /* Power on Interfaces */ + board_control_spi_sensors_power(true, 0xffff); + VDD_3V3_SPEKTRUM_POWER_EN(true); + + px4_platform_init(); + + stm32_spiinitialize(); + + /* configure the DMA allocator */ + if (board_dma_alloc_init() < 0) { + syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); + } + + + /* initial LED state */ + drv_led_start(); + led_off(LED_RED); + led_on(LED_GREEN); // Indicate Power. + led_off(LED_BLUE); + + if (board_hardfault_init(2, true) != 0) { + led_on(LED_RED); + } + +#ifdef CONFIG_MMCSD + /* Mount the SDIO-based MMC/SD block driver */ + /* First, get an instance of the SDIO interface */ + struct sdio_dev_s *sdio_dev = sdio_initialize(0); // SDIO_SLOTNO 0 Only one slot + + if (!sdio_dev) { + syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", 0); + } + + if (mmcsd_slotinitialize(0, sdio_dev) != OK) { + syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver\n"); + } + + /* Assume that the SD card is inserted. What choice do we have? */ + sdio_mediachange(sdio_dev, true); +#endif /* CONFIG_MMCSD */ + + /* Configure the HW based on the manifest */ + + px4_platform_configure(); + + return OK; +} diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/led.c b/boards/3dr/ctrl-zero-h7-oem-revg/src/led.c new file mode 100755 index 000000000000..0e7beb3a9e3b --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/led.c @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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 led.c + * + * LED backend. + */ + +#include + +#include + +#include "chip.h" +#include "stm32_gpio.h" +#include "board_config.h" + +#include +#include + +/* + * Ideally we'd be able to get these from arm_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(int led); +__END_DECLS + +static uint32_t g_ledmap[] = { + GPIO_nLED_BLUE, // Indexed by LED_BLUE + GPIO_nLED_RED, // Indexed by LED_RED + GPIO_nLED_GREEN, // Indexed by LED_GREEN +}; + +__EXPORT void led_init(void) +{ + for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) { + if (g_ledmap[l] != 0) { + stm32_configgpio(g_ledmap[l]); + } + } +} + +static void phy_set_led(int led, bool state) +{ + /* Drive Low to switch on */ + if (g_ledmap[led] != 0) { + stm32_gpiowrite(g_ledmap[led], !state); + } +} + +static bool phy_get_led(int led) +{ + /* If Low it is on */ + if (g_ledmap[led] != 0) { + return !stm32_gpioread(g_ledmap[led]); + } + + return false; +} + +__EXPORT void led_on(int led) +{ + phy_set_led(led, true); +} + +__EXPORT void led_off(int led) +{ + phy_set_led(led, false); +} + +__EXPORT void led_toggle(int led) +{ + phy_set_led(led, !phy_get_led(led)); +} diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/spi.cpp b/boards/3dr/ctrl-zero-h7-oem-revg/src/spi.cpp new file mode 100755 index 000000000000..4a4c3502bbd9 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/spi.cpp @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * Copyright (C) 2021 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 + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM20948, SPI::CS{GPIO::PortE, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin12}), + }, {GPIO::PortE, GPIO::Pin3}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10}), + initSPIDevice(DRV_BARO_DEVTYPE_DPS310, SPI::CS{GPIO::PortD, GPIO::Pin7}), + }), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin10}, SPI::DRDY{GPIO::PortF, GPIO::Pin3}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin6}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/timer_config.cpp b/boards/3dr/ctrl-zero-h7-oem-revg/src/timer_config.cpp new file mode 100755 index 000000000000..e23880491444 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/timer_config.cpp @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (C) 2021 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 + +constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { + initIOTimer(Timer::Timer1, DMA{DMA::Index1}), + initIOTimer(Timer::Timer4, DMA{DMA::Index1}), + initIOTimer(Timer::Timer8, DMA{DMA::Index1}), +}; + +constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel4}, {GPIO::PortE, GPIO::Pin14}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel3}, {GPIO::PortE, GPIO::Pin13}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel2}, {GPIO::PortE, GPIO::Pin11}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortE, GPIO::Pin9}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel2}, {GPIO::PortD, GPIO::Pin13}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}), + initIOTimerChannel(io_timers, {Timer::Timer8, Timer::Channel1}, {GPIO::PortI, GPIO::Pin5}), + initIOTimerChannel(io_timers, {Timer::Timer8, Timer::Channel2}, {GPIO::PortI, GPIO::Pin6}), +}; + +constexpr io_timers_channel_mapping_t io_timers_channel_mapping = + initIOTimerChannelMapping(io_timers, timer_io_channels); diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/usb.c b/boards/3dr/ctrl-zero-h7-oem-revg/src/usb.c new file mode 100755 index 000000000000..fe32ce71ac63 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/usb.c @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * Copyright (C) 2021 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 usb.c + * + * Board-specific USB functions. + */ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include "board_config.h" + +/************************************************************************************ + * Name: stm32_usbsuspend + * + * Description: + * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ +__EXPORT void stm32_usbinitialize(void) +{ + /* The OTG FS has an internal soft pull-up */ + + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + +#ifdef CONFIG_STM32H7_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); +#endif +} + + + +__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + uinfo("resume: %d\n", resume); +} From af0129dab7521bff7807feb0bfe5a7f338f7975c Mon Sep 17 00:00:00 2001 From: Ramon Roche Date: Wed, 14 Aug 2024 07:42:20 -0700 Subject: [PATCH 64/90] github: update bug report template Removes unnecessary required fields --- .github/ISSUE_TEMPLATE/bug_report.yml | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/.github/ISSUE_TEMPLATE/bug_report.yml b/.github/ISSUE_TEMPLATE/bug_report.yml index 60d93ea9b811..ffbb271a900f 100644 --- a/.github/ISSUE_TEMPLATE/bug_report.yml +++ b/.github/ISSUE_TEMPLATE/bug_report.yml @@ -20,14 +20,14 @@ body: 3. Took off '....' 4. See error validations: - required: true + required: false - type: textarea attributes: label: Expected behavior description: A clear and concise description of what you expected to happen. validations: - required: true + required: false - type: textarea attributes: @@ -45,7 +45,7 @@ body: placeholder: | # PASTE HERE THE LINK TO THE LOG validations: - required: true + required: false - type: markdown attributes: @@ -60,14 +60,14 @@ body: placeholder: | # If you don't know the version, paste the output of `ver all` in the MAVLink Shell of QGC validations: - required: true + required: false - type: input attributes: label: Flight controller description: Specify your flight controller model (what type is it, where was it bought from, ...). validations: - required: true + required: false - type: dropdown attributes: From 2a124fd99898166637e6bea3e3123aa6bfe176ef Mon Sep 17 00:00:00 2001 From: Vilius Date: Thu, 15 Aug 2024 12:29:02 +0300 Subject: [PATCH 65/90] Add Bosch BMM350 magnetometer (#23362) * Add Bosch BMM350 magnetometer * BMM350 replace info messages with debug messages * BMM350 update measurement interval * BMM350 fix offsets, update based on review * BMM350 Update default parameters to 50Hz * Update OTP Word LSB check * BMM350 fix styles and formatting * BMM350 update error checks --- src/drivers/drv_sensor.h | 2 + src/drivers/magnetometer/bosch/CMakeLists.txt | 1 + .../magnetometer/bosch/bmm350/BMM350.cpp | 773 ++++++++++++++++++ .../magnetometer/bosch/bmm350/BMM350.hpp | 210 +++++ .../bosch/bmm350/Bosch_BMM350_registers.hpp | 159 ++++ .../magnetometer/bosch/bmm350/CMakeLists.txt | 48 ++ src/drivers/magnetometer/bosch/bmm350/Kconfig | 5 + .../magnetometer/bosch/bmm350/bmm350_main.cpp | 89 ++ .../magnetometer/bosch/bmm350/module.yaml | 44 + 9 files changed, 1331 insertions(+) create mode 100755 src/drivers/magnetometer/bosch/bmm350/BMM350.cpp create mode 100644 src/drivers/magnetometer/bosch/bmm350/BMM350.hpp create mode 100644 src/drivers/magnetometer/bosch/bmm350/Bosch_BMM350_registers.hpp create mode 100644 src/drivers/magnetometer/bosch/bmm350/CMakeLists.txt create mode 100644 src/drivers/magnetometer/bosch/bmm350/Kconfig create mode 100644 src/drivers/magnetometer/bosch/bmm350/bmm350_main.cpp create mode 100644 src/drivers/magnetometer/bosch/bmm350/module.yaml diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 378c92b6736c..53eecce4be71 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -242,6 +242,8 @@ #define DRV_INS_DEVTYPE_VN300 0xE3 #define DRV_DIFF_PRESS_DEVTYPE_ASP5033 0xE4 +#define DRV_MAG_DEVTYPE_BMM350 0xE5 + #define DRV_DEVTYPE_UNUSED 0xff #endif /* _DRV_SENSOR_H */ diff --git a/src/drivers/magnetometer/bosch/CMakeLists.txt b/src/drivers/magnetometer/bosch/CMakeLists.txt index b814e7224fb1..d7e848ec86a4 100644 --- a/src/drivers/magnetometer/bosch/CMakeLists.txt +++ b/src/drivers/magnetometer/bosch/CMakeLists.txt @@ -32,3 +32,4 @@ ############################################################################ add_subdirectory(bmm150) +add_subdirectory(bmm350) diff --git a/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp b/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp new file mode 100755 index 000000000000..e4e8e19f9c5e --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp @@ -0,0 +1,773 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2022 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 "BMM350.hpp" +using namespace time_literals; + +BMM350::BMM350(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config), + ModuleParams(nullptr), + _px4_mag(get_device_id(), config.rotation) + +{ +} + +BMM350::~BMM350() +{ + perf_free(_reset_perf); + perf_free(_bad_read_perf); + perf_free(_self_test_failed_perf); +} + +int BMM350::init() +{ + ModuleParams::updateParams(); + ParametersUpdate(true); + int ret = I2C::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("I2C::init failed (%i)", ret); + return ret; + } + + return Reset() ? 0 : -1; +} + +bool BMM350::Reset() +{ + RegisterWrite(Register::CMD, SOFT_RESET); + _state = STATE::RESET; + ScheduleClear(); + ScheduleDelayed(1_ms); + return true; +} + +void BMM350::print_status() +{ + I2CSPIDriverBase::print_status(); + + perf_print_counter(_reset_perf); + perf_print_counter(_bad_read_perf); + perf_print_counter(_self_test_failed_perf); +} + +void BMM350::ParametersUpdate(bool force) +{ + if (_parameter_update_sub.updated() || force) { + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + updateParams(); + UpdateMagParams(); + } +} + +void BMM350::UpdateMagParams() +{ + uint8_t odr = GetODR(_param_bmm350_odr.get()); + uint8_t avg = GetAVG(_param_bmm350_avg.get()); + + _mag_odr_mode = odr; + _mag_avg_mode = avg; + _mag_pad_drive = static_cast(_param_bmm350_drive.get()); + PX4_DEBUG("Set params odr = %d, avg = %d, drive = %d", _mag_odr_mode, _mag_avg_mode, _mag_pad_drive); +} + +uint8_t BMM350::GetODR(int value) +{ + switch (value) { + case 0: return ODR_400HZ; + + case 1: return ODR_200HZ; + + case 2: return ODR_100HZ; + + case 3: return ODR_50HZ; + + case 4: return ODR_25HZ; + + case 5: return ODR_12_5HZ; + + case 6: return ODR_6_25HZ; + + case 7: return ODR_3_125HZ; + + case 8: return ODR_1_5625HZ; + + default: return ODR_200HZ; + } +} + +hrt_abstime BMM350::OdrToUs(uint8_t odr) +{ + switch (odr) { + case ODR_400HZ: + return 2500_us; + + case ODR_200HZ: + return 5000_us; + + case ODR_100HZ: + return 10000_us; + + case ODR_50HZ: + return 20000_us; + + case ODR_25HZ: + return 40000_us; + + case ODR_12_5HZ: + return 80000_us; + + case ODR_6_25HZ: + return 160000_us; + + case ODR_3_125HZ: + return 320000_us; + + case ODR_1_5625HZ: + return 640000_us; + + default: + return 5000_us; + } +} + +uint8_t BMM350::GetAVG(int value) +{ + switch (value) { + case 0: return AVG_NO_AVG; + + case 1: return AVG_2; + + case 2: return AVG_4; + + case 3: return AVG_8; + + default: return AVG_2; + } +} + + +int BMM350::probe() +{ + for (int i = 0; i < 3; i++) { + uint8_t chip_id; + + if (PX4_OK == RegisterRead(Register::CHIP_ID, &chip_id)) { + PX4_DEBUG("CHIP_ID: 0x%02hhX", chip_id); + + if (chip_id == chip_identification_number) { + return PX4_OK; + } + } + } + + return PX4_ERROR; +} + +void BMM350::RunImpl() +{ + const hrt_abstime now = hrt_absolute_time(); + int ret = PX4_OK; + ParametersUpdate(); + + switch (_state) { + case STATE::RESET: { + if (RegisterWrite(Register::CMD, SOFT_RESET) == PX4_OK) { + _reset_timestamp = now; + _state = STATE::WAIT_FOR_RESET; + perf_count(_reset_perf); + } + + ScheduleDelayed(10_ms); + } + break; + + case STATE::WAIT_FOR_RESET: { + ret = probe(); + + if (ret == PX4_OK) { + _state = STATE::RESET; + uint8_t status_0; + ret = RegisterRead(Register::PMU_STATUS_0, &status_0); + + if (ret == PX4_OK && (status_0 & PWR_NORMAL) != 0) { + ret = PX4_ERROR; + } + + if (ret == PX4_OK) { + ret = UpdateMagOffsets(); + } + + if (ret == PX4_OK) { + PX4_DEBUG("Going to FGR"); + _state = STATE::FGR; + + } + + ScheduleDelayed(10_ms); + + } else { + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Reset failed, retrying"); + _state = STATE::RESET; + ScheduleDelayed(100_ms); + + } else { + PX4_DEBUG("Reset not complete, check again in 10 ms"); + ScheduleDelayed(10_ms); + } + } + } + break; + + case STATE::FGR: { + _state = STATE::RESET; + uint8_t odr_reg_data = (ODR_100HZ & 0xf); + odr_reg_data = ((odr_reg_data & ~(0x30)) | ((AVG_2 << 0x4) & 0x30)); + ret = RegisterWrite(Register::PMU_CMD_AGGR_SET, odr_reg_data); + + if (ret == PX4_OK) { + ret = RegisterWrite(Register::PMU_CMD_AXIS_EN, EN_XYZ); + } + + if (ret == PX4_OK) { + ret = RegisterWrite(Register::PMU_CMD, PMU_CMD_FGR); + } + + if (ret == PX4_OK) { + PX4_DEBUG("Going to BR"); + _state = STATE::BR; + } + + ScheduleDelayed(30_ms); + } + break; + + case STATE::BR: { + uint8_t status_0; + _state = STATE::RESET; + ret = RegisterRead(Register::PMU_STATUS_0, &status_0); + + if (ret == PX4_OK && PMU_CMD_STATUS_0_RES(status_0) == PMU_STATUS_FGR) { + ret = RegisterWrite(Register::PMU_CMD, PMU_CMD_BR_FAST); + + if (ret == PX4_OK) { + PX4_DEBUG("Going to after reset"); + _state = STATE::AFTER_RESET; + } + } + + ScheduleDelayed(4_ms); + } + break; + + case STATE::AFTER_RESET: { + uint8_t status_0; + _state = STATE::RESET; + ret = RegisterRead(Register::PMU_STATUS_0, &status_0); + + if (ret == PX4_OK && PMU_CMD_STATUS_0_RES(status_0) == PMU_STATUS_BR_FAST) { + _state = STATE::MEASURE_FORCED; + _self_test_state = SELF_TEST_STATE::INIT; + PX4_DEBUG("Going to fast FM"); + } + + ScheduleNow(); + } + break; + + case STATE::MEASURE_FORCED: { + ret = RegisterWrite(Register::PMU_CMD, PMU_CMD_FAST_FM); + + if (ret == PX4_OK) { + _state = STATE::SELF_TEST_CHECK; + + } else { + _state = STATE::RESET; + } + + ScheduleDelayed(OdrToUs(_mag_odr_mode)); + break; + } + + case STATE::SELF_TEST_CHECK: { + + float xyzt[4]; + _state = STATE::RESET; + + if (ReadOutRawData(xyzt) != PX4_OK) { + perf_count(_self_test_failed_perf); + ScheduleNow(); + break; + } + + switch (_self_test_state) { + case SELF_TEST_STATE::INIT: + memcpy(_initial_self_test_values, xyzt, sizeof(_initial_self_test_values)); + + if (RegisterWrite(Register::TMR_SELF_TEST_USER, SELF_TEST_POS_X) == PX4_OK) { + _self_test_state = SELF_TEST_STATE::POS_X; + _state = STATE::MEASURE_FORCED; + } + + break; + + case SELF_TEST_STATE::POS_X: + if (xyzt[0] - _initial_self_test_values[0] >= 130.0f && + RegisterWrite(Register::TMR_SELF_TEST_USER, SELF_TEST_NEG_X) == PX4_OK) { + _self_test_state = SELF_TEST_STATE::NEG_X; + _state = STATE::MEASURE_FORCED; + } + + break; + + case SELF_TEST_STATE::NEG_X: + if (xyzt[0] - _initial_self_test_values[0] <= -130.0f && + RegisterWrite(Register::TMR_SELF_TEST_USER, SELF_TEST_POS_Y) == PX4_OK) { + _self_test_state = SELF_TEST_STATE::POS_Y; + _state = STATE::MEASURE_FORCED; + } + + break; + + case SELF_TEST_STATE::POS_Y: + if (xyzt[1] - _initial_self_test_values[1] >= 130.0f && + RegisterWrite(Register::TMR_SELF_TEST_USER, SELF_TEST_NEG_Y) == PX4_OK) { + _self_test_state = SELF_TEST_STATE::NEG_Y; + _state = STATE::MEASURE_FORCED; + } + + break; + + case SELF_TEST_STATE::NEG_Y: + if (xyzt[1] - _initial_self_test_values[1] <= -130.0f && + RegisterWrite(Register::TMR_SELF_TEST_USER, SELF_TEST_DISABLE) == PX4_OK) { + PX4_DEBUG("Self test good, going to configure"); + _state = STATE::CONFIGURE; + } + + break; + + } + + if (_state == STATE::RESET) { + PX4_DEBUG("Self test failed"); + perf_count(_self_test_failed_perf); + } + + ScheduleDelayed(1_ms); + } + break; + + + case STATE::CONFIGURE: + if (Configure() == PX4_OK) { + _state = STATE::READ; + PX4_DEBUG("Configure went fine"); + ScheduleOnInterval(OdrToUs(_mag_odr_mode), 50_ms); + + } else { + // CONFIGURE not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Configure failed, resetting"); + _state = STATE::RESET; + + } else { + PX4_DEBUG("Configure failed, retrying"); + } + + ScheduleDelayed(100_ms); + } + + break; + + case STATE::READ: { + // -- start get_compensated_mag_xyz_temp_data + float out_data[4] = {0.0f}; + float dut_offset_coeff[3], dut_sensit_coeff[3], dut_tcos[3], dut_tcss[3]; + float cr_ax_comp_x, cr_ax_comp_y, cr_ax_comp_z; + + ret = ReadOutRawData(out_data); + + if (ret == PX4_OK) { + // Apply compensation to temperature reading + out_data[3] = (1 + _mag_comp_vals.dut_sensit_coef.t_sens) * out_data[3] + + _mag_comp_vals.dut_offset_coef.t_offs; + + // Store magnetic compensation structure to an array + dut_offset_coeff[0] = _mag_comp_vals.dut_offset_coef.offset_x; + dut_offset_coeff[1] = _mag_comp_vals.dut_offset_coef.offset_y; + dut_offset_coeff[2] = _mag_comp_vals.dut_offset_coef.offset_z; + + dut_sensit_coeff[0] = _mag_comp_vals.dut_sensit_coef.sens_x; + dut_sensit_coeff[1] = _mag_comp_vals.dut_sensit_coef.sens_y; + dut_sensit_coeff[2] = _mag_comp_vals.dut_sensit_coef.sens_z; + + dut_tcos[0] = _mag_comp_vals.dut_tco.tco_x; + dut_tcos[1] = _mag_comp_vals.dut_tco.tco_y; + dut_tcos[2] = _mag_comp_vals.dut_tco.tco_z; + + dut_tcss[0] = _mag_comp_vals.dut_tcs.tcs_x; + dut_tcss[1] = _mag_comp_vals.dut_tcs.tcs_y; + dut_tcss[2] = _mag_comp_vals.dut_tcs.tcs_z; + + for (int idx = 0; idx < 3; idx++) { + out_data[idx] *= 1 + dut_sensit_coeff[idx]; + out_data[idx] += dut_offset_coeff[idx]; + out_data[idx] += dut_tcos[idx] * (out_data[3] - _mag_comp_vals.dut_t0); + out_data[idx] /= 1 + dut_tcss[idx] * (out_data[3] - _mag_comp_vals.dut_t0); + } + + cr_ax_comp_x = (out_data[0] - _mag_comp_vals.cross_axis.cross_x_y * out_data[1]) / + (1 - _mag_comp_vals.cross_axis.cross_y_x * _mag_comp_vals.cross_axis.cross_x_y); + cr_ax_comp_y = (out_data[1] - _mag_comp_vals.cross_axis.cross_y_x * out_data[0]) / + (1 - _mag_comp_vals.cross_axis.cross_y_x * _mag_comp_vals.cross_axis.cross_x_y); + cr_ax_comp_z = + (out_data[2] + + (out_data[0] * + (_mag_comp_vals.cross_axis.cross_y_x * _mag_comp_vals.cross_axis.cross_z_y - + _mag_comp_vals.cross_axis.cross_z_x) - + out_data[1] * + (_mag_comp_vals.cross_axis.cross_z_y - _mag_comp_vals.cross_axis.cross_x_y * + _mag_comp_vals.cross_axis.cross_z_x)) / + (1 - _mag_comp_vals.cross_axis.cross_y_x * _mag_comp_vals.cross_axis.cross_x_y)); + + out_data[0] = cr_ax_comp_x; + out_data[1] = cr_ax_comp_y; + out_data[2] = cr_ax_comp_z; + _px4_mag.set_error_count(perf_event_count(_bad_read_perf) + perf_event_count(_self_test_failed_perf)); + _px4_mag.set_temperature(out_data[3]); + _px4_mag.update(now, out_data[0], out_data[1], out_data[2]); + + } else { + perf_count(_bad_read_perf); + } + } + + break; + } +} + +int BMM350::Configure() +{ + PX4_DEBUG("Configuring"); + uint8_t readData = 0; + int ret; + + // Set pad drive + ret = RegisterWrite(Register::PAD_CTRL, (_mag_pad_drive & 0x7)); + + if (ret != PX4_OK) { + PX4_DEBUG("Couldn't set pad drive, defaults to 7"); + return ret; + } + + // Set PMU data aggregation + uint8_t odr = _mag_odr_mode; + uint8_t avg = _mag_avg_mode; + + if (odr == ODR_400HZ && avg >= AVG_2) { + avg = AVG_NO_AVG; + + } else if (odr == ODR_200HZ && avg >= AVG_4) { + avg = AVG_2; + + } else if (odr == ODR_100HZ && avg >= AVG_8) { + avg = AVG_4; + } + + uint8_t odr_reg_data = (odr & 0xf); + odr_reg_data = ((odr_reg_data & ~(0x30)) | ((avg << 0x4) & 0x30)); + + ret = RegisterWrite(Register::PMU_CMD_AGGR_SET, odr_reg_data); + + if (ret != PX4_OK) { + PX4_DEBUG("Cannot set PMU AGG REG"); + return ret; + } + + ret = RegisterRead(Register::PMU_CMD_AGGR_SET, &readData); + + if (ret != PX4_OK || readData != odr_reg_data) { + PX4_DEBUG("Couldn't check PMU AGGR REG"); + return ret; + } + + odr_reg_data = PMU_CMD_UPDATE_OAE; + ret = RegisterWrite(Register::PMU_CMD, odr_reg_data); + + if (ret != PX4_OK) { + PX4_DEBUG("Couldn't write PMU CMD REG"); + return ret; + } + + ret = RegisterRead(Register::PMU_CMD, &readData); + + if (ret != PX4_OK || readData != odr_reg_data) { + PX4_DEBUG("Couldn't check PMU CMD REG"); + return ret; + } + + // Enable AXIS + uint8_t axis_data = EN_X | EN_Y | EN_Z; + // PMU_CMD_AXIS_EN + ret = RegisterWrite(Register::PMU_CMD_AXIS_EN, axis_data); + + if (ret != PX4_OK) { + PX4_DEBUG("Couldn't write AXIS data"); + return ret; + } + + ret = RegisterRead(Register::PMU_CMD_AXIS_EN, &readData); + + if (ret != PX4_OK || readData != axis_data) { + PX4_DEBUG("Couldn't cross check the set AXIS"); + return ret; + } + + ret = RegisterWrite(Register::PMU_CMD, PMU_CMD_NM); + + if (ret != PX4_OK) { + PX4_DEBUG("Failed to start mag in normal mode"); + return ret; + } + + // microTesla -> Gauss + _px4_mag.set_scale(0.01f); + + return ret; +} + +int32_t BMM350::FixSign(uint32_t inval, int8_t num_bits) +{ + int32_t power = 1 << (num_bits - 1); // Calculate 2^(num_bits - 1) + int32_t retval = static_cast(inval); + + if (retval >= power) { + retval -= (power << 1); // Equivalent to retval = retval - (power * 2) + } + + return retval; +} + +int BMM350::ReadOutRawData(float *out_data) +{ + if (out_data == NULL) { + return -1; + } + + float temp = 0.0; + struct BMM350::raw_mag_data raw_data = {0}; + + // --- Start read_uncomp_mag_temp_data + uint8_t mag_data[14] = {0}; + + uint32_t raw_mag_x, raw_mag_y, raw_mag_z, raw_temp; + uint8_t cmd = static_cast(Register::DATAX_XLSB); + + uint8_t res = transfer(&cmd, 1, (uint8_t *)&mag_data, sizeof(mag_data)); + + if (res != PX4_OK) { + return -1; + } + + raw_mag_x = mag_data[2] + ((uint32_t)mag_data[3] << 8) + ((uint32_t)mag_data[4] << 16); + raw_mag_y = mag_data[5] + ((uint32_t)mag_data[6] << 8) + ((uint32_t)mag_data[7] << 16); + raw_mag_z = mag_data[8] + ((uint32_t)mag_data[9] << 8) + ((uint32_t)mag_data[10] << 16); + raw_temp = mag_data[11] + ((uint32_t)mag_data[12] << 8) + ((uint32_t)mag_data[13] << 16); + + raw_data.raw_x = FixSign(raw_mag_x, 24); + raw_data.raw_y = FixSign(raw_mag_y, 24); + raw_data.raw_z = FixSign(raw_mag_z, 24); + raw_data.raw_t = FixSign(raw_temp, 24); + // --- End read_uncomp_mag_temp_data + + // --- Start read_out_raw_data + out_data[0] = (float)raw_data.raw_x * lsb_to_utc_degc[0]; + out_data[1] = (float)raw_data.raw_y * lsb_to_utc_degc[1]; + out_data[2] = (float)raw_data.raw_z * lsb_to_utc_degc[2]; + out_data[3] = (float)raw_data.raw_t * lsb_to_utc_degc[3]; + + // Adjust temperature + if (out_data[3] > 0.0f) { + temp = (float)(out_data[3] - (1 * 25.49f)); + + } else if (out_data[3] < 0.0f) { + temp = (float)(out_data[3] - (-1 * 25.49f)); + + } else { + temp = (float)(out_data[3]); + } + + out_data[3] = temp; + + return res; +} + +int BMM350::ReadOTPWord(uint8_t addr, uint16_t *lsb_msb) +{ + uint8_t otp_cmd = OTP_DIR_READ | (addr & OTP_WORD_MSK); + int ret = RegisterWrite(Register::OTP_CMD, otp_cmd); + uint8_t otp_status = 0; + + if (ret == PX4_OK) { + do { + ret = RegisterRead(Register::OTP_STATUS, &otp_status); + + if (ret != PX4_OK) { + return PX4_ERROR; + } + } while (!(otp_status & 0x01)); + + uint8_t msb = 0, lsb = 0; + ret = RegisterRead(Register::OTP_DATA_MSB, &msb); + + if (ret == PX4_OK) { + ret = RegisterRead(Register::OTP_DATA_LSB, &lsb); + + if (ret == PX4_OK) { + *lsb_msb = ((msb << 8) | lsb) & 0xffff; + } + } + } + + return ret; +} + +int BMM350::UpdateMagOffsets() +{ + PX4_DEBUG("DUMPING OTP"); + uint16_t otp_data[32] = {0}; + + for (int idx = 0; idx < 32; idx++) { + if (ReadOTPWord(idx, &otp_data[idx]) != PX4_OK) { + return PX4_ERROR; + } + + PX4_DEBUG("i: %i, val = %i", idx, otp_data[idx]); + } + + if (RegisterWrite(Register::OTP_CMD, PWR_OFF_OTP) != PX4_OK) { + return PX4_ERROR; + } + + PX4_DEBUG("var_id: %i", (otp_data[30] & 0x7f00) >> 9); + + PX4_DEBUG("UPDATING OFFSETS"); + uint16_t off_x_lsb_msb, off_y_lsb_msb, off_z_lsb_msb, t_off; + uint8_t sens_x, sens_y, sens_z, t_sens; + uint8_t tco_x, tco_y, tco_z; + uint8_t tcs_x, tcs_y, tcs_z; + uint8_t cross_x_y, cross_y_x, cross_z_x, cross_z_y; + + off_x_lsb_msb = otp_data[0x0E] & 0x0FFF; + off_y_lsb_msb = ((otp_data[0x0E] & 0xF000) >> 4) + + (otp_data[0x0F] & 0x00FF); + off_z_lsb_msb = (otp_data[0x0F] & 0x0F00) + + (otp_data[0x10] & 0x00FF); + t_off = otp_data[0x0D] & 0x00FF; + + _mag_comp_vals.dut_offset_coef.offset_x = FixSign(off_x_lsb_msb, 12); + _mag_comp_vals.dut_offset_coef.offset_y = FixSign(off_y_lsb_msb, 12); + _mag_comp_vals.dut_offset_coef.offset_z = FixSign(off_z_lsb_msb, 12); + _mag_comp_vals.dut_offset_coef.t_offs = FixSign(t_off, 8) / 5.0f; + + sens_x = (otp_data[0x10] & 0xFF00) >> 8; + sens_y = (otp_data[0x11] & 0x00FF); + sens_z = (otp_data[0x11] & 0xFF00) >> 8; + t_sens = (otp_data[0x0D] & 0xFF00) >> 8; + + _mag_comp_vals.dut_sensit_coef.sens_x = FixSign(sens_x, 8) / 256.0f; + _mag_comp_vals.dut_sensit_coef.sens_y = (FixSign(sens_y, 8) / 256.0f) + 0.01f; + _mag_comp_vals.dut_sensit_coef.sens_z = FixSign(sens_z, 8) / 256.0f; + _mag_comp_vals.dut_sensit_coef.t_sens = FixSign(t_sens, 8) / 512.0f; + + tco_x = (otp_data[0x12] & 0x00FF); + tco_y = (otp_data[0x13] & 0x00FF); + tco_z = (otp_data[0x14] & 0x00FF); + + _mag_comp_vals.dut_tco.tco_x = FixSign(tco_x, 8) / 32.0f; + _mag_comp_vals.dut_tco.tco_y = FixSign(tco_y, 8) / 32.0f; + _mag_comp_vals.dut_tco.tco_z = FixSign(tco_z, 8) / 32.0f; + + tcs_x = (otp_data[0x12] & 0xFF00) >> 8; + tcs_y = (otp_data[0x13] & 0xFF00) >> 8; + tcs_z = (otp_data[0x14] & 0xFF00) >> 8; + + _mag_comp_vals.dut_tcs.tcs_x = FixSign(tcs_x, 8) / 16384.0f; + _mag_comp_vals.dut_tcs.tcs_y = FixSign(tcs_y, 8) / 16384.0f; + _mag_comp_vals.dut_tcs.tcs_z = (FixSign(tcs_z, 8) / 16384.0f) - 0.0001f; + + _mag_comp_vals.dut_t0 = (FixSign(otp_data[0x18], 16) / 512.0f) + 23.0f; + + cross_x_y = (otp_data[0x15] & 0x00FF); + cross_y_x = (otp_data[0x15] & 0xFF00) >> 8; + cross_z_x = (otp_data[0x16] & 0x00FF); + cross_z_y = (otp_data[0x16] & 0xFF00) >> 8; + + _mag_comp_vals.cross_axis.cross_x_y = FixSign(cross_x_y, 8) / 800.0f; + _mag_comp_vals.cross_axis.cross_y_x = FixSign(cross_y_x, 8) / 800.0f; + _mag_comp_vals.cross_axis.cross_z_x = FixSign(cross_z_x, 8) / 800.0f; + _mag_comp_vals.cross_axis.cross_z_y = FixSign(cross_z_y, 8) / 800.0f; + return PX4_OK; +} + +int BMM350::RegisterRead(Register reg, uint8_t *value) +{ + const uint8_t cmd = static_cast(reg); + uint8_t buffer[3] = {0}; + int ret = transfer(&cmd, 1, buffer, 3); + + if (ret != PX4_OK) { + PX4_DEBUG("register read 0x%02hhX failed, ret = %d", cmd, ret); + + } else { + *value = buffer[2]; + } + + return ret; +} + +int BMM350::RegisterWrite(Register reg, uint8_t value) +{ + uint8_t buffer[2] {(uint8_t)reg, value}; + int ret = transfer(buffer, sizeof(buffer), nullptr, 0); + + if (ret != PX4_OK) { + PX4_DEBUG("register write 0x%02hhX failed, ret = %d", (uint8_t)reg, ret); + } + + return ret; +} diff --git a/src/drivers/magnetometer/bosch/bmm350/BMM350.hpp b/src/drivers/magnetometer/bosch/bmm350/BMM350.hpp new file mode 100644 index 000000000000..4ba7f535dbd9 --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/BMM350.hpp @@ -0,0 +1,210 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2022 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 BMM350.hpp + * + * Driver for the Bosch BMM350 connected via I2C. + * + */ + +#pragma once + +#include "Bosch_BMM350_registers.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace Bosch_BMM350; +using namespace time_literals; + +class BMM350 : public device::I2C, public I2CSPIDriver, public ModuleParams +{ +public: + BMM350(const I2CSPIDriverConfig &config); + ~BMM350() override; + + static void print_usage(); + + void RunImpl(); + + int init() override; + void print_status() override; + +private: + struct mag_temp_data { + float x; + float y; + float z; + float temp; + }; + + struct raw_mag_data { + int32_t raw_x; + int32_t raw_y; + int32_t raw_z; + int32_t raw_t; + }; + + struct register_config_t { + Register reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + + struct dut_offset_coef { + float t_offs; + float offset_x; + float offset_y; + float offset_z; + }; + struct dut_sensit_coef { + float t_sens; + float sens_x; + float sens_y; + float sens_z; + }; + + struct dut_tco { + float tco_x; + float tco_y; + float tco_z; + }; + + struct dut_tcs { + float tcs_x; + float tcs_y; + float tcs_z; + }; + + struct cross_axis { + float cross_x_y; + float cross_y_x; + float cross_z_x; + float cross_z_y; + }; + + struct mag_compensate_vals { + struct dut_offset_coef dut_offset_coef; + struct dut_sensit_coef dut_sensit_coef; + struct dut_tco dut_tco; + struct dut_tcs dut_tcs; + float dut_t0; + struct cross_axis cross_axis; + }; + + int probe() override; + bool Reset(); + int Configure(); + + int RegisterRead(Register reg, uint8_t *value); + int RegisterWrite(Register reg, uint8_t value); + + int8_t CompensateAxisAndTemp(); + int ReadOutRawData(float *out_data); + int ReadOTPWord(uint8_t addr, uint16_t *lsb_msb); + int32_t FixSign(uint32_t inval, int8_t num_bits); + + int UpdateMagOffsets(); + void ParametersUpdate(bool force = false); + void UpdateMagParams(); + uint8_t GetODR(int value); + hrt_abstime OdrToUs(uint8_t value); + uint8_t GetAVG(int value); + + PX4Magnetometer _px4_mag; + + perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME ": reset")}; + perf_counter_t _bad_read_perf{perf_alloc(PC_COUNT, MODULE_NAME ": bad read")}; + perf_counter_t _self_test_failed_perf{perf_alloc(PC_COUNT, MODULE_NAME ": self test failed")}; + + hrt_abstime _reset_timestamp{0}; + hrt_abstime _last_config_check_timestamp{0}; + + mag_compensate_vals _mag_comp_vals{0}; + + float _initial_self_test_values[4]; + + uint8_t _mag_odr_mode = ODR_200HZ; + uint8_t _mag_avg_mode = AVG_2; + uint8_t _mag_pad_drive = 7; + + + static constexpr float BXY_SENS = 14.55f; + static constexpr float BZ_SENS = 9.0f; + static constexpr float TEMP_SENS = 0.00204f; + static constexpr float INA_XY_GAIN_TRT = 19.46f; + static constexpr float INA_Z_GAIN_TRT = 31.0f; + static constexpr float ADC_GAIN = 1 / 1.5f; + static constexpr float LUT_GAIN = 0.714607238769531f; + static constexpr float POWER = 1000000.0f / 1048576.0f; + float lsb_to_utc_degc[4] = { + (POWER / (BXY_SENS *INA_XY_GAIN_TRT *ADC_GAIN * LUT_GAIN)), + (POWER / (BXY_SENS *INA_XY_GAIN_TRT *ADC_GAIN * LUT_GAIN)), + (POWER / (BZ_SENS *INA_Z_GAIN_TRT *ADC_GAIN * LUT_GAIN)), + 1 / (TEMP_SENS *ADC_GAIN *LUT_GAIN * 1048576) + }; + + enum class STATE : uint8_t { + RESET, + WAIT_FOR_RESET, + FGR, + BR, + AFTER_RESET, + MEASURE_FORCED, + SELF_TEST_CHECK, + CONFIGURE, + READ, + } _state{STATE::RESET}; + + enum class SELF_TEST_STATE : uint8_t { + INIT, + POS_X, + NEG_X, + POS_Y, + NEG_Y + } _self_test_state{SELF_TEST_STATE::INIT}; + + DEFINE_PARAMETERS( + (ParamInt) _param_bmm350_odr, + (ParamInt) _param_bmm350_avg, + (ParamInt) _param_bmm350_drive + ) + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; +}; diff --git a/src/drivers/magnetometer/bosch/bmm350/Bosch_BMM350_registers.hpp b/src/drivers/magnetometer/bosch/bmm350/Bosch_BMM350_registers.hpp new file mode 100644 index 000000000000..dc391f8824aa --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/Bosch_BMM350_registers.hpp @@ -0,0 +1,159 @@ +/**************************************************************************** + * + * Copyright (c) 2020-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. + * + ****************************************************************************/ + +/** + * @file Bosch_BMM350_registers.hpp + * + * Bosch BMM350 registers. + * + */ + +#pragma once + +#include + +namespace Bosch_BMM350 +{ +#define ENABLE_X_AXIS(axis_data) (axis_data = (axis_data & ~(0x01)) | (1 & 0x01)) +#define ENABLE_Y_AXIS(axis_data) (axis_data = ((axis_data & ~(0x02)) | ((1 << 0x1) & 0x02))) +#define ENABLE_Z_AXIS(axis_data) (axis_data = ((axis_data & ~(0x04)) | ((1 << 0x2) & 0x04))) + +static constexpr uint32_t I2C_SPEED = 400 * 1000; +static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0x14; +static constexpr uint8_t chip_identification_number = 0x33; + +enum class Register : uint8_t { + CHIP_ID = 0x00, + + PAD_CTRL = 0x03, + PMU_CMD_AGGR_SET = 0x04, + PMU_CMD_AXIS_EN = 0x05, + PMU_CMD = 0x06, + PMU_STATUS_0 = 0x07, + PMU_STATUS_1 = 0x08, + + I2C_WDT_SET = 0x0a, + + DATAX_XLSB = 0x31, + + OTP_CMD = 0x50, + OTP_DATA_MSB = 0x52, + OTP_DATA_LSB = 0x53, + OTP_STATUS = 0x55, + + TMR_SELF_TEST_USER = 0x60, + CMD = 0x7e +}; + +enum CONTROL_CMD : uint8_t { + SOFT_RESET = 0xb6, + EN_XYZ = 0x7 +}; + +enum PMU_CONTROL_CMD : uint8_t { + PMU_CMD_SUSPEND = 0x00, + PMU_CMD_NM = 0x01, + PMU_CMD_UPDATE_OAE = 0x02, + PMU_CMD_FM = 0x03, + PMU_CMD_FAST_FM = 0x04, + PMU_CMD_FGR = 0x05, + PMU_CMD_FAST_FGR = 0x06, + PMU_CMD_BR = 0x07, + PMU_CMD_BR_FAST = 0x08, + PMU_CMD_NM_TC = 0x09 +}; + +static inline uint8_t PMU_CMD_STATUS_0_RES(uint8_t val) +{ + return (val >> 5) & 0x7; +}; + +enum PMU_STATUS_0_BIT : uint8_t { + PMU_BUSY = (1 << 0), + ODR_OVWR = (1 << 1), + AVG_OVWR = (1 << 2), + PWR_NORMAL = (1 << 3), + ILLEGAL_CMD = (1 << 4) +}; + +enum PMU_STATUS_VALUE { + PMU_STATUS_SUS = 0x0, + PMU_STATUS_NM = 0x1, + PMU_STATUS_UPDATE_OAE = 0x2, + PMU_STATUS_FM = 0x3, + PMU_STATUS_FM_FAST = 0x4, + PMU_STATUS_FGR = 0x5, + PMU_STATUS_FGR_FAST = 0x6, + PMU_STATUS_BR = 0x7, + PMU_STATUS_BR_FAST = 0x7, +}; + +enum ODR_CONTROL_CMD : uint8_t { + ODR_400HZ = 0x2, + ODR_200HZ = 0x3, + ODR_100HZ = 0x4, + ODR_50HZ = 0x5, + ODR_25HZ = 0x6, + ODR_12_5HZ = 0x7, + ODR_6_25HZ = 0x8, + ODR_3_125HZ = 0x9, + ODR_1_5625HZ = 0xa +}; + +enum AVG_CONTROL_CMD : uint8_t { + AVG_NO_AVG = 0x0, + AVG_2 = 0x1, + AVG_4 = 0x2, + AVG_8 = 0x3 +}; + +enum ENABLE_AXIS_BIT : uint8_t { + EN_X = (1 << 0), + EN_Y = (1 << 1), + EN_Z = (1 << 2) +}; + +enum OTP_CONTROL_CMD : uint8_t { + PWR_OFF_OTP = 0x80, + OTP_DIR_READ = 0x20, + OTP_WORD_MSK = 0x1F, +}; + +enum SELF_TEST_CMD : uint8_t { + SELF_TEST_DISABLE = 0x00, + SELF_TEST_POS_X = 0x0d, + SELF_TEST_NEG_X = 0x0b, + SELF_TEST_POS_Y = 0x15, + SELF_TEST_NEG_Y = 0x13, +}; +} // namespace Bosch_BMM350 diff --git a/src/drivers/magnetometer/bosch/bmm350/CMakeLists.txt b/src/drivers/magnetometer/bosch/bmm350/CMakeLists.txt new file mode 100644 index 000000000000..ca253077287e --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/CMakeLists.txt @@ -0,0 +1,48 @@ +############################################################################ +# +# 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_module( + MODULE drivers__magnetometer__bosch__bmm350 + MAIN bmm350 + COMPILE_FLAGS + SRCS + BMM350.cpp + BMM350.hpp + bmm350_main.cpp + Bosch_BMM350_registers.hpp + DEPENDS + drivers_magnetometer + px4_work_queue + MODULE_CONFIG + module.yaml + ) diff --git a/src/drivers/magnetometer/bosch/bmm350/Kconfig b/src/drivers/magnetometer/bosch/bmm350/Kconfig new file mode 100644 index 000000000000..834a2c694780 --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_MAGNETOMETER_BOSCH_BMM350 + bool "bmm350" + default n + ---help--- + Enable support for bosch bmm350 diff --git a/src/drivers/magnetometer/bosch/bmm350/bmm350_main.cpp b/src/drivers/magnetometer/bosch/bmm350/bmm350_main.cpp new file mode 100644 index 000000000000..c707d7957b6b --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/bmm350_main.cpp @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2022 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 "BMM350.hpp" + +#include +#include + +void BMM350::print_usage() +{ + PRINT_MODULE_USAGE_NAME("bmm350", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x14); + PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int bmm350_main(int argc, char *argv[]) +{ + int ch; + using ThisDriver = BMM350; + BusCLIArguments cli{true, false}; + cli.default_i2c_frequency = I2C_SPEED; + cli.i2c_address = I2C_ADDRESS_DEFAULT; + + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { + switch (ch) { + case 'R': + cli.rotation = (enum Rotation)atoi(cli.optArg()); + break; + } + } + + const char *verb = cli.optArg(); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_BMM350); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + } + + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + } + + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/magnetometer/bosch/bmm350/module.yaml b/src/drivers/magnetometer/bosch/bmm350/module.yaml new file mode 100644 index 000000000000..140c5c321a8a --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/module.yaml @@ -0,0 +1,44 @@ +module_name: BMM350 +parameters: + - group: Magnetometer + definitions: + BMM350_ODR: + description: + short: BMM350 ODR rate + long: | + Defines which ODR rate to use during data polling. + type: enum + values: + 0: 400 Hz + 1: 200 Hz + 2: 100 Hz + 3: 50 Hz + 4: 25 Hz + 5: 12.5 Hz + 6: 6.25 Hz + 7: 3.125 Hz + 8: 1.5625 Hz + reboot_required: true + default: [3] + BMM350_AVG: + description: + short: BMM350 data averaging + long: | + Defines which averaging mode to use during data polling. + type: enum + values: + 0: No averaging + 1: 2 sample averaging + 2: 4 sample averaging + 3: 8 sample averaging + reboot_required: true + default: [1] + BMM350_DRIVE: + description: + short: BMM350 pad drive strength setting + long: | + This setting helps avoid signal problems like overshoot or undershoot. + type: int32 + min: 0 + max: 7 + default: [7] From f7e6e1354af1df0ab8ef3f0ad169518ad54d1989 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 14 Aug 2024 16:00:33 -0400 Subject: [PATCH 66/90] commander: power check only keep error thresholds --- .../checks/powerCheck.cpp | 20 +++++++------------ 1 file changed, 7 insertions(+), 13 deletions(-) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.cpp index 0b23a949aef6..69afd1c12cce 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.cpp @@ -74,16 +74,10 @@ void PowerChecks::checkAndReport(const Context &context, Report &reporter) if (!system_power.usb_connected) { float avionics_power_rail_voltage = system_power.voltage5v_v; - const float low_error_threshold = 4.5f; - const float low_warning_threshold = 4.8f; - const float high_warning_threshold = 5.4f; + const float low_error_threshold = 4.7f; + const float high_error_threshold = 5.4f; - if (avionics_power_rail_voltage < low_warning_threshold) { - NavModes affected_groups = NavModes::None; - - if (avionics_power_rail_voltage < low_error_threshold) { - affected_groups = NavModes::All; - } + if (avionics_power_rail_voltage < low_error_threshold) { /* EVENT * @description @@ -93,16 +87,16 @@ void PowerChecks::checkAndReport(const Context &context, Report &reporter) * This check can be configured via CBRK_SUPPLY_CHK parameter. * */ - reporter.healthFailure(affected_groups, health_component_t::system, + reporter.healthFailure(NavModes::All, health_component_t::system, events::ID("check_avionics_power_low"), - events::Log::Error, "Avionics Power low: {1:.2} Volt", avionics_power_rail_voltage, low_warning_threshold); + events::Log::Error, "Avionics Power low: {1:.2} Volt", avionics_power_rail_voltage, low_error_threshold); if (reporter.mavlink_log_pub()) { mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Avionics Power low: %6.2f Volt", (double)avionics_power_rail_voltage); } - } else if (avionics_power_rail_voltage > high_warning_threshold) { + } else if (avionics_power_rail_voltage > high_error_threshold) { /* EVENT * @description * Check the voltage supply to the FMU, it must be below {2:.2} Volt. @@ -113,7 +107,7 @@ void PowerChecks::checkAndReport(const Context &context, Report &reporter) */ reporter.healthFailure(NavModes::All, health_component_t::system, events::ID("check_avionics_power_high"), - events::Log::Error, "Avionics Power high: {1:.2} Volt", avionics_power_rail_voltage, high_warning_threshold); + events::Log::Error, "Avionics Power high: {1:.2} Volt", avionics_power_rail_voltage, high_error_threshold); if (reporter.mavlink_log_pub()) { mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Avionics Power high: %6.2f Volt", From 4a3cbecf014475342d00232b6210eb9c1a5183f5 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 14 Aug 2024 14:43:57 +0200 Subject: [PATCH 67/90] Commander: only add *autopilot disengaged* to failsafe notifactions in special cases Signed-off-by: Silvan Fuhrer --- src/modules/commander/failsafe/framework.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/failsafe/framework.cpp b/src/modules/commander/failsafe/framework.cpp index 9f88f85f3e8e..1eddc8a7f81c 100644 --- a/src/modules/commander/failsafe/framework.cpp +++ b/src/modules/commander/failsafe/framework.cpp @@ -224,6 +224,16 @@ void FailsafeBase::notifyUser(uint8_t user_intended_mode, Action action, Action {events::Log::Warning, events::LogInternal::Warning}, "Failsafe warning:", mavlink_mode); + } else if (action == Action::Descend || action == Action::FallbackAltCtrl || action == Action::FallbackStab) { + /* EVENT + * @description Failsafe actions that disengage the autopilot (remove position control) + * @type append_health_and_arming_messages + */ + events::send( + events::ID("commander_failsafe_enter_autopilot_disengaged"), + {events::Log::Critical, events::LogInternal::Warning}, + "Failsafe activated: Autopilot disengaged, switching to {2}", mavlink_mode, failsafe_action); + } else { /* EVENT * @type append_health_and_arming_messages @@ -231,7 +241,7 @@ void FailsafeBase::notifyUser(uint8_t user_intended_mode, Action action, Action events::send( events::ID("commander_failsafe_enter_generic"), {events::Log::Critical, events::LogInternal::Warning}, - "Failsafe activated: Autopilot disengaged, switching to {2}", mavlink_mode, failsafe_action); + "Failsafe activated: switching to {2}", mavlink_mode, failsafe_action); } } else { From 09638552b74bdd55e57126c82a427215b34c9985 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 16 Aug 2024 13:57:37 +0200 Subject: [PATCH 68/90] estimatorChecks: disable warning for imminent position failure if that is disabled (#23556) COM_POS_FS_EPH can be set to -1, in which case the actual failure eph is INFINITY. Signed-off-by: Silvan Fuhrer --- .../commander/HealthAndArmingChecks/checks/estimatorCheck.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index 888835ba7ef6..2b7c45318301 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -727,7 +727,7 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f _last_gpos_fail_time_us, !failsafe_flags.global_position_invalid); // Additional warning if the system is about to enter position-loss failsafe after dead-reckoning period - const float eph_critical = 2.5f * _param_com_pos_fs_eph.get(); // threshold used to trigger the navigation failsafe + const float eph_critical = 2.5f * lpos_eph_threshold; // threshold used to trigger the navigation failsafe const float gpos_critical_warning_thrld = math::max(0.9f * eph_critical, math::max(eph_critical - 10.f, 0.f)); estimator_status_flags_s estimator_status_flags; From 4f66410d24196fbaf62efe1a8c88e66048f6ff7c Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 16 Aug 2024 15:58:36 +0200 Subject: [PATCH 69/90] ROMFS gazebo iris opt flow: increase SENS_FLOW_MAXHGT to 15m (#23557) Signed-off-by: Silvan Fuhrer --- .../init.d-posix/airframes/1010_gazebo-classic_iris_opt_flow | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1010_gazebo-classic_iris_opt_flow b/ROMFS/px4fmu_common/init.d-posix/airframes/1010_gazebo-classic_iris_opt_flow index 38d9019adc70..d334bdb43088 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1010_gazebo-classic_iris_opt_flow +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1010_gazebo-classic_iris_opt_flow @@ -47,5 +47,5 @@ param set-default MPC_ALT_MODE 2 param set-default SENS_FLOW_ROT 6 param set-default SENS_FLOW_MINHGT 0.7 -param set-default SENS_FLOW_MAXHGT 3 +param set-default SENS_FLOW_MAXHGT 15 param set-default SENS_FLOW_MAXR 2.5 From ea673b0b5b945c590e07a88c9fc566f254986d9f Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 13 Aug 2024 09:55:51 +0200 Subject: [PATCH 70/90] navigator: check hagl failsafe centrally --- src/modules/navigator/mission_block.cpp | 26 ++++++++++++++++++++++++ src/modules/navigator/mission_block.h | 6 ++++++ src/modules/navigator/navigator_mode.cpp | 1 + src/modules/navigator/navigator_mode.h | 2 ++ src/modules/navigator/rtl.cpp | 2 ++ src/modules/navigator/rtl_direct.cpp | 10 --------- 6 files changed, 37 insertions(+), 10 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index f1f202231853..2659ad95cc57 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -1042,3 +1042,29 @@ void MissionBlock::updateAltToAvoidTerrainCollisionAndRepublishTriplet(mission_i _mission_item.altitude_is_relative = false; } } + +void MissionBlock::updateFailsafeChecks() +{ + updateMaxHaglFailsafe(); +} + +void MissionBlock::updateMaxHaglFailsafe() +{ + const float target_alt = _navigator->get_position_setpoint_triplet()->current.alt; + + if (_navigator->get_global_position()->terrain_alt_valid + && ((target_alt - _navigator->get_global_position()->terrain_alt) > _navigator->get_local_position()->hagl_max)) { + // Handle case where the altitude setpoint is above the maximum HAGL (height above ground level) + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Target altitude higher than max HAGL\t"); + events::send(events::ID("navigator_fail_max_hagl"), events::Log::Error, "Target altitude higher than max HAGL"); + + _navigator->trigger_hagl_failsafe(getNavigatorStateId()); + + // While waiting for a failsafe action from commander, keep the curren position + setLoiterItemFromCurrentPosition(&_mission_item); + + mission_item_to_position_setpoint(_mission_item, &_navigator->get_position_setpoint_triplet()->current); + + _navigator->set_position_setpoint_triplet_updated(); + } +} diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index edc1b0e843f5..c19fcfe11877 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -145,6 +145,8 @@ class MissionBlock : public NavigatorMode void set_align_mission_item(struct mission_item_s *const mission_item, const struct mission_item_s *const mission_item_next) const; + void updateFailsafeChecks() override; + protected: /** * @brief heading mode for setting navigation items @@ -249,4 +251,8 @@ class MissionBlock : public NavigatorMode bool _payload_deploy_ack_successful{false}; // Flag to keep track of whether we received an acknowledgement for a successful payload deployment hrt_abstime _payload_deployed_time{0}; // Last payload deployment start time to handle timeouts float _payload_deploy_timeout_s{0.0f}; // Timeout for payload deployment in Mission class, to prevent endless loop if successful deployment ack is never received + +private: + void updateMaxHaglFailsafe(); + }; diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index 392ca6abb458..1c0882923402 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -61,6 +61,7 @@ NavigatorMode::run(bool active) } else { /* periodic updates when active */ on_active(); + updateFailsafeChecks(); } } else { diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h index ca418e5b2d9a..9164a92b92fc 100644 --- a/src/modules/navigator/navigator_mode.h +++ b/src/modules/navigator/navigator_mode.h @@ -80,6 +80,8 @@ class NavigatorMode */ virtual void on_active(); + virtual void updateFailsafeChecks() {}; + protected: Navigator *_navigator{nullptr}; diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 6fef45f9df6e..bb3276b37818 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -283,10 +283,12 @@ void RTL::on_active() case RtlType::RTL_MISSION_FAST_REVERSE: case RtlType::RTL_DIRECT_MISSION_LAND: _rtl_mission_type_handle->on_active(); + _rtl_mission_type_handle->updateFailsafeChecks(); break; case RtlType::RTL_DIRECT: _rtl_direct.on_active(); + _rtl_direct.updateFailsafeChecks(); break; default: diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index f26df56530d2..145b99d0d1db 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -158,16 +158,6 @@ void RtlDirect::set_rtl_item() { position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - if (_global_pos_sub.get().terrain_alt_valid - && ((_rtl_alt - _global_pos_sub.get().terrain_alt) > _navigator->get_local_position()->hagl_max)) { - // Handle case where the RTL altidude is above the maximum HAGL and land in place instead of RTL - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return alt higher than max HAGL\t"); - events::send(events::ID("rtl_fail_max_hagl"), events::Log::Error, "RTL: return alt higher than max HAGL"); - - _navigator->trigger_hagl_failsafe(getNavigatorStateId()); - _rtl_state = RTLState::IDLE; - } - const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, _global_pos_sub.get().lat, _global_pos_sub.get().lon); const float loiter_altitude = math::min(_land_approach.height_m, _rtl_alt); From ad1d9e13128b433a0d7909050b2fec590951c4f7 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 16 Aug 2024 14:05:06 +0200 Subject: [PATCH 71/90] failsafe: do not add additional hold delay if failsafe action is hold --- src/modules/commander/failsafe/framework.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/failsafe/framework.cpp b/src/modules/commander/failsafe/framework.cpp index 1eddc8a7f81c..729205ebe89e 100644 --- a/src/modules/commander/failsafe/framework.cpp +++ b/src/modules/commander/failsafe/framework.cpp @@ -477,7 +477,7 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s // Check if we should enter delayed Hold if (_current_delay > 0 && !_user_takeover_active && allow_user_takeover <= UserTakeoverAllowed::AlwaysModeSwitchOnly - && selected_action != Action::Disarm && selected_action != Action::Terminate) { + && selected_action != Action::Disarm && selected_action != Action::Terminate && selected_action != Action::Hold) { returned_state.delayed_action = selected_action; selected_action = Action::Hold; allow_user_takeover = UserTakeoverAllowed::AlwaysModeSwitchOnly; From ea0ef154d8d4a718ab07629f7477144247885e79 Mon Sep 17 00:00:00 2001 From: Vilius Date: Sat, 17 Aug 2024 22:59:18 +0300 Subject: [PATCH 72/90] Fixes upload.sh for arkv6x (#23561) --- Tools/upload.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/upload.sh b/Tools/upload.sh index 25d41e389f76..2a6416ed374d 100755 --- a/Tools/upload.sh +++ b/Tools/upload.sh @@ -15,7 +15,7 @@ SERIAL_PORTS="/dev/tty.usbmodemPX*,/dev/tty.usbmodem*" fi if [ $SYSTYPE = "Linux" ]; then -SERIAL_PORTS="/dev/serial/by-id/*_PX4_*,/dev/serial/by-id/usb-3D_Robotics*,/dev/serial/by-id/usb-The_Autopilot*,/dev/serial/by-id/usb-Bitcraze*,/dev/serial/by-id/pci-Bitcraze*,/dev/serial/by-id/usb-Gumstix*,/dev/serial/by-id/usb-UVify*,/dev/serial/by-id/usb-ArduPilot*,/dev/serial/by-id/ARK*," +SERIAL_PORTS="/dev/serial/by-id/*_PX4_*,/dev/serial/by-id/usb-3D_Robotics*,/dev/serial/by-id/usb-The_Autopilot*,/dev/serial/by-id/usb-Bitcraze*,/dev/serial/by-id/pci-Bitcraze*,/dev/serial/by-id/usb-Gumstix*,/dev/serial/by-id/usb-UVify*,/dev/serial/by-id/usb-ArduPilot*,/dev/serial/by-id/usb-ARK*," fi if [[ $SYSTYPE = *"CYGWIN"* ]]; then From 435e9665b31f51478b81c05a5e5bc4f348d49e8c Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 19 Aug 2024 07:51:33 +0200 Subject: [PATCH 73/90] RTL: cone: never climb more than to RTL_RETURN_ALT (#23558) This is to prevent that a large NAV_ACC_RAD leads to very high return altitudes. Signed-off-by: Silvan Fuhrer --- src/modules/navigator/rtl.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index bb3276b37818..63e2ab185ff1 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -530,13 +530,14 @@ float RTL::calculate_return_alt_from_cone_half_angle(const PositionYawSetpoint & // avoid the vehicle touching the ground while still moving horizontally. const float return_altitude_min_outside_acceptance_rad_amsl = rtl_position.alt + 2.0f * _param_nav_acc_rad.get(); - float return_altitude_amsl = rtl_position.alt + _param_rtl_return_alt.get(); + const float max_return_altitude = rtl_position.alt + _param_rtl_return_alt.get(); + + float return_altitude_amsl = max_return_altitude; if (destination_dist <= _param_nav_acc_rad.get()) { return_altitude_amsl = rtl_position.alt + 2.0f * destination_dist; } else { - if (destination_dist <= _param_rtl_min_dist.get()) { // constrain cone half angle to meaningful values. All other cases are already handled above. @@ -551,7 +552,7 @@ float RTL::calculate_return_alt_from_cone_half_angle(const PositionYawSetpoint & return_altitude_amsl = max(return_altitude_amsl, return_altitude_min_outside_acceptance_rad_amsl); } - return max(return_altitude_amsl, _global_pos_sub.get().alt); + return constrain(return_altitude_amsl, _global_pos_sub.get().alt, max_return_altitude); } void RTL::init_rtl_mission_type() From e29a36adb46aed60e1166860ebb490400d962d96 Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Mon, 19 Aug 2024 08:01:43 +0200 Subject: [PATCH 74/90] Landing horizontal velocity compensation / unsteady landing (#23546) * initial working * implemented feedback --- .../flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp | 2 +- src/modules/navigator/land.cpp | 7 ++++++- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index dad05e3c31db..b2bb5632b7bf 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -452,7 +452,7 @@ bool FlightTaskAuto::_evaluateTriplets() _triplet_prev_wp(2) = -(_sub_triplet_setpoint.get().previous.alt - _reference_altitude); } else { - _triplet_prev_wp = _position; + _triplet_prev_wp = _triplet_target; } _prev_was_valid = _sub_triplet_setpoint.get().previous.valid; diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index 74f54fbb079a..8277378a6670 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -57,8 +57,13 @@ Land::on_activation() /* convert mission item to current setpoint */ struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - pos_sp_triplet->previous.valid = false; + + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + _navigator->calculate_breaking_stop(_mission_item.lat, _mission_item.lon); + } + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->previous.valid = false; pos_sp_triplet->next.valid = false; _navigator->set_position_setpoint_triplet_updated(); From 7e45f4915208b2d02e05e5741ff7456cf92120f9 Mon Sep 17 00:00:00 2001 From: jmackay2 <1.732mackay@gmail.com> Date: Mon, 19 Aug 2024 02:54:57 -0400 Subject: [PATCH 75/90] Update GZBridge to be able to use gazebo airspeed. Add quadtailsitter. (#23455) * Update GZBridge to be able to use gazebo airspeed. Add gz quadtailsitter. * Fix formatting --------- Co-authored-by: jmackay2 --- .../1045_gazebo-classic_quadtailsitter | 2 +- .../airframes/4004_gz_standard_vtol | 2 - .../airframes/4014_gz_quadtailsitter | 92 +++++++++++++++++++ .../init.d-posix/airframes/CMakeLists.txt | 1 + src/modules/simulation/gz_bridge/GZBridge.cpp | 16 ++-- src/modules/simulation/gz_bridge/GZBridge.hpp | 4 +- .../sensor_airspeed_sim/SensorAirspeedSim.cpp | 2 +- 7 files changed, 103 insertions(+), 16 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/4014_gz_quadtailsitter diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1045_gazebo-classic_quadtailsitter b/ROMFS/px4fmu_common/init.d-posix/airframes/1045_gazebo-classic_quadtailsitter index 87afb523b1a7..9539ba6d7402 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1045_gazebo-classic_quadtailsitter +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1045_gazebo-classic_quadtailsitter @@ -33,7 +33,7 @@ param set-default PWM_MAIN_FUNC3 103 param set-default PWM_MAIN_FUNC4 104 param set-default PWM_MAIN_FUNC5 0 -parm set-default FD_FAIL_R 70 +param set-default FD_FAIL_R 70 param set-default FW_P_TC 0.6 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 96bb25d69a52..580b6da368de 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 @@ -14,9 +14,7 @@ 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 # TODO: Enable motor failure detection when the # VTOL no longer reports 0A for all ESCs in SITL diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4014_gz_quadtailsitter b/ROMFS/px4fmu_common/init.d-posix/airframes/4014_gz_quadtailsitter new file mode 100644 index 000000000000..699d1dacc521 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4014_gz_quadtailsitter @@ -0,0 +1,92 @@ +#!/bin/sh +# +# @name Quadrotor + Tailsitter +# +# @type VTOL Quad Tailsitter +# + +. ${R}etc/init.d/rc.vtol_defaults + +PX4_SIMULATOR=${PX4_SIMULATOR:=gz} +PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=quadtailsitter} + +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 0 + +param set-default MAV_TYPE 20 + +param set-default CA_AIRFRAME 4 + +param set-default CA_ROTOR_COUNT 4 +param set-default CA_ROTOR0_PX 0.15 +param set-default CA_ROTOR0_PY 0.23 +param set-default CA_ROTOR0_KM 0.05 +param set-default CA_ROTOR1_PX -0.15 +param set-default CA_ROTOR1_PY -0.23 +param set-default CA_ROTOR1_KM 0.05 +param set-default CA_ROTOR2_PX 0.15 +param set-default CA_ROTOR2_PY -0.23 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -0.15 +param set-default CA_ROTOR3_PY 0.23 +param set-default CA_ROTOR3_KM -0.05 + +param set-default CA_SV_CS_COUNT 0 + +param set-default SIM_GZ_EC_FUNC1 101 +param set-default SIM_GZ_EC_MIN1 10 +param set-default SIM_GZ_EC_MAX1 1500 +param set-default SIM_GZ_EC_FUNC2 102 +param set-default SIM_GZ_EC_MIN2 10 +param set-default SIM_GZ_EC_MAX2 1500 +param set-default SIM_GZ_EC_FUNC3 103 +param set-default SIM_GZ_EC_MIN3 10 +param set-default SIM_GZ_EC_MAX3 1500 +param set-default SIM_GZ_EC_FUNC4 104 +param set-default SIM_GZ_EC_MIN4 10 +param set-default SIM_GZ_EC_MAX4 1500 + +param set-default FD_FAIL_R 70 + +param set-default FW_P_TC 0.6 + +param set-default FW_PR_I 0.3 +param set-default FW_PR_P 0.5 +param set-default FW_PSP_OFF 2 +param set-default FW_RR_FF 0.1 +param set-default FW_RR_I 0.1 +param set-default FW_RR_P 0.2 +param set-default FW_YR_FF 0 # make yaw rate controller very weak, only keep default P +param set-default FW_YR_I 0 +param set-default FW_THR_TRIM 0.35 +param set-default FW_THR_MAX 0.8 +param set-default FW_THR_MIN 0.05 +param set-default FW_T_CLMB_MAX 6 +param set-default FW_T_HRATE_FF 0.5 +param set-default FW_T_SINK_MAX 3 +param set-default FW_T_SINK_MIN 1.6 +param set-default FW_AIRSPD_STALL 10 +param set-default FW_AIRSPD_MIN 14 +param set-default FW_AIRSPD_TRIM 18 +param set-default FW_AIRSPD_MAX 22 + +param set-default MC_AIRMODE 2 +param set-default MAN_ARM_GESTURE 0 # required for yaw airmode +param set-default MC_ROLL_P 3 +param set-default MC_PITCH_P 3 +param set-default MC_ROLLRATE_P 0.3 +param set-default MC_PITCHRATE_P 0.3 + +param set-default VT_ARSP_TRANS 15 +param set-default VT_B_TRANS_DUR 5 +param set-default VT_FW_DIFTHR_EN 7 +param set-default VT_FW_DIFTHR_S_Y 1 +param set-default VT_F_TRANS_DUR 1.5 +param set-default VT_TYPE 0 + +param set-default WV_EN 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index 9235b2e66340..badd712b063f 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -85,6 +85,7 @@ px4_add_romfs_files( 4011_gz_lawnmower 4012_gz_rover_ackermann 4013_gz_x500_lidar + 4014_gz_quadtailsitter 6011_gazebo-classic_typhoon_h480 6011_gazebo-classic_typhoon_h480.post diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 3464c628694c..6b4042da8bb3 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -203,17 +203,15 @@ int GZBridge::init() PX4_WARN("failed to subscribe to %s", laser_scan_topic.c_str()); } -#if 0 - // Airspeed: /world/$WORLD/model/$MODEL/link/airspeed_link/sensor/air_speed/air_speed - std::string airpressure_topic = "/world/" + _world_name + "/model/" + _model_name + - "/link/airspeed_link/sensor/air_speed/air_speed"; + // Airspeed: /world/$WORLD/model/$MODEL/link/base_link/sensor/airspeed_sensor/air_speed + std::string airspeed_topic = "/world/" + _world_name + "/model/" + _model_name + + "/link/base_link/sensor/airspeed_sensor/air_speed"; - if (!_node.Subscribe(airpressure_topic, &GZBridge::airspeedCallback, this)) { - PX4_ERR("failed to subscribe to %s", airpressure_topic.c_str()); + if (!_node.Subscribe(airspeed_topic, &GZBridge::airspeedCallback, this)) { + PX4_ERR("failed to subscribe to %s", airspeed_topic.c_str()); return PX4_ERROR; } -#endif // Air pressure: /world/$WORLD/model/$MODEL/link/base_link/sensor/air_pressure_sensor/air_pressure std::string air_pressure_topic = "/world/" + _world_name + "/model/" + _model_name + "/link/base_link/sensor/air_pressure_sensor/air_pressure"; @@ -426,8 +424,7 @@ void GZBridge::barometerCallback(const gz::msgs::FluidPressure &air_pressure) pthread_mutex_unlock(&_node_mutex); } -#if 0 -void GZBridge::airspeedCallback(const gz::msgs::AirSpeedSensor &air_speed) +void GZBridge::airspeedCallback(const gz::msgs::AirSpeed &air_speed) { if (hrt_absolute_time() == 0) { return; @@ -452,7 +449,6 @@ void GZBridge::airspeedCallback(const gz::msgs::AirSpeedSensor &air_speed) pthread_mutex_unlock(&_node_mutex); } -#endif void GZBridge::imuCallback(const gz::msgs::IMU &imu) { diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 8a832f7961b2..824c7d471633 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -102,7 +102,7 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S void clockCallback(const gz::msgs::Clock &clock); - // void airspeedCallback(const gz::msgs::AirSpeedSensor &air_pressure); + 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); @@ -123,7 +123,7 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S // Subscriptions uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - //uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; + uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance)}; uORB::Publication _angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)}; uORB::Publication _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)}; diff --git a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp index 7b0d9556cadf..86fbbf29cd4c 100644 --- a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp +++ b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp @@ -195,7 +195,7 @@ int SensorAirspeedSim::print_usage(const char *reason) )DESCR_STR"); - PRINT_MODULE_USAGE_NAME("sensor_arispeed_sim", "system"); + PRINT_MODULE_USAGE_NAME("sensor_airspeed_sim", "system"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); From 4d21110cfb938455b21ad16c8463291094e9ec39 Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Mon, 19 Aug 2024 13:36:04 +0200 Subject: [PATCH 76/90] Documentation - improved GCS parameter readablity (#23376) improved GCS parameter description Co-authored-by: Hamish Willee Co-authored-by: Silvan Fuhrer --- .../mc_pos_control/multicopter_altitude_mode_params.c | 2 +- .../mc_pos_control/multicopter_autonomous_params.c | 2 +- .../mc_pos_control/multicopter_position_mode_params.c | 11 +++++++---- 3 files changed, 9 insertions(+), 6 deletions(-) diff --git a/src/modules/mc_pos_control/multicopter_altitude_mode_params.c b/src/modules/mc_pos_control/multicopter_altitude_mode_params.c index 3f654fade71a..9174b1ef0d83 100644 --- a/src/modules/mc_pos_control/multicopter_altitude_mode_params.c +++ b/src/modules/mc_pos_control/multicopter_altitude_mode_params.c @@ -95,7 +95,7 @@ PARAM_DEFINE_INT32(MPC_ALT_MODE, 2); /** * Maximum horizontal velocity for which position hold is enabled (use 0 to disable check) * - * Only used with MPC_POS_MODE 0 or MPC_ALT_MODE 2 + * Only used with MPC_POS_MODE Direct velocity or MPC_ALT_MODE 2 * * @unit m/s * @min 0 diff --git a/src/modules/mc_pos_control/multicopter_autonomous_params.c b/src/modules/mc_pos_control/multicopter_autonomous_params.c index 8c97e779be32..e2e453d975ed 100644 --- a/src/modules/mc_pos_control/multicopter_autonomous_params.c +++ b/src/modules/mc_pos_control/multicopter_autonomous_params.c @@ -76,7 +76,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_V_AUTO_DN, 1.5f); /** * Acceleration for autonomous and for manual modes * - * When piloting manually, this parameter is only used in MPC_POS_MODE 4. + * When piloting manually, this parameter is only used in MPC_POS_MODE Acceleration based. * * @unit m/s^2 * @min 2 diff --git a/src/modules/mc_pos_control/multicopter_position_mode_params.c b/src/modules/mc_pos_control/multicopter_position_mode_params.c index 56adad1144bf..a65ef4f5713d 100644 --- a/src/modules/mc_pos_control/multicopter_position_mode_params.c +++ b/src/modules/mc_pos_control/multicopter_position_mode_params.c @@ -35,12 +35,15 @@ * Position/Altitude mode variant * * The supported sub-modes are: - * 0 Sticks directly map to velocity setpoints without smoothing. + * - "Direct velocity": + * Sticks directly map to velocity setpoints without smoothing. * Also applies to vertical direction and Altitude mode. * Useful for velocity control tuning. - * 3 Sticks map to velocity but with maximum acceleration and jerk limits based on + * - "Smoothed velocity": + * Sticks map to velocity but with maximum acceleration and jerk limits based on * jerk optimized trajectory generator (different algorithm than 1). - * 4 Sticks map to acceleration and there's a virtual brake drag + * - "Acceleration based": + * Sticks map to acceleration and there's a virtual brake drag * * @value 0 Direct velocity * @value 3 Smoothed velocity @@ -122,7 +125,7 @@ PARAM_DEFINE_FLOAT(MPC_ACC_HOR_MAX, 5.f); * * Setting this to the maximum value essentially disables the limit. * - * Only used with smooth MPC_POS_MODE 3 and 4. + * Only used with smooth MPC_POS_MODE Smoothed velocity and Acceleration based. * * @unit m/s^3 * @min 0.5 From 0481c04b2b274306d9505c00207c36ab26733dd9 Mon Sep 17 00:00:00 2001 From: Alexis Guijarro Date: Thu, 15 Aug 2024 12:51:24 -0600 Subject: [PATCH 77/90] Nuttx with backport (stm32h7x3x): Add External Power Supply option --- 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 5d74bc138955..39bb38d82b12 160000 --- a/platforms/nuttx/NuttX/nuttx +++ b/platforms/nuttx/NuttX/nuttx @@ -1 +1 @@ -Subproject commit 5d74bc138955e6f010a38e0f87f34e9a9019aecc +Subproject commit 39bb38d82b128a57d0ca40c25ba3ad6601ec0a83 From 746ae257689610f7d244e8c7c791450f9394a47d Mon Sep 17 00:00:00 2001 From: Ramon Roche Date: Mon, 19 Aug 2024 07:41:25 -0700 Subject: [PATCH 78/90] ci: replace build workflows (#23550) --- .editorconfig | 2 +- .github/workflows/build_all_targets.yml | 83 +++++++++++ .github/workflows/compile_linux.yml | 58 -------- .github/workflows/compile_linux_arm64.yml | 54 ------- .github/workflows/compile_nuttx.yml | 137 ----------------- Tools/ci_build_all_runner.sh | 19 +++ Tools/generate_board_targets_json.py | 170 +++++++++++++++++++++- 7 files changed, 268 insertions(+), 255 deletions(-) create mode 100644 .github/workflows/build_all_targets.yml delete mode 100644 .github/workflows/compile_linux.yml delete mode 100644 .github/workflows/compile_linux_arm64.yml delete mode 100644 .github/workflows/compile_nuttx.yml create mode 100755 Tools/ci_build_all_runner.sh diff --git a/.editorconfig b/.editorconfig index 683f0fdc5159..1ac25a4f5860 100644 --- a/.editorconfig +++ b/.editorconfig @@ -9,6 +9,6 @@ tab_width = 8 # Not in the official standard, but supported by many editors max_line_length = 120 -[*.yaml] +[*.yaml, *.yml] indent_style = space indent_size = 2 diff --git a/.github/workflows/build_all_targets.yml b/.github/workflows/build_all_targets.yml new file mode 100644 index 000000000000..119236b3198e --- /dev/null +++ b/.github/workflows/build_all_targets.yml @@ -0,0 +1,83 @@ +name: Build all targets + +on: + push: + branches: + - 'main' + - 'stable' + - 'beta' + - 'release/*' + pull_request: + branches: + - '*' + +jobs: + group_targets: + name: Scan for Board Targets + runs-on: ubuntu-latest + outputs: + matrix: ${{ steps.set-matrix.outputs.matrix }} + timestamp: ${{ steps.set-timestamp.outputs.timestamp }} + steps: + - uses: actions/checkout@v4 + + - name: Install Python Dependencies + uses: py-actions/py-dependency-install@v4 + with: + path: "./Tools/setup/requirements.txt" + + - id: set-matrix + run: echo "::set-output name=matrix::$(./Tools/generate_board_targets_json.py --group)" + + - id: set-timestamp + run: echo "::set-output name=timestamp::$(date +"%Y%m%d%H%M%S")" + + setup: + name: ${{ matrix.group }} + runs-on: ubuntu-latest + needs: group_targets + strategy: + matrix: ${{ fromJson(needs.group_targets.outputs.matrix) }} + container: + image: ${{ matrix.container }} + steps: + - uses: actions/checkout@v4 + with: + fetch-depth: 0 + + - name: ownership workaround + run: git config --system --add safe.directory '*' + + - name: ccache setup keys + uses: actions/cache@v4 + with: + path: ~/.ccache + key: ${{ matrix.group }}-ccache-${{ needs.group_targets.outputs.timestamp }} + restore-keys: ${{ matrix.group }}-ccache-${{ needs.group_targets.outputs.timestamp }} + + - name: setup ccache + run: | + mkdir -p ~/.ccache + echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf + echo "compression = true" >> ~/.ccache/ccache.conf + echo "compression_level = 6" >> ~/.ccache/ccache.conf + echo "max_size = 120M" >> ~/.ccache/ccache.conf + echo "hash_dir = false" >> ~/.ccache/ccache.conf + ccache -s + ccache -z + + - name: build target group + run: | + ./Tools/ci_build_all_runner.sh ${{matrix.targets}} + + - name: Upload px4 package + uses: actions/upload-artifact@v4 + with: + name: px4_${{matrix.group}}_build_artifacts + path: | + build/**/*.px4 + build/**/*.bin + compression-level: 0 + + - name: ccache post-run + run: ccache -s diff --git a/.github/workflows/compile_linux.yml b/.github/workflows/compile_linux.yml deleted file mode 100644 index 23e46deab7f3..000000000000 --- a/.github/workflows/compile_linux.yml +++ /dev/null @@ -1,58 +0,0 @@ -name: Compile Linux Targets - -on: - push: - branches: - - 'main' - - 'stable' - - 'beta' - - 'release/*' - pull_request: - branches: - - '*' - -jobs: - build: - runs-on: ubuntu-latest - container: px4io/px4-dev-armhf:2023-06-26 - strategy: - matrix: - config: [ - beaglebone_blue_default, - emlid_navio2_default, - px4_raspberrypi_default, - scumaker_pilotpi_default, - ] - steps: - - uses: actions/checkout@v1 - with: - token: ${{secrets.ACCESS_TOKEN}} - - name: ownership workaround - run: git config --system --add safe.directory '*' - - name: Prepare ccache timestamp - id: ccache_cache_timestamp - shell: cmake -P {0} - run: | - string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC) - message("::set-output name=timestamp::${current_date}") - - name: ccache cache files - uses: actions/cache@v2 - with: - path: ~/.ccache - key: ${{matrix.config}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} - restore-keys: ${{matrix.config}}-ccache- - - name: setup ccache - run: | - mkdir -p ~/.ccache - echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf - echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 100M" >> ~/.ccache/ccache.conf - echo "hash_dir = false" >> ~/.ccache/ccache.conf - ccache -s - ccache -z - - - name: make ${{matrix.config}} - run: make ${{matrix.config}} - - name: ccache post-run - run: ccache -s diff --git a/.github/workflows/compile_linux_arm64.yml b/.github/workflows/compile_linux_arm64.yml deleted file mode 100644 index 5c1c318b411e..000000000000 --- a/.github/workflows/compile_linux_arm64.yml +++ /dev/null @@ -1,54 +0,0 @@ -name: Compile Linux ARM64 Targets - -on: - push: - branches: - - 'main' - - 'stable' - - 'beta' - - 'release/*' - pull_request: - branches: - - '*' - -jobs: - build: - runs-on: ubuntu-latest - container: px4io/px4-dev-aarch64:2022-08-12 - strategy: - matrix: - config: [ - scumaker_pilotpi_arm64, - ] - steps: - - uses: actions/checkout@v1 - with: - token: ${{secrets.ACCESS_TOKEN}} - - - name: Prepare ccache timestamp - id: ccache_cache_timestamp - shell: cmake -P {0} - run: | - string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC) - message("::set-output name=timestamp::${current_date}") - - name: ccache cache files - uses: actions/cache@v2 - with: - path: ~/.ccache - key: ${{matrix.config}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} - restore-keys: ${{matrix.config}}-ccache- - - name: setup ccache - run: | - mkdir -p ~/.ccache - echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf - echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 100M" >> ~/.ccache/ccache.conf - echo "hash_dir = false" >> ~/.ccache/ccache.conf - ccache -s - ccache -z - - - name: make ${{matrix.config}} - run: make ${{matrix.config}} - - name: ccache post-run - run: ccache -s diff --git a/.github/workflows/compile_nuttx.yml b/.github/workflows/compile_nuttx.yml deleted file mode 100644 index cc44c55ea252..000000000000 --- a/.github/workflows/compile_nuttx.yml +++ /dev/null @@ -1,137 +0,0 @@ -name: Compile Nuttx Targets - -on: - push: - branches: - - 'main' - - 'stable' - - 'beta' - - 'release/*' - pull_request: - branches: - - '*' - -jobs: - build: - runs-on: ubuntu-latest - container: px4io/px4-dev-nuttx-focal:2022-08-12 - strategy: - fail-fast: false - matrix: - config: [ - 3dr_ctrl-zero-h7-oem-revg, - airmind_mindpx-v2, - ark_can-flow, - ark_can-gps, - ark_can-rtk-gps, - ark_cannode, - ark_fmu-v6x, - ark_pi6x, - ark_septentrio-gps, - atl_mantis-edu, - av_x-v1, - bitcraze_crazyflie, - bitcraze_crazyflie21, - cuav_can-gps-v1, - cuav_nora, - cuav_x7pro, - cubepilot_cubeorange, - cubepilot_cubeorangeplus, - cubepilot_cubeyellow, - diatone_mamba-f405-mk2, - freefly_can-rtk-gps, - holybro_can-gps-v1, - holybro_durandal-v1, - holybro_kakutef7, - holybro_kakuteh7, - holybro_pix32v5, - matek_gnss-m9n-f4, - matek_h743, - matek_h743-mini, - matek_h743-slim, - modalai_fc-v1, - modalai_fc-v2, - mro_ctrl-zero-classic, - mro_ctrl-zero-f7, - mro_ctrl-zero-f7-oem, - mro_ctrl-zero-h7, - mro_ctrl-zero-h7-oem, - mro_pixracerpro, - mro_x21, - mro_x21-777, - nxp_fmuk66-e, - nxp_fmuk66-v3, - nxp_mr-canhubk3, - nxp_ucans32k146, - omnibus_f4sd, - px4_fmu-v2, - px4_fmu-v3, - px4_fmu-v4, - px4_fmu-v4pro, - px4_fmu-v5, - px4_fmu-v5x, - px4_fmu-v6c, - px4_fmu-v6u, - px4_fmu-v6x, - px4_fmu-v6xrt, - raspberrypi_pico, - sky-drones_smartap-airlink, - spracing_h7extreme, - uvify_core, - siyi_n7 - ] - steps: - - uses: actions/checkout@v1 - with: - token: ${{secrets.ACCESS_TOKEN}} - - - name: Prepare ccache timestamp - id: ccache_cache_timestamp - shell: cmake -P {0} - run: | - string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC) - message("::set-output name=timestamp::${current_date}") - - name: ccache cache files - uses: actions/cache@v2 - with: - path: ~/.ccache - key: ${{matrix.config}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} - restore-keys: ${{matrix.config}}-ccache- - - name: setup ccache - run: | - mkdir -p ~/.ccache - echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf - echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 120M" >> ~/.ccache/ccache.conf - echo "hash_dir = false" >> ~/.ccache/ccache.conf - ccache -s - ccache -z - - - name: make all_variants_${{matrix.config}} - run: make all_variants_${{matrix.config}} - timeout-minutes: 45 - - name: make ${{matrix.config}} bloaty_compileunits - run: make ${{matrix.config}} bloaty_compileunits || true - - name: make ${{matrix.config}} bloaty_inlines - run: make ${{matrix.config}} bloaty_inlines || true - - name: make ${{matrix.config}} bloaty_segments - run: make ${{matrix.config}} bloaty_segments || true - - name: make ${{matrix.config}} bloaty_symbols - run: make ${{matrix.config}} bloaty_symbols || true - - name: make ${{matrix.config}} bloaty_templates - run: make ${{matrix.config}} bloaty_templates || true - - name: make ${{matrix.config}} bloaty_ram - run: make ${{matrix.config}} bloaty_ram || true - - name: make ${{matrix.config}} bloaty_compare_master - run: make ${{matrix.config}} bloaty_compare_master || true - - name: ccache post-run - run: ccache -s - - - name: Upload px4 package - uses: actions/upload-artifact@v2 - with: - name: px4_package_${{matrix.config}} - path: | - build/**/*.px4 - build/**/*.bin diff --git a/Tools/ci_build_all_runner.sh b/Tools/ci_build_all_runner.sh new file mode 100755 index 000000000000..bfb77e2623a9 --- /dev/null +++ b/Tools/ci_build_all_runner.sh @@ -0,0 +1,19 @@ +#!/bin/bash +# This script is meant to be used by the build_all.yml workflow in a github runner +# Please only modify if you know what you are doing +set -e + +echo "### :clock1: Build Times" >> $GITHUB_STEP_SUMMARY +targets=$1 +for target in ${targets//,/ } +do + echo "::group::Building: [${target}]" + start=$(date +%s) + make $target + stop=$(date +%s) + diff=$(($stop-$start)) + build_time="$(($diff /60/60))h $(($diff /60))m $(($diff % 60))s elapsed" + echo -e "\033[0;32mBuild Time: [$build_time]" + echo "* **$target** - $build_time" >> $GITHUB_STEP_SUMMARY + echo "::endgroup::" +done diff --git a/Tools/generate_board_targets_json.py b/Tools/generate_board_targets_json.py index 931d5c14776f..649b3fceefa6 100755 --- a/Tools/generate_board_targets_json.py +++ b/Tools/generate_board_targets_json.py @@ -23,11 +23,14 @@ help='Verbose Output') parser.add_argument('-p', '--pretty', dest='pretty', action='store_true', help='Pretty output instead of a single line') +parser.add_argument('-g', '--groups', dest='group', action='store_true', + help='Groups targets') args = parser.parse_args() verbose = args.verbose build_configs = [] +grouped_targets = {} excluded_boards = ['modalai_voxl2', 'px4_ros2'] # TODO: fix and enable excluded_manufacturers = ['atlflight'] excluded_platforms = ['qurt'] @@ -37,10 +40,27 @@ 'uavcanv1', # TODO: fix and enable ] +github_action_config = { 'include': build_configs } +extra_args = {} +if args.pretty: + extra_args['indent'] = 2 + +def chunks(arr, size): + # splits array into parts + for i in range(0, len(arr), size): + yield arr[i:i + size] + +def comma_targets(targets): + # turns array of targets into a comma split string + return ",".join(targets) + def process_target(px4board_file, target_name): + # reads through the board file and grabs + # useful information for building ret = None platform = None toolchain = None + group = None if px4board_file.endswith("default.px4board") or \ px4board_file.endswith("recovery.px4board") or \ @@ -63,22 +83,34 @@ def process_target(px4board_file, target_name): # get the container based on the platform and toolchain if platform == 'posix': container = 'px4io/px4-dev-base-focal:2021-09-08' + group = 'base' if toolchain: if toolchain.startswith('aarch64'): container = 'px4io/px4-dev-aarch64:2022-08-12' + group = 'aarch64' elif toolchain == 'arm-linux-gnueabihf': container = 'px4io/px4-dev-armhf:2023-06-26' + group = 'armhf' else: if verbose: print(f'unmatched toolchain: {toolchain}') elif platform == 'nuttx': container = 'px4io/px4-dev-nuttx-focal:2022-08-12' + group = 'nuttx' else: if verbose: print(f'unmatched platform: {platform}') ret = {'target': target_name, 'container': container} + if(args.group): + ret['arch'] = group return ret +# Look for board targets in the ./boards directory +if(verbose): + print("=======================") + print("= scanning for boards =") + print("=======================") + for manufacturer in os.scandir(os.path.join(source_dir, 'boards')): if not manufacturer.is_dir(): continue @@ -105,12 +137,140 @@ def process_target(px4board_file, target_name): if verbose: print(f'excluding label {label} ({target_name})') continue target = process_target(files.path, target_name) + if (args.group and target is not None): + if (target['arch'] not in grouped_targets): + grouped_targets[target['arch']] = {} + grouped_targets[target['arch']]['container'] = target['container'] + grouped_targets[target['arch']]['manufacturers'] = {} + if(manufacturer.name not in grouped_targets[target['arch']]['manufacturers']): + grouped_targets[target['arch']]['manufacturers'][manufacturer.name] = {} + grouped_targets[target['arch']]['manufacturers'][manufacturer.name] = [] + grouped_targets[target['arch']]['manufacturers'][manufacturer.name].append(target_name) if target is not None: build_configs.append(target) +if(verbose): + import pprint + print("============================") + print("= Boards found in ./boards =") + print("============================") + pprint.pp(grouped_targets) -github_action_config = { 'include': build_configs } -extra_args = {} -if args.pretty: - extra_args['indent'] = 2 -print(json.dumps(github_action_config, **extra_args)) +if (args.group): + # if we are using this script for grouping builds + # we loop trough the manufacturers list and split their targets + # if a manufacturer has more than a LIMIT of boards then we split that + # into sub groups such as "arch-manufacturer name-index" + # example: + # nuttx-px4-0 + # nuttx-px4-1 + # nuttx-px4-2 + # nuttx-ark-0 + # nuttx-ark-1 + # if the manufacturer doesn't have more targets than LIMIT then we add + # them to a generic group with the following structure "arch-index" + # example: + # nuttx-0 + # nuttx-1 + final_groups = [] + temp_group = [] + group_number = {} + last_man = '' + last_arch = '' + SPLIT_LIMIT = 10 + LOWER_LIMIT = 5 + for arch in grouped_targets: + if(last_arch == ''): + last_arch = arch + if(arch not in group_number): + group_number[arch] = 0 + + if(last_arch != arch and len(temp_group) > 0): + group_name = last_arch + "-" + str(group_number[last_arch]) + group_number[last_arch] += 1 + targets = comma_targets(temp_group) + final_groups.append({ + "container": grouped_targets[last_arch]['container'], + "targets": targets, + "arch": last_arch, + "group": group_name, + "len": len(temp_group) + }) + last_arch = arch + temp_group = [] + for man in grouped_targets[arch]['manufacturers']: + for tar in grouped_targets[arch]['manufacturers'][man]: + if(last_man != man): + man_len = len(grouped_targets[arch]['manufacturers'][man]) + if(man_len > LOWER_LIMIT and man_len < (SPLIT_LIMIT + 1)): + # Manufacturers can have their own group + group_name = arch + "-" + man + targets = comma_targets(grouped_targets[arch]['manufacturers'][man]) + last_man = man + final_groups.append({ + "container": grouped_targets[arch]['container'], + "targets": targets, + "arch": arch, + "group": group_name, + "len": len(grouped_targets[arch]['manufacturers'][man]) + }) + elif(man_len >= (SPLIT_LIMIT + 1)): + # Split big man groups into subgroups + # example: Pixhawk + chunk_limit = SPLIT_LIMIT + chunk_counter = 0 + for chunk in chunks(grouped_targets[arch]['manufacturers'][man], chunk_limit): + group_name = arch + "-" + man + "-" + str(chunk_counter) + targets = comma_targets(chunk) + last_man = man + final_groups.append({ + "container": grouped_targets[arch]['container'], + "targets": targets, + "arch": arch, + "group": group_name, + "len": len(chunk), + }) + chunk_counter += 1 + else: + temp_group.append(tar) + + if(last_arch != arch and len(temp_group) > 0): + group_name = last_arch + "-" + str(group_number[last_arch]) + group_number[last_arch] += 1 + targets = comma_targets(temp_group) + final_groups.append({ + "container": grouped_targets[last_arch]['container'], + "targets": targets, + "arch": last_arch, + "group": group_name, + "len": len(temp_group) + }) + last_arch = arch + temp_group = [] + if(len(temp_group) > (LOWER_LIMIT - 1)): + group_name = arch + "-" + str(group_number[arch]) + last_arch = arch + group_number[arch] += 1 + targets = comma_targets(temp_group) + final_groups.append({ + "container": grouped_targets[arch]['container'], + "targets": targets, + "arch": arch, + "group": group_name, + "len": len(temp_group) + }) + temp_group = [] + if(verbose): + import pprint + print("================") + print("= final_groups =") + print("================") + pprint.pp(final_groups) + + print("===============") + print("= JSON output =") + print("===============") + + print(json.dumps({ "include": final_groups }, **extra_args)) +else: + print(json.dumps(github_action_config, **extra_args)) From 072892fbefc1f4b3e4a3ef71f01f4b33316d5d1f Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Mon, 22 Apr 2024 10:53:09 -0700 Subject: [PATCH 79/90] romfs: rcS: support storage on other then SD card --- ROMFS/px4fmu_common/init.d/rcS | 36 ++++++++++++++++++++-------------- 1 file changed, 21 insertions(+), 15 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 39eed1bcab4d..28804361d1ed 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -30,7 +30,7 @@ set LOGGER_BUF 8 set PARAM_FILE "" set PARAM_BACKUP_FILE "" set RC_INPUT_ARGS "" -set SDCARD_AVAILABLE no +set STORAGE_AVAILABLE no set SDCARD_EXT_PATH /fs/microsd/ext_autostart set SDCARD_FORMAT no set STARTUP_TUNE 1 @@ -62,11 +62,11 @@ then umount /fs/microsd else - set SDCARD_AVAILABLE yes + set STORAGE_AVAILABLE yes fi fi - if [ $SDCARD_AVAILABLE = no -o $SDCARD_FORMAT = yes ] + if [ $STORAGE_AVAILABLE = no -o $SDCARD_FORMAT = yes ] then echo "INFO [init] formatting /dev/mmcsd0" set STARTUP_TUNE 15 # tune 15 = SD_ERROR (overridden to SD_INIT if format + mount succeeds) @@ -77,7 +77,7 @@ then if mount -t vfat /dev/mmcsd0 /fs/microsd then - set SDCARD_AVAILABLE yes + set STORAGE_AVAILABLE yes set STARTUP_TUNE 14 # tune 14 = SD_INIT else echo "ERROR [init] card mount failed" @@ -86,16 +86,22 @@ then echo "ERROR [init] format failed" fi fi +else + # Is there a device mounted for storage + if mft query -q -k MTD -s MTD_PARAMETERS -v /mnt/microsd + then + set STORAGE_AVAILABLE yes + fi +fi - if [ $SDCARD_AVAILABLE = yes ] +if [ $STORAGE_AVAILABLE = yes ] +then + if hardfault_log check then - if hardfault_log check + set STARTUP_TUNE 2 # tune 2 = ERROR_TUNE + if hardfault_log commit then - set STARTUP_TUNE 2 # tune 2 = ERROR_TUNE - if hardfault_log commit - then - hardfault_log reset - fi + hardfault_log reset fi fi @@ -172,7 +178,7 @@ else fi fi - if [ $SDCARD_AVAILABLE = yes ] + if [ $STORAGE_AVAILABLE = yes ] then param select-backup $PARAM_BACKUP_FILE fi @@ -220,8 +226,8 @@ else if [ ${VEHICLE_TYPE} == none ] then - # Look for airframe on SD card - if [ $SDCARD_AVAILABLE = yes ] + # Use external startup file + if [ $STORAGE_AVAILABLE = yes ] then . ${R}etc/init.d/rc.autostart_ext else @@ -615,7 +621,7 @@ unset PARAM_FILE unset PARAM_BACKUP_FILE unset PARAM_DEFAULTS_VER unset RC_INPUT_ARGS -unset SDCARD_AVAILABLE +unset STORAGE_AVAILABLE unset SDCARD_EXT_PATH unset SDCARD_FORMAT unset STARTUP_TUNE From 07734c243ff74b6e5191471858fb32300879262e Mon Sep 17 00:00:00 2001 From: Niklas Hauser Date: Mon, 5 Aug 2024 15:15:03 +0200 Subject: [PATCH 80/90] mtd: Initialized the RAMTRON speed with 30MHz --- platforms/nuttx/src/px4/common/px4_mtd.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/platforms/nuttx/src/px4/common/px4_mtd.cpp b/platforms/nuttx/src/px4/common/px4_mtd.cpp index 24d88ed3d89a..6723c35ecadb 100644 --- a/platforms/nuttx/src/px4/common/px4_mtd.cpp +++ b/platforms/nuttx/src/px4/common/px4_mtd.cpp @@ -75,11 +75,11 @@ static int ramtron_attach(mtd_instance_s &instance) return ENXIO; #else - /* start the RAMTRON driver, attempt 10 times */ + /* start the RAMTRON driver at 30MHz */ - int spi_speed_mhz = 10; + unsigned long spi_speed_hz = 30'000'000; - for (int i = 0; i < 10; i++) { + for (int i = 0; spi_speed_hz > 0; i++) { /* initialize the right spi */ struct spi_dev_s *spi = px4_spibus_initialize(px4_find_spi_bus(instance.devid)); @@ -90,7 +90,7 @@ static int ramtron_attach(mtd_instance_s &instance) /* this resets the spi bus, set correct bus speed again */ SPI_LOCK(spi, true); - SPI_SETFREQUENCY(spi, spi_speed_mhz * 1000 * 1000); + SPI_SETFREQUENCY(spi, spi_speed_hz); SPI_SETBITS(spi, 8); SPI_SETMODE(spi, SPIDEV_MODE3); SPI_SELECT(spi, instance.devid, false); @@ -108,7 +108,7 @@ static int ramtron_attach(mtd_instance_s &instance) } // try reducing speed for next attempt - spi_speed_mhz--; + spi_speed_hz -= 1'000'000; px4_usleep(10000); } @@ -118,7 +118,7 @@ static int ramtron_attach(mtd_instance_s &instance) return -EIO; } - int ret = instance.mtd_dev->ioctl(instance.mtd_dev, MTDIOC_SETSPEED, (unsigned long)spi_speed_mhz * 1000 * 1000); + int ret = instance.mtd_dev->ioctl(instance.mtd_dev, MTDIOC_SETSPEED, spi_speed_hz); if (ret != OK) { // FIXME: From the previous warning call, it looked like this should have been fatal error instead. Tried From c60b1d1a5f2b675bc6724642f038997a8d3921e0 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Mon, 15 Jul 2024 15:04:26 +0200 Subject: [PATCH 81/90] board_hw_rev_ver: Support EEPROM-only HW IDs --- .../stm32_common/board_hw_info/board_hw_rev_ver.c | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c b/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c index 6d484c0a826f..1a505e98217f 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c @@ -73,7 +73,7 @@ static char hw_base_info[HW_INFO_SIZE] = {0}; /**************************************************************************** * Protected Functions ****************************************************************************/ - +#if !defined(BOARD_HAS_ONLY_EEPROM_VERSIONING) static int dn_to_ordinal(uint16_t dn) { /* Table is scaled for 12, so if ADC is in 16 bit mode @@ -111,6 +111,7 @@ static int dn_to_ordinal(uint16_t dn) return -1; } +#endif /* BOARD_HAS_ONLY_EEPROM_VERSIONING */ /************************************************************************************ * Name: read_id_dn @@ -143,7 +144,7 @@ static int dn_to_ordinal(uint16_t dn) * -EIO - FAiled to init or read the ADC * ************************************************************************************/ - +#if !defined(BOARD_HAS_ONLY_EEPROM_VERSIONING) static int read_id_dn(int *id, uint32_t gpio_drive, uint32_t gpio_sense, int adc_channel) { int rv = -EIO; @@ -328,9 +329,15 @@ static int read_id_dn(int *id, uint32_t gpio_drive, uint32_t gpio_sense, int adc stm32_configgpio(gpio_drive); return rv; } +#endif /* BOARD_HAS_ONLY_EEPROM_VERSIONING */ static int determine_hw_info(int *revision, int *version) { +#if defined(BOARD_HAS_ONLY_EEPROM_VERSIONING) + *revision = HW_ID_EEPROM; + *version = HW_ID_EEPROM; + return OK; +#else int dn; int rv = read_id_dn(&dn, GPIO_HW_REV_DRIVE, GPIO_HW_REV_SENSE, ADC_HW_REV_SENSE_CHANNEL); @@ -344,6 +351,7 @@ static int determine_hw_info(int *revision, int *version) } return rv; +#endif } /**************************************************************************** From ecfdbd2e60660a373c158544d4580b21d4f4059b Mon Sep 17 00:00:00 2001 From: Niklas Hauser Date: Wed, 31 Jul 2024 14:36:49 +0200 Subject: [PATCH 82/90] littlefs: needs more stack when used --- .../px4_work_queue/WorkQueueManager.hpp | 2 +- src/modules/logger/Kconfig | 8 ++++++++ src/modules/logger/logger.cpp | 2 +- src/systemcmds/hardfault_log/CMakeLists.txt | 1 + src/systemcmds/param/CMakeLists.txt | 1 + 5 files changed, 12 insertions(+), 2 deletions(-) diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp index 761041fa0e83..68ac3f1ffea9 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp @@ -89,7 +89,7 @@ static constexpr wq_config_t ttyS9{"wq:ttyS9", 1728, -30}; static constexpr wq_config_t ttyACM0{"wq:ttyACM0", 1728, -31}; static constexpr wq_config_t ttyUnknown{"wq:ttyUnknown", 1728, -32}; -static constexpr wq_config_t lp_default{"wq:lp_default", 2000, -50}; +static constexpr wq_config_t lp_default{"wq:lp_default", 2350, -50}; static constexpr wq_config_t test1{"wq:test1", 2000, 0}; static constexpr wq_config_t test2{"wq:test2", 2000, 0}; diff --git a/src/modules/logger/Kconfig b/src/modules/logger/Kconfig index db2c5a75460e..985fd3d56ed1 100644 --- a/src/modules/logger/Kconfig +++ b/src/modules/logger/Kconfig @@ -10,3 +10,11 @@ menuconfig USER_LOGGER depends on BOARD_PROTECTED && MODULES_LOGGER ---help--- Put logger in userspace memory + +menuconfig LOGGER_STACK_SIZE + int "stack size of logger task" + default 3700 + depends on MODULES_LOGGER + ---help--- + Stack size of the logger task. Some configurations require more stack + than the default. diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index be66d5465c95..8d897a103a4d 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -176,7 +176,7 @@ int Logger::task_spawn(int argc, char *argv[]) _task_id = px4_task_spawn_cmd("logger", SCHED_DEFAULT, SCHED_PRIORITY_LOG_CAPTURE, - PX4_STACK_ADJUSTED(3700), + PX4_STACK_ADJUSTED(CONFIG_LOGGER_STACK_SIZE), (px4_main_t)&run_trampoline, (char *const *)argv); diff --git a/src/systemcmds/hardfault_log/CMakeLists.txt b/src/systemcmds/hardfault_log/CMakeLists.txt index fca7fc683929..8cd4ca9fea86 100644 --- a/src/systemcmds/hardfault_log/CMakeLists.txt +++ b/src/systemcmds/hardfault_log/CMakeLists.txt @@ -35,6 +35,7 @@ px4_add_module( MAIN hardfault_log COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable + STACK_MAIN 4096 SRCS hardfault_log.c DEPENDS diff --git a/src/systemcmds/param/CMakeLists.txt b/src/systemcmds/param/CMakeLists.txt index 0a27cf958949..056401daf75e 100644 --- a/src/systemcmds/param/CMakeLists.txt +++ b/src/systemcmds/param/CMakeLists.txt @@ -35,6 +35,7 @@ px4_add_module( MAIN param COMPILE_FLAGS -Wno-array-bounds + STACK_MAIN 4096 SRCS param.cpp DEPENDS From f2f4488594f21b83ed9d717a250fd61b93a356ca Mon Sep 17 00:00:00 2001 From: Thomas Stauber <48206725+ThomasRigi@users.noreply.github.com> Date: Mon, 19 Aug 2024 17:14:12 +0200 Subject: [PATCH 83/90] drivers/gps: publish secondary instance satellite_info if main instance is advertised --- src/drivers/gps/gps.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index c1dd783be029..004be4cc8b90 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -1214,11 +1214,13 @@ GPS::publish() void GPS::publishSatelliteInfo() { - if (_instance == Instance::Main) { + if (_instance == Instance::Main || _is_gps_main_advertised.load()) { if (_p_report_sat_info != nullptr) { _report_sat_info_pub.publish(*_p_report_sat_info); } + _is_gps_main_advertised.store(true); + } else { //we don't publish satellite info for the secondary gps } From 98eae3cd4cc1fef11f12d50563d5bbfc5141d833 Mon Sep 17 00:00:00 2001 From: Beniamino Pozzan Date: Sun, 18 Aug 2024 10:09:29 +0100 Subject: [PATCH 84/90] fix: `make help` on Ubuntu 22.04 Ubuntu 22.04 uses make 4.3 which broke the current `make help` target Reference: https://stackoverflow.com/a/26339924 Signed-off-by: Beniamino Pozzan --- Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile b/Makefile index 37c61db1c0b3..5de475d79074 100644 --- a/Makefile +++ b/Makefile @@ -557,7 +557,7 @@ help: @echo "Usage: $(MAKE) " @echo "Where is one of:" @$(MAKE) -pRrq -f $(lastword $(MAKEFILE_LIST)) : 2>/dev/null | \ - awk -v RS= -F: '/^# File/,/^# Finished Make data base/ {if ($$1 !~ "^[#.]") {print $$1}}' | sort | \ + awk -v RS= -F: '/(^|\n)# Files(\n|$$)/,/(^|\n)# Finished Make data base/ {if ($$1 !~ "^[#.]") {print $$1}}' | sort | \ egrep -v -e '^[^[:alnum:]]' -e '^($(subst $(space),|,$(ALL_CONFIG_TARGETS)))$$' -e '_default$$' -e '^(Makefile)' @echo @echo "Or, $(MAKE) []" From 093117957948556b1e10808b942ea456a4b0526b Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 20 Aug 2024 13:27:12 +0200 Subject: [PATCH 85/90] ekf2: extract WMM update logic --- .../ekf2/EKF/aid_sources/gnss/gps_checks.cpp | 32 +------------ src/modules/ekf2/EKF/ekf.h | 1 + src/modules/ekf2/EKF/ekf_helper.cpp | 48 +++++++++++++------ 3 files changed, 36 insertions(+), 45 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp index da7fc4e16060..94fa6f04f381 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp @@ -98,37 +98,7 @@ void Ekf::collect_gps(const gnssSample &gps) const bool gps_rough_2d_fix = (gps.fix_type >= 2) && (gps.hacc < 1000); if (gps_rough_2d_fix && (_gps_checks_passed || !_NED_origin_initialised)) { - - // If we have good GPS data set the origin's WGS-84 position to the last gps fix -#if defined(CONFIG_EKF2_MAGNETOMETER) - - // set the magnetic field data returned by the geo library using the current GPS position - const float mag_declination_gps = math::radians(get_mag_declination_degrees(gps.lat, gps.lon)); - const float mag_inclination_gps = math::radians(get_mag_inclination_degrees(gps.lat, gps.lon)); - const float mag_strength_gps = get_mag_strength_gauss(gps.lat, gps.lon); - - if (PX4_ISFINITE(mag_declination_gps) && PX4_ISFINITE(mag_inclination_gps) && PX4_ISFINITE(mag_strength_gps)) { - - const bool mag_declination_changed = (fabsf(mag_declination_gps - _mag_declination_gps) > math::radians(1.f)); - const bool mag_inclination_changed = (fabsf(mag_inclination_gps - _mag_inclination_gps) > math::radians(1.f)); - - if ((_wmm_gps_time_last_set == 0) - || !PX4_ISFINITE(_mag_declination_gps) - || !PX4_ISFINITE(_mag_inclination_gps) - || !PX4_ISFINITE(_mag_strength_gps) - || mag_declination_changed - || mag_inclination_changed - ) { - _mag_declination_gps = mag_declination_gps; - _mag_inclination_gps = mag_inclination_gps; - _mag_strength_gps = mag_strength_gps; - - _wmm_gps_time_last_set = _time_delayed_us; - } - } - -#endif // CONFIG_EKF2_MAGNETOMETER - + updateWmm(gps.lat, gps.lon); _earth_rate_NED = calcEarthRateNED((float)math::radians(gps.lat)); } diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index f3f055434e62..a7c43567df52 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -259,6 +259,7 @@ class Ekf final : public EstimatorInterface // return true if the origin is valid bool getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const; bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float eph = 0.f, float epv = 0.f); + void updateWmm(double lat, double lon); // get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const; diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index d88f7c400d92..ae3e9e355e0a 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -96,20 +96,7 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons _pos_ref.initReference(latitude, longitude, _time_delayed_us); _gps_alt_ref = altitude; -#if defined(CONFIG_EKF2_MAGNETOMETER) - const float mag_declination_gps = math::radians(get_mag_declination_degrees(latitude, longitude)); - const float mag_inclination_gps = math::radians(get_mag_inclination_degrees(latitude, longitude)); - const float mag_strength_gps = get_mag_strength_gauss(latitude, longitude); - - if (PX4_ISFINITE(mag_declination_gps) && PX4_ISFINITE(mag_inclination_gps) && PX4_ISFINITE(mag_strength_gps)) { - _mag_declination_gps = mag_declination_gps; - _mag_inclination_gps = mag_inclination_gps; - _mag_strength_gps = mag_strength_gps; - - _wmm_gps_time_last_set = _time_delayed_us; - } - -#endif // CONFIG_EKF2_MAGNETOMETER + updateWmm(current_lat, current_lon); _gpos_origin_eph = eph; _gpos_origin_epv = epv; @@ -144,6 +131,39 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons return false; } +void Ekf::updateWmm(const double lat, const double lon) +{ +#if defined(CONFIG_EKF2_MAGNETOMETER) + + // set the magnetic field data returned by the geo library using the current GPS position + const float mag_declination_gps = math::radians(get_mag_declination_degrees(lat, lon)); + const float mag_inclination_gps = math::radians(get_mag_inclination_degrees(lat, lon)); + const float mag_strength_gps = get_mag_strength_gauss(lat, lon); + + if (PX4_ISFINITE(mag_declination_gps) && PX4_ISFINITE(mag_inclination_gps) && PX4_ISFINITE(mag_strength_gps)) { + + const bool mag_declination_changed = (fabsf(mag_declination_gps - _mag_declination_gps) > math::radians(1.f)); + const bool mag_inclination_changed = (fabsf(mag_inclination_gps - _mag_inclination_gps) > math::radians(1.f)); + + if ((_wmm_gps_time_last_set == 0) + || !PX4_ISFINITE(_mag_declination_gps) + || !PX4_ISFINITE(_mag_inclination_gps) + || !PX4_ISFINITE(_mag_strength_gps) + || mag_declination_changed + || mag_inclination_changed + ) { + _mag_declination_gps = mag_declination_gps; + _mag_inclination_gps = mag_inclination_gps; + _mag_strength_gps = mag_strength_gps; + + _wmm_gps_time_last_set = _time_delayed_us; + } + } + +#endif // CONFIG_EKF2_MAGNETOMETER +} + + void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const { float eph = INFINITY; From f252e20eaec78972ff0723af26c922c3015aa6a6 Mon Sep 17 00:00:00 2001 From: Jaeyoung Lim Date: Wed, 21 Aug 2024 01:36:08 +0200 Subject: [PATCH 86/90] Revert "Update GZBridge to be able to use gazebo airspeed. Add quadtailsitter. (#23455)" (#23583) This reverts commit 7e45f4915208b2d02e05e5741ff7456cf92120f9. Co-authored-by: jmackay2 <1.732mackay@gmail.com> --- .../1045_gazebo-classic_quadtailsitter | 2 +- .../airframes/4004_gz_standard_vtol | 2 + .../airframes/4014_gz_quadtailsitter | 92 ------------------- .../init.d-posix/airframes/CMakeLists.txt | 1 - src/modules/simulation/gz_bridge/GZBridge.cpp | 16 ++-- src/modules/simulation/gz_bridge/GZBridge.hpp | 4 +- .../sensor_airspeed_sim/SensorAirspeedSim.cpp | 2 +- 7 files changed, 16 insertions(+), 103 deletions(-) delete mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/4014_gz_quadtailsitter diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1045_gazebo-classic_quadtailsitter b/ROMFS/px4fmu_common/init.d-posix/airframes/1045_gazebo-classic_quadtailsitter index 9539ba6d7402..87afb523b1a7 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1045_gazebo-classic_quadtailsitter +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1045_gazebo-classic_quadtailsitter @@ -33,7 +33,7 @@ param set-default PWM_MAIN_FUNC3 103 param set-default PWM_MAIN_FUNC4 104 param set-default PWM_MAIN_FUNC5 0 -param set-default FD_FAIL_R 70 +parm set-default FD_FAIL_R 70 param set-default FW_P_TC 0.6 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 580b6da368de..96bb25d69a52 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 @@ -14,7 +14,9 @@ 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 # TODO: Enable motor failure detection when the # VTOL no longer reports 0A for all ESCs in SITL diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4014_gz_quadtailsitter b/ROMFS/px4fmu_common/init.d-posix/airframes/4014_gz_quadtailsitter deleted file mode 100644 index 699d1dacc521..000000000000 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4014_gz_quadtailsitter +++ /dev/null @@ -1,92 +0,0 @@ -#!/bin/sh -# -# @name Quadrotor + Tailsitter -# -# @type VTOL Quad Tailsitter -# - -. ${R}etc/init.d/rc.vtol_defaults - -PX4_SIMULATOR=${PX4_SIMULATOR:=gz} -PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} -PX4_SIM_MODEL=${PX4_SIM_MODEL:=quadtailsitter} - -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 0 - -param set-default MAV_TYPE 20 - -param set-default CA_AIRFRAME 4 - -param set-default CA_ROTOR_COUNT 4 -param set-default CA_ROTOR0_PX 0.15 -param set-default CA_ROTOR0_PY 0.23 -param set-default CA_ROTOR0_KM 0.05 -param set-default CA_ROTOR1_PX -0.15 -param set-default CA_ROTOR1_PY -0.23 -param set-default CA_ROTOR1_KM 0.05 -param set-default CA_ROTOR2_PX 0.15 -param set-default CA_ROTOR2_PY -0.23 -param set-default CA_ROTOR2_KM -0.05 -param set-default CA_ROTOR3_PX -0.15 -param set-default CA_ROTOR3_PY 0.23 -param set-default CA_ROTOR3_KM -0.05 - -param set-default CA_SV_CS_COUNT 0 - -param set-default SIM_GZ_EC_FUNC1 101 -param set-default SIM_GZ_EC_MIN1 10 -param set-default SIM_GZ_EC_MAX1 1500 -param set-default SIM_GZ_EC_FUNC2 102 -param set-default SIM_GZ_EC_MIN2 10 -param set-default SIM_GZ_EC_MAX2 1500 -param set-default SIM_GZ_EC_FUNC3 103 -param set-default SIM_GZ_EC_MIN3 10 -param set-default SIM_GZ_EC_MAX3 1500 -param set-default SIM_GZ_EC_FUNC4 104 -param set-default SIM_GZ_EC_MIN4 10 -param set-default SIM_GZ_EC_MAX4 1500 - -param set-default FD_FAIL_R 70 - -param set-default FW_P_TC 0.6 - -param set-default FW_PR_I 0.3 -param set-default FW_PR_P 0.5 -param set-default FW_PSP_OFF 2 -param set-default FW_RR_FF 0.1 -param set-default FW_RR_I 0.1 -param set-default FW_RR_P 0.2 -param set-default FW_YR_FF 0 # make yaw rate controller very weak, only keep default P -param set-default FW_YR_I 0 -param set-default FW_THR_TRIM 0.35 -param set-default FW_THR_MAX 0.8 -param set-default FW_THR_MIN 0.05 -param set-default FW_T_CLMB_MAX 6 -param set-default FW_T_HRATE_FF 0.5 -param set-default FW_T_SINK_MAX 3 -param set-default FW_T_SINK_MIN 1.6 -param set-default FW_AIRSPD_STALL 10 -param set-default FW_AIRSPD_MIN 14 -param set-default FW_AIRSPD_TRIM 18 -param set-default FW_AIRSPD_MAX 22 - -param set-default MC_AIRMODE 2 -param set-default MAN_ARM_GESTURE 0 # required for yaw airmode -param set-default MC_ROLL_P 3 -param set-default MC_PITCH_P 3 -param set-default MC_ROLLRATE_P 0.3 -param set-default MC_PITCHRATE_P 0.3 - -param set-default VT_ARSP_TRANS 15 -param set-default VT_B_TRANS_DUR 5 -param set-default VT_FW_DIFTHR_EN 7 -param set-default VT_FW_DIFTHR_S_Y 1 -param set-default VT_F_TRANS_DUR 1.5 -param set-default VT_TYPE 0 - -param set-default WV_EN 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index badd712b063f..9235b2e66340 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -85,7 +85,6 @@ px4_add_romfs_files( 4011_gz_lawnmower 4012_gz_rover_ackermann 4013_gz_x500_lidar - 4014_gz_quadtailsitter 6011_gazebo-classic_typhoon_h480 6011_gazebo-classic_typhoon_h480.post diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 6b4042da8bb3..3464c628694c 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -203,15 +203,17 @@ int GZBridge::init() PX4_WARN("failed to subscribe to %s", laser_scan_topic.c_str()); } - // Airspeed: /world/$WORLD/model/$MODEL/link/base_link/sensor/airspeed_sensor/air_speed - std::string airspeed_topic = "/world/" + _world_name + "/model/" + _model_name + - "/link/base_link/sensor/airspeed_sensor/air_speed"; +#if 0 + // Airspeed: /world/$WORLD/model/$MODEL/link/airspeed_link/sensor/air_speed/air_speed + std::string airpressure_topic = "/world/" + _world_name + "/model/" + _model_name + + "/link/airspeed_link/sensor/air_speed/air_speed"; - if (!_node.Subscribe(airspeed_topic, &GZBridge::airspeedCallback, this)) { - PX4_ERR("failed to subscribe to %s", airspeed_topic.c_str()); + if (!_node.Subscribe(airpressure_topic, &GZBridge::airspeedCallback, this)) { + PX4_ERR("failed to subscribe to %s", airpressure_topic.c_str()); return PX4_ERROR; } +#endif // Air pressure: /world/$WORLD/model/$MODEL/link/base_link/sensor/air_pressure_sensor/air_pressure std::string air_pressure_topic = "/world/" + _world_name + "/model/" + _model_name + "/link/base_link/sensor/air_pressure_sensor/air_pressure"; @@ -424,7 +426,8 @@ void GZBridge::barometerCallback(const gz::msgs::FluidPressure &air_pressure) pthread_mutex_unlock(&_node_mutex); } -void GZBridge::airspeedCallback(const gz::msgs::AirSpeed &air_speed) +#if 0 +void GZBridge::airspeedCallback(const gz::msgs::AirSpeedSensor &air_speed) { if (hrt_absolute_time() == 0) { return; @@ -449,6 +452,7 @@ void GZBridge::airspeedCallback(const gz::msgs::AirSpeed &air_speed) pthread_mutex_unlock(&_node_mutex); } +#endif void GZBridge::imuCallback(const gz::msgs::IMU &imu) { diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 824c7d471633..8a832f7961b2 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -102,7 +102,7 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S void clockCallback(const gz::msgs::Clock &clock); - void airspeedCallback(const gz::msgs::AirSpeed &air_speed); + // void airspeedCallback(const gz::msgs::AirSpeedSensor &air_pressure); void barometerCallback(const gz::msgs::FluidPressure &air_pressure); void imuCallback(const gz::msgs::IMU &imu); void poseInfoCallback(const gz::msgs::Pose_V &pose); @@ -123,7 +123,7 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S // Subscriptions uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; + //uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance)}; uORB::Publication _angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)}; uORB::Publication _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)}; diff --git a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp index 86fbbf29cd4c..7b0d9556cadf 100644 --- a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp +++ b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp @@ -195,7 +195,7 @@ int SensorAirspeedSim::print_usage(const char *reason) )DESCR_STR"); - PRINT_MODULE_USAGE_NAME("sensor_airspeed_sim", "system"); + PRINT_MODULE_USAGE_NAME("sensor_arispeed_sim", "system"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); From 3478765c310d3d34fc139dbf62594c41da6f85a9 Mon Sep 17 00:00:00 2001 From: KonradRudin <98741601+KonradRudin@users.noreply.github.com> Date: Wed, 21 Aug 2024 09:08:36 +0200 Subject: [PATCH 87/90] Navigator: MissionFeasibilityCheck: Rework 1st waypoint check (#23568) * FeasibilityChecker: only warn when first waypoint is too far, but still accept mission as valid * feasiblityChecker: make distance to first waypoint check against home position instead of current position, so it is more constant during a flight * Apply suggestions from code review Co-authored-by: Silvan Fuhrer * feasibilityCheckerTest: update tests to not fail for first waypoint check * feasibilityChecker: make comment for 1stwaypointcheck event * Feasibility check unit test: fix comment Signed-off-by: Silvan Fuhrer --------- Signed-off-by: Silvan Fuhrer Co-authored-by: Silvan Fuhrer --- .../MissionFeasibility/FeasibilityChecker.cpp | 40 ++++++++----------- .../MissionFeasibility/FeasibilityChecker.hpp | 4 -- .../FeasibilityCheckerTest.cpp | 4 +- .../navigator/mission_feasibility_checker.cpp | 1 - 4 files changed, 19 insertions(+), 30 deletions(-) diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp index c684f0119d9b..c2b3a8119807 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp @@ -49,7 +49,6 @@ void FeasibilityChecker::reset() _mission_validity_failed = false; _takeoff_failed = false; _land_pattern_validity_failed = false; - _distance_first_waypoint_failed = false; _distance_between_waypoints_failed = false; _fixed_wing_land_approach_failed = false; _takeoff_land_available_failed = false; @@ -118,12 +117,6 @@ void FeasibilityChecker::updateData() _is_landed = land_detected.landed; } - if (_vehicle_global_position_sub.updated()) { - vehicle_global_position_s vehicle_global_position = {}; - _vehicle_global_position_sub.copy(&vehicle_global_position); - _current_position_lat_lon = matrix::Vector2d(vehicle_global_position.lat, vehicle_global_position.lon); - } - if (_rtl_status_sub.updated()) { rtl_status_s rtl_status = {}; _rtl_status_sub.copy(&rtl_status); @@ -199,8 +192,8 @@ void FeasibilityChecker::doCommonChecks(mission_item_s &mission_item, const int _distance_between_waypoints_failed = !checkDistancesBetweenWaypoints(mission_item); } - if (!_distance_first_waypoint_failed) { - _distance_first_waypoint_failed = !checkHorizontalDistanceToFirstWaypoint(mission_item); + if (!_first_waypoint_found) { + checkHorizontalDistanceToFirstWaypoint(mission_item); } if (!_takeoff_failed) { @@ -631,31 +624,32 @@ bool FeasibilityChecker::hasMissionBothOrNeitherTakeoffAndLanding() bool FeasibilityChecker::checkHorizontalDistanceToFirstWaypoint(mission_item_s &mission_item) { if (_param_mis_dist_1wp > FLT_EPSILON && - (_current_position_lat_lon.isAllFinite()) && !_first_waypoint_found && + (_home_lat_lon.isAllFinite()) && MissionBlock::item_contains_position(mission_item)) { _first_waypoint_found = true; - float dist_to_1wp_from_current_pos = 1e6f; - - if (_current_position_lat_lon.isAllFinite()) { - dist_to_1wp_from_current_pos = get_distance_to_next_waypoint( - mission_item.lat, mission_item.lon, - _current_position_lat_lon(0), _current_position_lat_lon(1)); - } + const float dist_to_1wp_from_home_pos = get_distance_to_next_waypoint( + mission_item.lat, mission_item.lon, + _home_lat_lon(0), _home_lat_lon(1)); - if (dist_to_1wp_from_current_pos < _param_mis_dist_1wp) { + if (dist_to_1wp_from_home_pos < _param_mis_dist_1wp) { return true; } else { /* item is too far from current position */ mavlink_log_critical(_mavlink_log_pub, - "First waypoint too far away: %dm, %d max\t", - (int)dist_to_1wp_from_current_pos, (int)_param_mis_dist_1wp); - events::send(events::ID("navigator_mis_first_wp_too_far"), {events::Log::Error, events::LogInternal::Info}, - "First waypoint too far away: {1m} (maximum: {2m})", (uint32_t)dist_to_1wp_from_current_pos, - (uint32_t)_param_mis_dist_1wp); + "First waypoint far away from home: %dm. Correct mission loaded?\t", + (int)dist_to_1wp_from_home_pos); + /* EVENT + * @description + * + * This check can be configured via MIS_DIST_1WP parameter. + * + */ + events::send(events::ID("navigator_mis_first_wp_far"), {events::Log::Warning, events::LogInternal::Info}, + "First waypoint far away from Home: {1m} Correct mission loaded?", (uint32_t)dist_to_1wp_from_home_pos); return false; } diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp index fd18cbcfb68e..d0905d363545 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp @@ -78,7 +78,6 @@ class FeasibilityChecker : public ModuleParams bool someCheckFailed() { return _takeoff_failed || - _distance_first_waypoint_failed || _distance_between_waypoints_failed || _land_pattern_validity_failed || _fixed_wing_land_approach_failed || @@ -97,7 +96,6 @@ class FeasibilityChecker : public ModuleParams uORB::Subscription _home_pos_sub{ORB_ID(home_position)}; uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _land_detector_sub{ORB_ID(vehicle_land_detected)}; - uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; uORB::Subscription _rtl_status_sub{ORB_ID(rtl_status)}; // parameters @@ -110,14 +108,12 @@ class FeasibilityChecker : public ModuleParams float _home_alt_msl{NAN}; bool _has_vtol_approach{false}; matrix::Vector2d _home_lat_lon = matrix::Vector2d((double)NAN, (double)NAN); - matrix::Vector2d _current_position_lat_lon = matrix::Vector2d((double)NAN, (double)NAN); VehicleType _vehicle_type{VehicleType::RotaryWing}; // internal flags to keep track of which checks failed bool _mission_validity_failed{false}; bool _takeoff_failed{false}; bool _land_pattern_validity_failed{false}; - bool _distance_first_waypoint_failed{false}; bool _distance_between_waypoints_failed{false}; bool _fixed_wing_land_approach_failed{false}; bool _takeoff_land_available_failed{false}; diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp index 0de54f33766c..43662441488a 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp @@ -170,9 +170,9 @@ TEST_F(FeasibilityCheckerTest, check_dist_first_waypoint) mission_item.lat = lat_new; mission_item.lon = lon_new; - // THEN: fail + // THEN: pass checker.processNextItem(mission_item, 0, 1); - ASSERT_EQ(checker.someCheckFailed(), true); + ASSERT_EQ(checker.someCheckFailed(), false); // BUT WHEN: valid current position fist WP 499m away from current checker.reset(); diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index dc2420d55bf5..8049df1236cf 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -41,7 +41,6 @@ */ #include "mission_feasibility_checker.h" -#include "MissionFeasibility/FeasibilityChecker.hpp" #include "mission_block.h" #include "navigator.h" From b2f663648e9858c0f63e50a87faa347431a12e8c Mon Sep 17 00:00:00 2001 From: Ramon Roche Date: Wed, 21 Aug 2024 07:56:37 -0700 Subject: [PATCH 88/90] ci: github actions runs-on Dronecode AWS Infra * ci: try runs-on Dronecode Infra * ci: comment on how to disable RunsOn * Update .github/workflows/build_all_targets.yml --- .github/workflows/build_all_targets.yml | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/.github/workflows/build_all_targets.yml b/.github/workflows/build_all_targets.yml index 119236b3198e..0aa6ce92e592 100644 --- a/.github/workflows/build_all_targets.yml +++ b/.github/workflows/build_all_targets.yml @@ -1,3 +1,8 @@ +# NOTE: this workflow is now running on Dronecode / PX4 AWS account. +# - If you want to keep the tests running in GitHub Actions you need to uncomment the "runs-on: ubuntu-latest" lines +# and comment the "runs-on: [runs-on,runner=..." lines. +# - If you would like to duplicate this setup try setting up "RunsOn" on your own AWS account try https://runs-on.com + name: Build all targets on: @@ -14,7 +19,8 @@ on: jobs: group_targets: name: Scan for Board Targets - runs-on: ubuntu-latest + # runs-on: ubuntu-latest + runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}"] outputs: matrix: ${{ steps.set-matrix.outputs.matrix }} timestamp: ${{ steps.set-timestamp.outputs.timestamp }} @@ -34,7 +40,8 @@ jobs: setup: name: ${{ matrix.group }} - runs-on: ubuntu-latest + # runs-on: ubuntu-latest + runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}"] needs: group_targets strategy: matrix: ${{ fromJson(needs.group_targets.outputs.matrix) }} From ae165561076541efd2db00b28a05ad42c190b9ff Mon Sep 17 00:00:00 2001 From: Jaeyoung Lim Date: Wed, 21 Aug 2024 17:41:47 +0200 Subject: [PATCH 89/90] simulation/gz_bridge: follow model in gz GUI (#22808) --- src/modules/simulation/gz_bridge/GZBridge.cpp | 118 ++++++++++++++++-- src/modules/simulation/gz_bridge/GZBridge.hpp | 41 ++++++ 2 files changed, 151 insertions(+), 8 deletions(-) diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 3464c628694c..f7d80d32cac6 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -150,16 +150,31 @@ int GZBridge::init() // 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 (_node.Request(create_service, req, 1000, rep, result)) { - if (!rep.data() || !result) { - PX4_ERR("EntityFactory service call failed."); - return PX4_ERROR; - } - - } else { - PX4_ERR("Service call timed out. Check GZ_SIM_RESOURCE_PATH is set correctly."); + if (!callEntityFactoryService(create_service, req)) { return PX4_ERROR; } + + std::string scene_info_service = "/world/default/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."); + system_usleep(2000000); + + } else { + scene_created = true; + } + } + + gz::msgs::StringMsg follow_msg{}; + follow_msg.set_data(_model_name); + bool call_string_service = 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); } } @@ -829,6 +844,93 @@ void GZBridge::laserScanCallback(const gz::msgs::LaserScan &scan) _obstacle_distance_pub.publish(obs); } +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_ERR("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, 1000, rep, result)) { + if (!result) { + PX4_ERR("Scene Info service call failed."); + return false; + + } else { + return true; + } + + } else { + PX4_ERR("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, 1000, rep, result)) { + if (!rep.data() || !result) { + PX4_ERR("String service call failed"); + return false; + + } + } + + else { + PX4_ERR("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, 1000, rep, result)) { + if (!rep.data() || !result) { + PX4_ERR("String service call failed"); + return false; + + } + } + + else { + PX4_ERR("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 diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 8a832f7961b2..fc2cb3a6e9ad 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -69,6 +69,8 @@ #include #include #include +#include +#include using namespace time_literals; @@ -110,6 +112,45 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S void navSatCallback(const gz::msgs::NavSat &nav_sat); 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) From b33b0398ddb97b0ba3295cf57a8d90d6e6de69fa Mon Sep 17 00:00:00 2001 From: jmackay2 <1.732mackay@gmail.com> Date: Wed, 21 Aug 2024 20:30:10 -0400 Subject: [PATCH 90/90] Fix param typo in quadtailsitter airframe (#23588) --- .../init.d-posix/airframes/1045_gazebo-classic_quadtailsitter | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1045_gazebo-classic_quadtailsitter b/ROMFS/px4fmu_common/init.d-posix/airframes/1045_gazebo-classic_quadtailsitter index 87afb523b1a7..9539ba6d7402 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1045_gazebo-classic_quadtailsitter +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1045_gazebo-classic_quadtailsitter @@ -33,7 +33,7 @@ param set-default PWM_MAIN_FUNC3 103 param set-default PWM_MAIN_FUNC4 104 param set-default PWM_MAIN_FUNC5 0 -parm set-default FD_FAIL_R 70 +param set-default FD_FAIL_R 70 param set-default FW_P_TC 0.6