From 0704580a30bb319fddc8c036675996a3e7202704 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Thu, 11 Dec 2025 20:10:22 -0900 Subject: [PATCH 1/5] gps: fix RTCM injection to use inject-before-read pattern as before. Add RTCM parser to frame-align injection. Drain GpsInjectData uORB queue into RTCM parser buffer and then inject. Add GPS_UBX_PPK parameter to enable MSM7 output from the GPS module (rather than the default of MSM4) which is required for PPK workflows. --- src/drivers/gps/CMakeLists.txt | 1 + src/drivers/gps/devices | 2 +- src/drivers/gps/gps.cpp | 95 +++++++++++++++++++++++----------- src/drivers/gps/params.c | 9 ++++ 4 files changed, 75 insertions(+), 32 deletions(-) diff --git a/src/drivers/gps/CMakeLists.txt b/src/drivers/gps/CMakeLists.txt index 4719e0e34e..3c3c0cc3e0 100644 --- a/src/drivers/gps/CMakeLists.txt +++ b/src/drivers/gps/CMakeLists.txt @@ -55,4 +55,5 @@ px4_add_module( module.yaml DEPENDS git_gps_devices + gnss ) diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices index caf5158061..991f771a8e 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit caf5158061bd10e79c9f042abb62c86bc6f3e7a7 +Subproject commit 991f771a8e192ccde3feb84fad3fb4fa3a3ef7bb diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index e7dbab947a..2e1bdccdbd 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -67,6 +67,8 @@ #include #include +#include + #ifndef CONSTRAINED_FLASH # include "devices/src/ashtech.h" # include "devices/src/emlid_reach.h" @@ -215,6 +217,8 @@ private: gps_dump_s *_dump_from_device{nullptr}; gps_dump_comm_mode_t _dump_communication_mode{gps_dump_comm_mode_t::Disabled}; + gnss::Rtcm3Parser _rtcm_parser{}; + static px4::atomic_bool _is_gps_main_advertised; ///< for the second gps we want to make sure that it gets instance 1 /// and thus we wait until the first one publishes at least one message. @@ -264,7 +268,7 @@ private: * @param data * @param len */ - inline bool injectData(uint8_t *data, size_t len); + inline bool injectData(const uint8_t *data, size_t len); /** * set the Baudrate @@ -285,7 +289,7 @@ private: * @param mode calling source * @param msg_to_gps_device if true, this is a message sent to the gps device, otherwise it's from the device */ - void dumpGpsData(uint8_t *data, size_t len, gps_dump_comm_mode_t mode, bool msg_to_gps_device); + void dumpGpsData(const uint8_t *data, size_t len, gps_dump_comm_mode_t mode, bool msg_to_gps_device); void initializeCommunicationDump(); @@ -472,28 +476,16 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout) const int max_timeout = 50; int timeout_adjusted = math::min(max_timeout, timeout); + handleInjectDataTopic(); + if (_interface == GPSHelper::Interface::UART) { - - const ssize_t read_at_least = math::min(character_count, buf_length); - - // handle injection data before read if caught up - if (_uart.bytesAvailable() < read_at_least) { - handleInjectDataTopic(); - } - - ret = _uart.readAtLeast(buf, buf_length, read_at_least, timeout_adjusted); - - if (ret > 0) { - _num_bytes_read += ret; - } + ret = _uart.readAtLeast(buf, buf_length, math::min(character_count, buf_length), timeout_adjusted); // SPI is only supported on LInux #if defined(__PX4_LINUX) } else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)) { - handleInjectDataTopic(); - //Poll only for the SPI data. In the same thread we also need to handle orb messages, //so ideally we would poll on both, the SPI fd and orb subscription. Unfortunately the //two pollings use different underlying mechanisms (at least under posix), which makes this @@ -583,6 +575,7 @@ void GPS::handleInjectDataTopic() // Looking at 8 packets thus guarantees, that at least a full injection // data set is evaluated. // Moving Base reuires a higher rate, so we allow up to 8 packets. + // Drain uORB messages into RTCM parser and inject full messages after draining the queue. const size_t max_num_injections = gps_inject_data_s::ORB_QUEUE_LENGTH; size_t num_injections = 0; @@ -592,13 +585,13 @@ void GPS::handleInjectDataTopic() // Prevent injection of data from self if (msg.device_id != get_device_id()) { - /* Write the message to the gps device. Note that the message could be fragmented. - * But as we don't write anywhere else to the device during operation, we don't - * need to assemble the message first. - */ - injectData(msg.data, msg.len); + // Add data to the RTCM parser buffer for frame reassembly + size_t added = _rtcm_parser.addData(msg.data, msg.len); + + if (added < msg.len) { + PX4_WARN("RTCM buffer full, dropped %zu bytes", msg.len - added); + } - ++_rtcm_injection_rate_message_count; _last_rtcm_injection_time = hrt_absolute_time(); } } @@ -616,9 +609,30 @@ void GPS::handleInjectDataTopic() } } while (updated && num_injections < max_num_injections); + + // Now inject all complete RTCM frames from the parser buffer + size_t frame_len = {}; + const uint8_t *frame_ptr = {}; + + while ((frame_ptr = _rtcm_parser.getNextMessage(&frame_len)) != nullptr) { + // Check TX buffer space before writing + if (_interface == GPSHelper::Interface::UART) { + ssize_t tx_available = _uart.txSpaceAvailable(); + + if ((ssize_t)frame_len > tx_available) { + // TX buffer full, stop and let it drain - frames stay in parser buffer + PX4_WARN("TX buffer full!"); + break; + } + } + + injectData(frame_ptr, frame_len); + _rtcm_parser.consumeMessage(frame_len); + _rtcm_injection_rate_message_count++; + } } -bool GPS::injectData(uint8_t *data, size_t len) +bool GPS::injectData(const uint8_t *data, size_t len) { dumpGpsData(data, len, gps_dump_comm_mode_t::Full, true); @@ -687,7 +701,7 @@ void GPS::initializeCommunicationDump() _dump_communication_mode = (gps_dump_comm_mode_t)param_dump_comm; } -void GPS::dumpGpsData(uint8_t *data, size_t len, gps_dump_comm_mode_t mode, bool msg_to_gps_device) +void GPS::dumpGpsData(const uint8_t *data, size_t len, gps_dump_comm_mode_t mode, bool msg_to_gps_device) { gps_dump_s *dump_data = msg_to_gps_device ? _dump_to_device : _dump_from_device; @@ -695,7 +709,7 @@ void GPS::dumpGpsData(uint8_t *data, size_t len, gps_dump_comm_mode_t mode, bool return; } - dump_data->instance = (uint8_t)_instance; + dump_data->device_id = get_device_id(); while (len > 0) { size_t write_len = len; @@ -797,6 +811,13 @@ GPS::run() param_get(handle, &f9p_uart2_baudrate); } + handle = param_find("GPS_UBX_PPK"); + int32_t ppk_output = 0; + + if (handle != PARAM_INVALID) { + param_get(handle, &ppk_output); + } + int32_t gnssSystemsParam = static_cast(GPSHelper::GNSSSystemsMask::RECEIVER_DEFAULTS); if (_instance == Instance::Main) { @@ -878,11 +899,21 @@ GPS::run() _mode = gps_driver_mode_t::UBX; /* FALLTHROUGH */ - case gps_driver_mode_t::UBX: - _helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_sensor_gps, _p_report_sat_info, - gps_ubx_dynmodel, heading_offset, f9p_uart2_baudrate, ubx_mode); - set_device_type(DRV_GPS_DEVTYPE_UBX); - break; + case gps_driver_mode_t::UBX: { + GPSDriverUBX::Settings settings = { + .dynamic_model = (uint8_t)gps_ubx_dynmodel, + .heading_offset = heading_offset, + .uart2_baudrate = f9p_uart2_baudrate, + .ppk_output = ppk_output > 0, + .mode = ubx_mode, + }; + + _helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_sensor_gps, _p_report_sat_info, settings); + + set_device_type(DRV_GPS_DEVTYPE_UBX); + break; + } + #ifndef CONSTRAINED_FLASH case gps_driver_mode_t::MTK: @@ -1020,6 +1051,8 @@ GPS::run() healthy_timeout += TIMEOUT_DUMP_ADD; } + PX4_INFO("GPS device configured @ %u baud", _baudrate); + while ((helper_ret = _helper->receive(receive_timeout)) > 0 && !should_exit()) { if (helper_ret & 1) { diff --git a/src/drivers/gps/params.c b/src/drivers/gps/params.c index 92fb83323d..dbc655c8d3 100644 --- a/src/drivers/gps/params.c +++ b/src/drivers/gps/params.c @@ -144,6 +144,15 @@ PARAM_DEFINE_INT32(GPS_UBX_BAUD2, 230400); */ PARAM_DEFINE_INT32(GPS_UBX_CFG_INTF, 0); +/** + * Enable MSM7 message output for PPK workflow. + * + * @boolean + * @reboot_required true + * @group GPS + */ +PARAM_DEFINE_INT32(GPS_UBX_PPK, 0); + /** * Wipes the flash config of UBX modules. * From 67d62cb371d111c62616cb79c01ea7320287a292 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Thu, 11 Dec 2025 21:32:26 -0900 Subject: [PATCH 2/5] Revert "gps: fix RTCM injection to use inject-before-read pattern as before. Add RTCM parser to frame-align injection. Drain GpsInjectData uORB queue into RTCM parser buffer and then inject. Add GPS_UBX_PPK parameter to enable MSM7 output from the GPS module (rather than the default of MSM4) which is required for PPK workflows." This reverts commit 0704580a30bb319fddc8c036675996a3e7202704. --- src/drivers/gps/CMakeLists.txt | 1 - src/drivers/gps/devices | 2 +- src/drivers/gps/gps.cpp | 95 +++++++++++----------------------- src/drivers/gps/params.c | 9 ---- 4 files changed, 32 insertions(+), 75 deletions(-) diff --git a/src/drivers/gps/CMakeLists.txt b/src/drivers/gps/CMakeLists.txt index 3c3c0cc3e0..4719e0e34e 100644 --- a/src/drivers/gps/CMakeLists.txt +++ b/src/drivers/gps/CMakeLists.txt @@ -55,5 +55,4 @@ px4_add_module( module.yaml DEPENDS git_gps_devices - gnss ) diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices index 991f771a8e..caf5158061 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit 991f771a8e192ccde3feb84fad3fb4fa3a3ef7bb +Subproject commit caf5158061bd10e79c9f042abb62c86bc6f3e7a7 diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 2e1bdccdbd..e7dbab947a 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -67,8 +67,6 @@ #include #include -#include - #ifndef CONSTRAINED_FLASH # include "devices/src/ashtech.h" # include "devices/src/emlid_reach.h" @@ -217,8 +215,6 @@ private: gps_dump_s *_dump_from_device{nullptr}; gps_dump_comm_mode_t _dump_communication_mode{gps_dump_comm_mode_t::Disabled}; - gnss::Rtcm3Parser _rtcm_parser{}; - static px4::atomic_bool _is_gps_main_advertised; ///< for the second gps we want to make sure that it gets instance 1 /// and thus we wait until the first one publishes at least one message. @@ -268,7 +264,7 @@ private: * @param data * @param len */ - inline bool injectData(const uint8_t *data, size_t len); + inline bool injectData(uint8_t *data, size_t len); /** * set the Baudrate @@ -289,7 +285,7 @@ private: * @param mode calling source * @param msg_to_gps_device if true, this is a message sent to the gps device, otherwise it's from the device */ - void dumpGpsData(const uint8_t *data, size_t len, gps_dump_comm_mode_t mode, bool msg_to_gps_device); + void dumpGpsData(uint8_t *data, size_t len, gps_dump_comm_mode_t mode, bool msg_to_gps_device); void initializeCommunicationDump(); @@ -476,16 +472,28 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout) const int max_timeout = 50; int timeout_adjusted = math::min(max_timeout, timeout); - handleInjectDataTopic(); - if (_interface == GPSHelper::Interface::UART) { - ret = _uart.readAtLeast(buf, buf_length, math::min(character_count, buf_length), timeout_adjusted); + + const ssize_t read_at_least = math::min(character_count, buf_length); + + // handle injection data before read if caught up + if (_uart.bytesAvailable() < read_at_least) { + handleInjectDataTopic(); + } + + ret = _uart.readAtLeast(buf, buf_length, read_at_least, timeout_adjusted); + + if (ret > 0) { + _num_bytes_read += ret; + } // SPI is only supported on LInux #if defined(__PX4_LINUX) } else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)) { + handleInjectDataTopic(); + //Poll only for the SPI data. In the same thread we also need to handle orb messages, //so ideally we would poll on both, the SPI fd and orb subscription. Unfortunately the //two pollings use different underlying mechanisms (at least under posix), which makes this @@ -575,7 +583,6 @@ void GPS::handleInjectDataTopic() // Looking at 8 packets thus guarantees, that at least a full injection // data set is evaluated. // Moving Base reuires a higher rate, so we allow up to 8 packets. - // Drain uORB messages into RTCM parser and inject full messages after draining the queue. const size_t max_num_injections = gps_inject_data_s::ORB_QUEUE_LENGTH; size_t num_injections = 0; @@ -585,13 +592,13 @@ void GPS::handleInjectDataTopic() // Prevent injection of data from self if (msg.device_id != get_device_id()) { - // Add data to the RTCM parser buffer for frame reassembly - size_t added = _rtcm_parser.addData(msg.data, msg.len); - - if (added < msg.len) { - PX4_WARN("RTCM buffer full, dropped %zu bytes", msg.len - added); - } + /* Write the message to the gps device. Note that the message could be fragmented. + * But as we don't write anywhere else to the device during operation, we don't + * need to assemble the message first. + */ + injectData(msg.data, msg.len); + ++_rtcm_injection_rate_message_count; _last_rtcm_injection_time = hrt_absolute_time(); } } @@ -609,30 +616,9 @@ void GPS::handleInjectDataTopic() } } while (updated && num_injections < max_num_injections); - - // Now inject all complete RTCM frames from the parser buffer - size_t frame_len = {}; - const uint8_t *frame_ptr = {}; - - while ((frame_ptr = _rtcm_parser.getNextMessage(&frame_len)) != nullptr) { - // Check TX buffer space before writing - if (_interface == GPSHelper::Interface::UART) { - ssize_t tx_available = _uart.txSpaceAvailable(); - - if ((ssize_t)frame_len > tx_available) { - // TX buffer full, stop and let it drain - frames stay in parser buffer - PX4_WARN("TX buffer full!"); - break; - } - } - - injectData(frame_ptr, frame_len); - _rtcm_parser.consumeMessage(frame_len); - _rtcm_injection_rate_message_count++; - } } -bool GPS::injectData(const uint8_t *data, size_t len) +bool GPS::injectData(uint8_t *data, size_t len) { dumpGpsData(data, len, gps_dump_comm_mode_t::Full, true); @@ -701,7 +687,7 @@ void GPS::initializeCommunicationDump() _dump_communication_mode = (gps_dump_comm_mode_t)param_dump_comm; } -void GPS::dumpGpsData(const uint8_t *data, size_t len, gps_dump_comm_mode_t mode, bool msg_to_gps_device) +void GPS::dumpGpsData(uint8_t *data, size_t len, gps_dump_comm_mode_t mode, bool msg_to_gps_device) { gps_dump_s *dump_data = msg_to_gps_device ? _dump_to_device : _dump_from_device; @@ -709,7 +695,7 @@ void GPS::dumpGpsData(const uint8_t *data, size_t len, gps_dump_comm_mode_t mode return; } - dump_data->device_id = get_device_id(); + dump_data->instance = (uint8_t)_instance; while (len > 0) { size_t write_len = len; @@ -811,13 +797,6 @@ GPS::run() param_get(handle, &f9p_uart2_baudrate); } - handle = param_find("GPS_UBX_PPK"); - int32_t ppk_output = 0; - - if (handle != PARAM_INVALID) { - param_get(handle, &ppk_output); - } - int32_t gnssSystemsParam = static_cast(GPSHelper::GNSSSystemsMask::RECEIVER_DEFAULTS); if (_instance == Instance::Main) { @@ -899,21 +878,11 @@ GPS::run() _mode = gps_driver_mode_t::UBX; /* FALLTHROUGH */ - case gps_driver_mode_t::UBX: { - GPSDriverUBX::Settings settings = { - .dynamic_model = (uint8_t)gps_ubx_dynmodel, - .heading_offset = heading_offset, - .uart2_baudrate = f9p_uart2_baudrate, - .ppk_output = ppk_output > 0, - .mode = ubx_mode, - }; - - _helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_sensor_gps, _p_report_sat_info, settings); - - set_device_type(DRV_GPS_DEVTYPE_UBX); - break; - } - + case gps_driver_mode_t::UBX: + _helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_sensor_gps, _p_report_sat_info, + gps_ubx_dynmodel, heading_offset, f9p_uart2_baudrate, ubx_mode); + set_device_type(DRV_GPS_DEVTYPE_UBX); + break; #ifndef CONSTRAINED_FLASH case gps_driver_mode_t::MTK: @@ -1051,8 +1020,6 @@ GPS::run() healthy_timeout += TIMEOUT_DUMP_ADD; } - PX4_INFO("GPS device configured @ %u baud", _baudrate); - while ((helper_ret = _helper->receive(receive_timeout)) > 0 && !should_exit()) { if (helper_ret & 1) { diff --git a/src/drivers/gps/params.c b/src/drivers/gps/params.c index dbc655c8d3..92fb83323d 100644 --- a/src/drivers/gps/params.c +++ b/src/drivers/gps/params.c @@ -144,15 +144,6 @@ PARAM_DEFINE_INT32(GPS_UBX_BAUD2, 230400); */ PARAM_DEFINE_INT32(GPS_UBX_CFG_INTF, 0); -/** - * Enable MSM7 message output for PPK workflow. - * - * @boolean - * @reboot_required true - * @group GPS - */ -PARAM_DEFINE_INT32(GPS_UBX_PPK, 0); - /** * Wipes the flash config of UBX modules. * From c25fcabcc637b3685800f057e1213c7e86a7fd64 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Thu, 11 Dec 2025 23:11:03 -0900 Subject: [PATCH 3/5] esc_battery: fix current reporting --- src/modules/esc_battery/EscBattery.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/esc_battery/EscBattery.cpp b/src/modules/esc_battery/EscBattery.cpp index 78d1c2c71f..6dfc17aeb3 100644 --- a/src/modules/esc_battery/EscBattery.cpp +++ b/src/modules/esc_battery/EscBattery.cpp @@ -105,7 +105,6 @@ EscBattery::Run() } average_voltage_v /= online_esc_count; - total_current_a /= online_esc_count; average_temperature_c /= online_esc_count; _battery.setConnected(true); From 12745baf6cf8c30ecd9bf565583eb7e69ed2f57e Mon Sep 17 00:00:00 2001 From: Alex Klimaj Date: Fri, 12 Dec 2025 11:31:19 -0700 Subject: [PATCH 4/5] Adds configurable I2C address for PCA9685 PWM driver (#26051) * Adds configurable I2C address for PCA9685 PWM driver Introduces a parameter to set the I2C address for the PCA9685 PWM output driver, enhancing flexibility for hardware variations. Updates documentation and board initialization scripts to support the new configuration and streamline device startup. * Update src/drivers/pca9685_pwm_out/module.yaml Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> * Update src/drivers/pca9685_pwm_out/module.yaml --------- Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> --- boards/ark/fmu-v6x/default.px4board | 1 + boards/ark/fmu-v6x/init/rc.board_sensors | 6 +++++ boards/ark/fpv/default.px4board | 1 + boards/ark/fpv/init/rc.board_sensors | 6 +++++ boards/ark/pi6x/default.px4board | 1 + boards/ark/pi6x/init/rc.board_sensors | 6 +++++ boards/auterion/fmu-v6s/init/rc.board_sensors | 2 +- .../navigator/init/rc.board_defaults | 4 +++ .../navigator/init/rc.board_sensors | 2 +- .../en/advanced_config/parameter_reference.md | 10 +++++++ docs/en/modules/modules_driver.md | 2 ++ src/drivers/pca9685_pwm_out/main.cpp | 27 +++++++++++++++++-- src/drivers/pca9685_pwm_out/module.yaml | 10 +++++++ 13 files changed, 74 insertions(+), 4 deletions(-) diff --git a/boards/ark/fmu-v6x/default.px4board b/boards/ark/fmu-v6x/default.px4board index e2433a53cb..d72a3f8a4e 100644 --- a/boards/ark/fmu-v6x/default.px4board +++ b/boards/ark/fmu-v6x/default.px4board @@ -40,6 +40,7 @@ CONFIG_DRIVERS_MAGNETOMETER_ST_IIS2MDC=y CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_POWER_MONITOR_INA228=y CONFIG_DRIVERS_POWER_MONITOR_INA238=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_PX4IO=y CONFIG_COMMON_RC=y diff --git a/boards/ark/fmu-v6x/init/rc.board_sensors b/boards/ark/fmu-v6x/init/rc.board_sensors index c16ed31f69..7b2259f8ad 100644 --- a/boards/ark/fmu-v6x/init/rc.board_sensors +++ b/boards/ark/fmu-v6x/init/rc.board_sensors @@ -97,5 +97,11 @@ bmm150 -I start # Internal Baro on I2C bmp388 -I start +# Start an external PWM generator +if param greater PCA9685_EN_BUS 0 +then + pca9685_pwm_out start +fi + unset HAVE_PM2 unset HAVE_PM3 diff --git a/boards/ark/fpv/default.px4board b/boards/ark/fpv/default.px4board index abea5574b0..d4515d5f67 100644 --- a/boards/ark/fpv/default.px4board +++ b/boards/ark/fpv/default.px4board @@ -21,6 +21,7 @@ CONFIG_DRIVERS_IMU_MURATA_SCH16T=y CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y CONFIG_DRIVERS_OSD_MSP_OSD=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_COMMON_RC=y CONFIG_DRIVERS_UAVCAN=y diff --git a/boards/ark/fpv/init/rc.board_sensors b/boards/ark/fpv/init/rc.board_sensors index 896b9ffd7d..e719938f86 100644 --- a/boards/ark/fpv/init/rc.board_sensors +++ b/boards/ark/fpv/init/rc.board_sensors @@ -16,3 +16,9 @@ iis2mdc -R 0 -I -b 4 start # Internal Baro on I2C bmp388 -I -b 2 start + +# Start an external PWM generator +if param greater PCA9685_EN_BUS 0 +then + pca9685_pwm_out start +fi diff --git a/boards/ark/pi6x/default.px4board b/boards/ark/pi6x/default.px4board index dda45cdf23..6ad26366b3 100644 --- a/boards/ark/pi6x/default.px4board +++ b/boards/ark/pi6x/default.px4board @@ -19,6 +19,7 @@ CONFIG_COMMON_LIGHT=y CONFIG_DRIVERS_MAGNETOMETER_MEMSIC_MMC5983MA=y CONFIG_DRIVERS_MAGNETOMETER_ST_IIS2MDC=y CONFIG_DRIVERS_OPTICAL_FLOW_PAW3902=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_COMMON_RC=y diff --git a/boards/ark/pi6x/init/rc.board_sensors b/boards/ark/pi6x/init/rc.board_sensors index cc97b1d601..c128fbca35 100644 --- a/boards/ark/pi6x/init/rc.board_sensors +++ b/boards/ark/pi6x/init/rc.board_sensors @@ -34,3 +34,9 @@ paw3902 -s -b 3 start -Y 90 # Internal distance sensor afbrs50 start + +# Start an external PWM generator +if param greater PCA9685_EN_BUS 0 +then + pca9685_pwm_out start +fi diff --git a/boards/auterion/fmu-v6s/init/rc.board_sensors b/boards/auterion/fmu-v6s/init/rc.board_sensors index 4c4be283d7..ccf6536cf0 100644 --- a/boards/auterion/fmu-v6s/init/rc.board_sensors +++ b/boards/auterion/fmu-v6s/init/rc.board_sensors @@ -74,5 +74,5 @@ ist8310 -X -b 1 -R 10 start # Start an external PWM generator if param greater PCA9685_EN_BUS 0 then - pca9685_pwm_out start -b 1 + pca9685_pwm_out start fi diff --git a/boards/bluerobotics/navigator/init/rc.board_defaults b/boards/bluerobotics/navigator/init/rc.board_defaults index 18dc930ba7..ca136db1e3 100644 --- a/boards/bluerobotics/navigator/init/rc.board_defaults +++ b/boards/bluerobotics/navigator/init/rc.board_defaults @@ -11,3 +11,7 @@ param set BAT1_V_DIV 5.7 # Always keep current config param set SYS_AUTOCONFIG 0 + +# PCA9685 PWM Out defaults +param set-default PCA9685_EN_BUS 4 +param set-default PCA9685_I2C_ADDR 64 diff --git a/boards/bluerobotics/navigator/init/rc.board_sensors b/boards/bluerobotics/navigator/init/rc.board_sensors index 411995664b..790099d8af 100644 --- a/boards/bluerobotics/navigator/init/rc.board_sensors +++ b/boards/bluerobotics/navigator/init/rc.board_sensors @@ -28,7 +28,7 @@ then echo "ads1115 not found." fi -if ! pca9685_pwm_out start -a 0x40 -b 4 +if ! pca9685_pwm_out start then echo "pca9685_pwm_out not found." fi diff --git a/docs/en/advanced_config/parameter_reference.md b/docs/en/advanced_config/parameter_reference.md index a9a13e4984..aa05292a85 100644 --- a/docs/en/advanced_config/parameter_reference.md +++ b/docs/en/advanced_config/parameter_reference.md @@ -486,6 +486,16 @@ The integer refers to the I2C bus number where PCA9685 is connected. | ------ | -------- | -------- | --------- | ------- | ---- | |   | 0 | 10 | | 0 | +### PCA9685_I2C_ADDR (`INT32`) {#PCA9685_I2C_ADDR} + +I2C address of PCA9685. + +The default address is 0x40 (64). + +| Reboot | minValue | maxValue | increment | default | unit | +| ------ | -------- | -------- | --------- | ------- | ---- | +|   | 0 | 127 | | 64 | + ### PCA9685_FAIL1 (`INT32`) {#PCA9685_FAIL1} PCA9685 Output Channel 1 Failsafe Value. diff --git a/docs/en/modules/modules_driver.md b/docs/en/modules/modules_driver.md index 2b0b2a6b7e..9db2c26c0c 100644 --- a/docs/en/modules/modules_driver.md +++ b/docs/en/modules/modules_driver.md @@ -899,6 +899,8 @@ fetching the latest mixing result and write them to PCA9685 at its scheduling ti It can do full 12bits output as duty-cycle mode, while also able to output precious pulse width that can be accepted by most ESCs and servos. +The I2C bus and address can be configured via parameters `PCA9685_EN_BUS` and `PCA9685_I2C_ADDR`, or via command line arguments. + ### Examples It is typically started with: diff --git a/src/drivers/pca9685_pwm_out/main.cpp b/src/drivers/pca9685_pwm_out/main.cpp index 2d22c36a77..44464e9511 100644 --- a/src/drivers/pca9685_pwm_out/main.cpp +++ b/src/drivers/pca9685_pwm_out/main.cpp @@ -47,6 +47,7 @@ #include #include #include +#include #include "PCA9685.h" @@ -356,8 +357,30 @@ int PCA9685Wrapper::custom_command(int argc, char **argv) { int PCA9685Wrapper::task_spawn(int argc, char **argv) { int ch; - int address=PCA9685_DEFAULT_ADDRESS; - int iicbus=PCA9685_DEFAULT_IICBUS; + int address = PCA9685_DEFAULT_ADDRESS; + int iicbus = PCA9685_DEFAULT_IICBUS; + + int32_t en_bus = 0; + param_t param_handle = param_find("PCA9685_EN_BUS"); + + if (param_handle != PARAM_INVALID) { + param_get(param_handle, &en_bus); + + if (en_bus > 0) { + iicbus = en_bus; + } + } + + int32_t i2c_addr = 0; + param_handle = param_find("PCA9685_I2C_ADDR"); + + if (param_handle != PARAM_INVALID) { + param_get(param_handle, &i2c_addr); + + if (i2c_addr > 0) { + address = i2c_addr; + } + } int myoptind = 1; const char *myoptarg = nullptr; diff --git a/src/drivers/pca9685_pwm_out/module.yaml b/src/drivers/pca9685_pwm_out/module.yaml index 57294c0c81..18d999830a 100644 --- a/src/drivers/pca9685_pwm_out/module.yaml +++ b/src/drivers/pca9685_pwm_out/module.yaml @@ -29,6 +29,16 @@ parameters: min: 0 max: 10 default: 0 + PCA9685_I2C_ADDR: + description: + short: I2C address of PCA9685 + long: | + I2C address of PCA9685. + The default address is 0x40 (64). + type: int32 + min: 1 + max: 127 + default: 64 PCA9685_SCHD_HZ: description: short: PWM update rate From b92d21bd3198b60e241d9fc43815d30ca6fd203b Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Fri, 12 Dec 2025 09:31:33 -0900 Subject: [PATCH 5/5] serial: add txSpaceAvailable function (#26069) * serial: add txSpaceAvailable function * serial: txSpaceAvailable and bytesAvailable fixups --- platforms/common/Serial.cpp | 5 ++++ .../include/px4_platform_common/Serial.hpp | 1 + platforms/nuttx/src/px4/common/SerialImpl.cpp | 26 ++++++++++++++++++- .../src/px4/common/include/SerialImpl.hpp | 1 + platforms/posix/include/SerialImpl.hpp | 1 + platforms/posix/src/px4/common/SerialImpl.cpp | 21 ++++++++++++++- platforms/qurt/include/SerialImpl.hpp | 1 + platforms/qurt/src/px4/SerialImpl.cpp | 21 ++++++++++++++- 8 files changed, 74 insertions(+), 3 deletions(-) diff --git a/platforms/common/Serial.cpp b/platforms/common/Serial.cpp index 748dcba9b2..a129fe2492 100644 --- a/platforms/common/Serial.cpp +++ b/platforms/common/Serial.cpp @@ -74,6 +74,11 @@ ssize_t Serial::bytesAvailable() return _impl.bytesAvailable(); } +ssize_t Serial::txSpaceAvailable() +{ + return _impl.txSpaceAvailable(); +} + ssize_t Serial::read(uint8_t *buffer, size_t buffer_size) { return _impl.read(buffer, buffer_size); diff --git a/platforms/common/include/px4_platform_common/Serial.hpp b/platforms/common/include/px4_platform_common/Serial.hpp index 983834ccf6..bf14035a5e 100644 --- a/platforms/common/include/px4_platform_common/Serial.hpp +++ b/platforms/common/include/px4_platform_common/Serial.hpp @@ -62,6 +62,7 @@ public: bool close(); ssize_t bytesAvailable(); + ssize_t txSpaceAvailable(); ssize_t read(uint8_t *buffer, size_t buffer_size); ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_ms = 0); diff --git a/platforms/nuttx/src/px4/common/SerialImpl.cpp b/platforms/nuttx/src/px4/common/SerialImpl.cpp index f8968e4881..9bee2ddf23 100644 --- a/platforms/nuttx/src/px4/common/SerialImpl.cpp +++ b/platforms/nuttx/src/px4/common/SerialImpl.cpp @@ -293,12 +293,36 @@ ssize_t SerialImpl::bytesAvailable() { if (!_open) { PX4_ERR("Device not open!"); + errno = EBADF; return -1; } ssize_t bytes_available = 0; int ret = ioctl(_serial_fd, FIONREAD, &bytes_available); - return ret >= 0 ? bytes_available : 0; + + if (ret < 0) { + return -1; + } + + return bytes_available; +} + +ssize_t SerialImpl::txSpaceAvailable() +{ + if (!_open) { + PX4_ERR("Device not open!"); + errno = EBADF; + return -1; + } + + ssize_t space_available = 0; + int ret = ioctl(_serial_fd, FIONSPACE, &space_available); + + if (ret < 0) { + return -1; + } + + return space_available; } ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size) diff --git a/platforms/nuttx/src/px4/common/include/SerialImpl.hpp b/platforms/nuttx/src/px4/common/include/SerialImpl.hpp index 82e4ddd29d..32c549ad6e 100644 --- a/platforms/nuttx/src/px4/common/include/SerialImpl.hpp +++ b/platforms/nuttx/src/px4/common/include/SerialImpl.hpp @@ -60,6 +60,7 @@ public: bool close(); ssize_t bytesAvailable(); + ssize_t txSpaceAvailable(); ssize_t read(uint8_t *buffer, size_t buffer_size); ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0); diff --git a/platforms/posix/include/SerialImpl.hpp b/platforms/posix/include/SerialImpl.hpp index 82e4ddd29d..32c549ad6e 100644 --- a/platforms/posix/include/SerialImpl.hpp +++ b/platforms/posix/include/SerialImpl.hpp @@ -60,6 +60,7 @@ public: bool close(); ssize_t bytesAvailable(); + ssize_t txSpaceAvailable(); ssize_t read(uint8_t *buffer, size_t buffer_size); ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0); diff --git a/platforms/posix/src/px4/common/SerialImpl.cpp b/platforms/posix/src/px4/common/SerialImpl.cpp index cb12b6919d..4859832829 100644 --- a/platforms/posix/src/px4/common/SerialImpl.cpp +++ b/platforms/posix/src/px4/common/SerialImpl.cpp @@ -281,12 +281,31 @@ ssize_t SerialImpl::bytesAvailable() { if (!_open) { PX4_ERR("Device not open!"); + errno = EBADF; return -1; } ssize_t bytes_available = 0; int ret = ioctl(_serial_fd, FIONREAD, &bytes_available); - return ret >= 0 ? bytes_available : 0; + + if (ret < 0) { + return -1; + } + + return bytes_available; +} + +ssize_t SerialImpl::txSpaceAvailable() +{ + if (!_open) { + PX4_ERR("Device not open!"); + errno = EBADF; + return -1; + } + + // POSIX/Linux doesn't have a direct equivalent to NuttX's FIONSPACE + errno = ENOSYS; + return -1; } ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size) diff --git a/platforms/qurt/include/SerialImpl.hpp b/platforms/qurt/include/SerialImpl.hpp index c7739429ef..09797a33f7 100644 --- a/platforms/qurt/include/SerialImpl.hpp +++ b/platforms/qurt/include/SerialImpl.hpp @@ -59,6 +59,7 @@ public: bool close(); ssize_t bytesAvailable(); + ssize_t txSpaceAvailable(); ssize_t read(uint8_t *buffer, size_t buffer_size); ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0); diff --git a/platforms/qurt/src/px4/SerialImpl.cpp b/platforms/qurt/src/px4/SerialImpl.cpp index 5c31be9a5c..c7d78baffe 100644 --- a/platforms/qurt/src/px4/SerialImpl.cpp +++ b/platforms/qurt/src/px4/SerialImpl.cpp @@ -156,14 +156,33 @@ ssize_t SerialImpl::bytesAvailable() { if (!_open) { PX4_ERR("Device not open!"); + errno = EBADF; return -1; } uint32_t rx_bytes = 0; - (void) fc_uart_rx_available(_serial_fd, &rx_bytes); + int ret = fc_uart_rx_available(_serial_fd, &rx_bytes); + + if (ret < 0) { + return -1; + } + return (ssize_t) rx_bytes; } +ssize_t SerialImpl::txSpaceAvailable() +{ + if (!_open) { + PX4_ERR("Device not open!"); + errno = EBADF; + return -1; + } + + // QURT doesn't have a direct equivalent to NuttX's FIONSPACE + errno = ENOSYS; + return -1; +} + ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size) { if (!_open) {