From bd76832f34e744548593f344c130b3d86abd95c7 Mon Sep 17 00:00:00 2001 From: Eric Katzfey Date: Sat, 21 Feb 2026 16:43:16 -0700 Subject: [PATCH] voxl2: Added components to the board build that are in the ModalAI fork but missing in mainline --- .gitmodules | 6 + Tools/astyle/files_to_check_code_style.sh | 2 + boards/modalai/voxl2-slpi/default.px4board | 7 + boards/modalai/voxl2/default.px4board | 2 + boards/modalai/voxl2/scripts/install-voxl.sh | 5 + boards/modalai/voxl2/src/CMakeLists.txt | 8 + boards/modalai/voxl2/src/board_config.h | 14 +- boards/modalai/voxl2/src/i2c.cpp | 40 +++ .../modalai/voxl2/src/lib/mpa/CMakeLists.txt | 44 +++ .../modalai/voxl2/src/lib/mpa/libmodal-json | 1 + .../modalai/voxl2/src/lib/mpa/libmodal-pipe | 1 + boards/modalai/voxl2/src/lib/mpa/mpa.cpp | 262 ++++++++++++++++++ boards/modalai/voxl2/src/lib/mpa/mpa.hpp | 83 ++++++ .../modules/sensor_baro_bridge/CMakeLists.txt | 43 +++ .../sensor_baro_bridge/sensor_baro_bridge.cpp | 183 ++++++++++++ .../vehicle_air_data_bridge/CMakeLists.txt | 43 +++ .../vehicle_air_data_bridge.cpp | 183 ++++++++++++ .../CMakeLists.txt | 43 +++ .../vehicle_local_position_bridge.cpp | 238 ++++++++++++++++ boards/modalai/voxl2/src/spi.cpp | 40 +++ boards/modalai/voxl2/target/voxl-px4 | 23 +- boards/modalai/voxl2/target/voxl-px4-start | 142 ++++++---- src/drivers/barometer/dps310/CMakeLists.txt | 1 + 23 files changed, 1351 insertions(+), 63 deletions(-) create mode 100644 boards/modalai/voxl2/src/i2c.cpp create mode 100644 boards/modalai/voxl2/src/lib/mpa/CMakeLists.txt create mode 160000 boards/modalai/voxl2/src/lib/mpa/libmodal-json create mode 160000 boards/modalai/voxl2/src/lib/mpa/libmodal-pipe create mode 100644 boards/modalai/voxl2/src/lib/mpa/mpa.cpp create mode 100644 boards/modalai/voxl2/src/lib/mpa/mpa.hpp create mode 100644 boards/modalai/voxl2/src/modules/sensor_baro_bridge/CMakeLists.txt create mode 100644 boards/modalai/voxl2/src/modules/sensor_baro_bridge/sensor_baro_bridge.cpp create mode 100644 boards/modalai/voxl2/src/modules/vehicle_air_data_bridge/CMakeLists.txt create mode 100644 boards/modalai/voxl2/src/modules/vehicle_air_data_bridge/vehicle_air_data_bridge.cpp create mode 100644 boards/modalai/voxl2/src/modules/vehicle_local_position_bridge/CMakeLists.txt create mode 100644 boards/modalai/voxl2/src/modules/vehicle_local_position_bridge/vehicle_local_position_bridge.cpp create mode 100644 boards/modalai/voxl2/src/spi.cpp diff --git a/.gitmodules b/.gitmodules index 86cb85c961..2d406b1017 100644 --- a/.gitmodules +++ b/.gitmodules @@ -109,3 +109,9 @@ [submodule "src/lib/rl_tools/rl_tools"] path = src/lib/rl_tools/rl_tools url = https://github.com/rl-tools/rl-tools.git +[submodule "libmodal-json"] + path = boards/modalai/voxl2/src/lib/mpa/libmodal-json + url = https://gitlab.com/voxl-public/voxl-sdk/core-libs/libmodal-json.git +[submodule "libmodal-pipe"] + path = boards/modalai/voxl2/src/lib/mpa/libmodal-pipe + url = https://gitlab.com/voxl-public/voxl-sdk/core-libs/libmodal-pipe.git diff --git a/Tools/astyle/files_to_check_code_style.sh b/Tools/astyle/files_to_check_code_style.sh index b70766efa2..b9e48d1d77 100755 --- a/Tools/astyle/files_to_check_code_style.sh +++ b/Tools/astyle/files_to_check_code_style.sh @@ -39,6 +39,8 @@ exec find boards msg src platforms test \ -path src/lib/cdrstream/rosidl -prune -o \ -path src/modules/zenoh/zenoh-pico -prune -o \ -path boards/modalai/voxl2/libfc-sensor-api -prune -o \ + -path boards/modalai/voxl2/src/lib/mpa/libmodal-json -prune -o \ + -path boards/modalai/voxl2/src/lib/mpa/libmodal-pipe -prune -o \ -path src/drivers/actuators/vertiq_io/iq-module-communication-cpp -prune -o \ -path src/lib/tensorflow_lite_micro/tflite_micro -prune -o \ -path src/drivers/ins/sbgecom/sbgECom -prune -o \ diff --git a/boards/modalai/voxl2-slpi/default.px4board b/boards/modalai/voxl2-slpi/default.px4board index 045de27b88..6afa6f0370 100644 --- a/boards/modalai/voxl2-slpi/default.px4board +++ b/boards/modalai/voxl2-slpi/default.px4board @@ -5,6 +5,7 @@ CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP101XX=y CONFIG_DRIVERS_BAROMETER_MS5611=y CONFIG_DRIVERS_BAROMETER_BMP280=y CONFIG_DRIVERS_BAROMETER_BMP388=y +CONFIG_DRIVERS_BAROMETER_DPS310=y CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_MS4525DO=y CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L0X=y CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L1X=y @@ -16,6 +17,7 @@ CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8308=y CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y +CONFIG_DRIVERS_MAGNETOMETER_ST_IIS2MDC=y CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y CONFIG_DRIVERS_QSHELL_QURT=y CONFIG_DRIVERS_RC_CRSF_RC=y @@ -29,6 +31,11 @@ CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y diff --git a/boards/modalai/voxl2/default.px4board b/boards/modalai/voxl2/default.px4board index 9bae25cd47..ab95da688d 100644 --- a/boards/modalai/voxl2/default.px4board +++ b/boards/modalai/voxl2/default.px4board @@ -3,6 +3,8 @@ CONFIG_BOARD_LINUX_TARGET=y CONFIG_BOARD_TOOLCHAIN="aarch64-linux-gnu" CONFIG_BOARD_ROOT_PATH="/data/px4" CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y +CONFIG_DRIVERS_BAROMETER_DPS310=y +CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP101XX=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_OSD_MSP_OSD=y CONFIG_DRIVERS_QSHELL_POSIX=y diff --git a/boards/modalai/voxl2/scripts/install-voxl.sh b/boards/modalai/voxl2/scripts/install-voxl.sh index a908b0a835..88112d61ea 100755 --- a/boards/modalai/voxl2/scripts/install-voxl.sh +++ b/boards/modalai/voxl2/scripts/install-voxl.sh @@ -128,6 +128,11 @@ adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-flight_mode_manager" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-imu_server" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-apps_sbus" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-voxl_save_cal_params" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-vehicle_air_data_bridge" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-sensor_baro_bridge" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-dps310" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-icp101xx" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-vehicle_local_position_bridge" # Make sure any required directories exist adb shell "/bin/mkdir -p /data/px4/param" diff --git a/boards/modalai/voxl2/src/CMakeLists.txt b/boards/modalai/voxl2/src/CMakeLists.txt index 85b1cd7dd1..77e7a451ad 100644 --- a/boards/modalai/voxl2/src/CMakeLists.txt +++ b/boards/modalai/voxl2/src/CMakeLists.txt @@ -39,12 +39,20 @@ set(DISABLE_PARAMS_MODULE_SCOPING TRUE PARENT_SCOPE) add_library(drivers_board board_config.h + i2c.cpp init.c boardctl.c + spi.cpp ) # Add custom drivers add_subdirectory(${PX4_BOARD_DIR}/src/drivers/apps_sbus) +# Add custom libraries +add_subdirectory(${PX4_BOARD_DIR}/src/lib/mpa) + # Add custom modules add_subdirectory(${PX4_BOARD_DIR}/src/modules/voxl_save_cal_params) +add_subdirectory(${PX4_BOARD_DIR}/src/modules/vehicle_air_data_bridge) +add_subdirectory(${PX4_BOARD_DIR}/src/modules/sensor_baro_bridge) +add_subdirectory(${PX4_BOARD_DIR}/src/modules/vehicle_local_position_bridge) diff --git a/boards/modalai/voxl2/src/board_config.h b/boards/modalai/voxl2/src/board_config.h index 68ee5389f1..fa2fee21b4 100644 --- a/boards/modalai/voxl2/src/board_config.h +++ b/boards/modalai/voxl2/src/board_config.h @@ -42,9 +42,21 @@ #define CONFIG_BOARDCTL_RESET #define BOARD_HAS_NO_BOOTLOADER -// Define this as empty since there are no I2C buses +// Define this as empty since i2c clock init isn't required #define BOARD_I2C_BUS_CLOCK_INIT +/* + * I2C buses + */ +#define CONFIG_I2C 1 +#define PX4_NUMBER_I2C_BUSES 1 + +/* + * SPI buses + */ +#define CONFIG_SPI 1 +#define BOARD_SPI_BUS_MAX_BUS_ITEMS 1 + #include #include diff --git a/boards/modalai/voxl2/src/i2c.cpp b/boards/modalai/voxl2/src/i2c.cpp new file mode 100644 index 0000000000..fbc45684bb --- /dev/null +++ b/boards/modalai/voxl2/src/i2c.cpp @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * Copyright (C) 2025-2026 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_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(0) +}; diff --git a/boards/modalai/voxl2/src/lib/mpa/CMakeLists.txt b/boards/modalai/voxl2/src/lib/mpa/CMakeLists.txt new file mode 100644 index 0000000000..5ae8e364f0 --- /dev/null +++ b/boards/modalai/voxl2/src/lib/mpa/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2025-2026 ModalAI, Inc. 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_git_submodule(TARGET git_mpa_libmodal-json PATH "libmodal-json") +px4_add_git_submodule(TARGET git_mpa_libmodal-pipe PATH "libmodal-pipe") + +px4_add_library(mpa mpa.cpp) + +target_link_libraries(mpa PRIVATE ${CMAKE_DL_LIBS}) + +target_include_directories(mpa PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/libmodal-json/library/include + ${CMAKE_CURRENT_SOURCE_DIR}/libmodal-pipe/library/include +) diff --git a/boards/modalai/voxl2/src/lib/mpa/libmodal-json b/boards/modalai/voxl2/src/lib/mpa/libmodal-json new file mode 160000 index 0000000000..a18d9eee62 --- /dev/null +++ b/boards/modalai/voxl2/src/lib/mpa/libmodal-json @@ -0,0 +1 @@ +Subproject commit a18d9eee62465b8eef52251515f7195874afa260 diff --git a/boards/modalai/voxl2/src/lib/mpa/libmodal-pipe b/boards/modalai/voxl2/src/lib/mpa/libmodal-pipe new file mode 160000 index 0000000000..be51027375 --- /dev/null +++ b/boards/modalai/voxl2/src/lib/mpa/libmodal-pipe @@ -0,0 +1 @@ +Subproject commit be51027375ca4071a2c2ba7410fb2ddd4bbead0c diff --git a/boards/modalai/voxl2/src/lib/mpa/mpa.cpp b/boards/modalai/voxl2/src/lib/mpa/mpa.cpp new file mode 100644 index 0000000000..2f270b66c2 --- /dev/null +++ b/boards/modalai/voxl2/src/lib/mpa/mpa.cpp @@ -0,0 +1,262 @@ +/**************************************************************************** + * + * Copyright (c) 2025-2026 ModalAI, inc. 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 "mpa.hpp" +#include +#include +#include + +bool MPA::initialized = false; +void *MPA::handle = nullptr; +int MPA::current_client = 0; +int MPA::current_server = 0; + +MPA::pipe_client_set_simple_helper_cb_t MPA::helper_cb = nullptr; +MPA::pipe_client_set_connect_cb_t MPA::connect_cb = nullptr; +MPA::pipe_client_set_disconnect_cb_t MPA::disconnect_cb = nullptr; +MPA::pipe_client_open_t MPA::open_pipe = nullptr; +MPA::pipe_server_create_t MPA::create_pipe = nullptr; +MPA::pipe_server_write_t MPA::write_pipe = nullptr; +MPA::pipe_server_set_control_cb_t MPA::set_control_cb = nullptr; +MPA::pipe_server_close_t MPA::close_pipe = nullptr; +MPA::mpa_data_cb_t MPA::data_cb[MAX_MPA_CLIENTS]; + +// called whenever we connect or reconnect to the server +void MPA::ConnectCB(__attribute__((unused)) int ch, __attribute__((unused)) void *context) +{ + PX4_INFO("vfc status server connected"); + return; +} + +// called whenever we disconnect from the server +void MPA::DisconnectCB(__attribute__((unused)) int ch, __attribute__((unused)) void *context) +{ + PX4_INFO("vfc status server disconnected"); + return; +} + +void MPA::HelperCB(__attribute__((unused)) int ch, char *data, int bytes, __attribute__((unused)) void *context) +{ + // PX4_INFO("Got %d bytes in pipe callback", bytes); + + if (data_cb[ch]) { data_cb[ch](data, bytes); } + + return; +} + +int MPA::PipeClient(const char *pipe_name, int size, mpa_data_cb_t cb) +{ + if (!initialized) { + PX4_ERR("Cannot open pipe %s before initialization", pipe_name); + return -1; + } + + printf("waiting for server for pipe %s\n", pipe_name); + + if (open_pipe(current_client, pipe_name, "px4", EN_PIPE_CLIENT_SIMPLE_HELPER, size * 10) < 0) { + PX4_ERR("Error opening pipe %s", pipe_name); + return -1; + } + + data_cb[current_client] = cb; + current_client++; + + return current_client - 1; +} + +int MPA::PipeCreate(char *pipe_name, int flags) +{ + if (!initialized) { + PX4_ERR("Cannot open pipe %s before initialization", pipe_name); + return -1; + } + + pipe_info_t server_pipe; + strncpy(server_pipe.name, pipe_name, MODAL_PIPE_MAX_NAME_LEN); + server_pipe.name[MODAL_PIPE_MAX_NAME_LEN - 1] = 0; + server_pipe.location[0] = 0; + server_pipe.type[0] = 0; + strncpy(server_pipe.server_name, "px4_mpa", MODAL_PIPE_MAX_NAME_LEN); + server_pipe.size_bytes = MODAL_PIPE_DEFAULT_PIPE_SIZE; + server_pipe.server_pid = 0; + + if (create_pipe(current_server, server_pipe, flags) < 0) { + // remove_pid_file(server_pipe.server_name); + PX4_ERR("Error opening pipe %s", pipe_name); + return -1; + } + + current_server++; + + return current_server - 1; +} + +int MPA::PipeWrite(int ch, const void *data, int bytes) +{ + return write_pipe(ch, data, bytes); +} + +int MPA::PipeServerSetControlCb(int ch, mpa_control_cb_t cb, void *context) +{ + return set_control_cb(ch, cb, context); +} + +void MPA::PipeServerClose(int ch) +{ + if (close_pipe) { + close_pipe(ch); + } +} + +int MPA::Initialize() +{ + if (initialized) { + // Already successfully initialized + return 0; + } + + char libname[] = "libmodal_pipe.so"; + handle = dlopen(libname, RTLD_LAZY | RTLD_GLOBAL); + + if (!handle) { + PX4_ERR("Error opening library %s: %s\n", libname, dlerror()); + return -1; + + } else { + PX4_INFO("Successfully loaded library %s", libname); + } + + // set up all our MPA callbacks + char helper_cb_name[] = "pipe_client_set_simple_helper_cb"; + helper_cb = (pipe_client_set_simple_helper_cb_t) dlsym(handle, helper_cb_name); + + if (!helper_cb) { + PX4_ERR("Error finding symbol %s: %s\n", helper_cb_name, dlerror()); + return -1; + + } else { + PX4_DEBUG("Successfully loaded function %s", helper_cb_name); + } + + helper_cb(0, HelperCB, NULL); + + char connect_cb_name[] = "pipe_client_set_connect_cb"; + connect_cb = (pipe_client_set_connect_cb_t) dlsym(handle, connect_cb_name); + + if (!connect_cb) { + PX4_ERR("Error finding symbol %s: %s", connect_cb_name, dlerror()); + return -1; + + } else { + PX4_DEBUG("Successfully loaded function %s", connect_cb_name); + } + + connect_cb(0, ConnectCB, NULL); + + char disconnect_cb_name[] = "pipe_client_set_disconnect_cb"; + disconnect_cb = (pipe_client_set_disconnect_cb_t) dlsym(handle, disconnect_cb_name); + + if (!disconnect_cb) { + PX4_ERR("Error finding symbol %s: %s", disconnect_cb_name, dlerror()); + return -1; + + } else { + PX4_DEBUG("Successfully loaded function %s", disconnect_cb_name); + } + + disconnect_cb(0, DisconnectCB, NULL); + + // request a new pipe from the server + char open_pipe_name[] = "pipe_client_open"; + open_pipe = (pipe_client_open_t) dlsym(handle, open_pipe_name); + + if (!open_pipe) { + PX4_ERR("Error finding symbol %s: %s", open_pipe_name, dlerror()); + return -1; + + } else { + PX4_DEBUG("Successfully loaded function %s", open_pipe_name); + } + + // Create a new server pipe + char create_pipe_name[] = "pipe_server_create"; + create_pipe = (pipe_server_create_t) dlsym(handle, create_pipe_name); + + if (!create_pipe) { + PX4_ERR("Error finding symbol %s: %s", create_pipe_name, dlerror()); + return -1; + + } else { + PX4_DEBUG("Successfully loaded function %s", create_pipe_name); + } + + // Write to a server pipe + char write_pipe_name[] = "pipe_server_write"; + write_pipe = (pipe_server_write_t) dlsym(handle, write_pipe_name); + + if (!write_pipe) { + PX4_ERR("Error finding symbol %s: %s", write_pipe_name, dlerror()); + return -1; + + } else { + PX4_DEBUG("Successfully loaded function %s", write_pipe_name); + } + + // Set control callback for server pipe + char set_control_cb_name[] = "pipe_server_set_control_cb"; + set_control_cb = (pipe_server_set_control_cb_t) dlsym(handle, set_control_cb_name); + + if (!set_control_cb) { + PX4_ERR("Error finding symbol %s: %s", set_control_cb_name, dlerror()); + return -1; + + } else { + PX4_DEBUG("Successfully loaded function %s", set_control_cb_name); + } + + // Close server pipe + char close_pipe_name[] = "pipe_server_close"; + close_pipe = (pipe_server_close_t) dlsym(handle, close_pipe_name); + + if (!close_pipe) { + PX4_ERR("Error finding symbol %s: %s", close_pipe_name, dlerror()); + return -1; + + } else { + PX4_DEBUG("Successfully loaded function %s", close_pipe_name); + } + + initialized = true; + + return 0; +} diff --git a/boards/modalai/voxl2/src/lib/mpa/mpa.hpp b/boards/modalai/voxl2/src/lib/mpa/mpa.hpp new file mode 100644 index 0000000000..cd3640ba55 --- /dev/null +++ b/boards/modalai/voxl2/src/lib/mpa/mpa.hpp @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * Copyright (c) 2025-2026 ModalAI, inc. 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 + +#pragma once + +class MPA +{ +public: + static int Initialize(); + + typedef void (*mpa_data_cb_t)(char *data, int bytes); + typedef void (*mpa_control_cb_t)(int ch, char *data, int bytes, void *context); + + static int PipeClient(const char *pipe_name, int size, mpa_data_cb_t cb); + + static int PipeCreate(char *pipe_name, int flags = 0); + static int PipeWrite(int ch, const void *data, int bytes); + static int PipeServerSetControlCb(int ch, mpa_control_cb_t cb, void *context); + static void PipeServerClose(int ch); + +private: + static void HelperCB(__attribute__((unused)) int ch, char *data, int bytes, __attribute__((unused)) void *context); + static void DisconnectCB(__attribute__((unused)) int ch, __attribute__((unused)) void *context); + static void ConnectCB(__attribute__((unused)) int ch, __attribute__((unused)) void *context); + + typedef int (*pipe_client_set_simple_helper_cb_t)(int ch, client_simple_cb *cb, void *context); + typedef int (*pipe_client_set_connect_cb_t)(int ch, client_connect_cb *cb, void *context); + typedef int (*pipe_client_set_disconnect_cb_t)(int ch, client_disc_cb *cb, void *context); + typedef int (*pipe_client_open_t)(int ch, const char *name_or_location, const char *client_name, int flags, int buf_len); + typedef int (*pipe_server_create_t)(int ch, pipe_info_t info, int flags); + typedef int (*pipe_server_write_t)(int ch, const void *data, int bytes); + typedef int (*pipe_server_set_control_cb_t)(int ch, server_control_cb *cb, void *context); + typedef void (*pipe_server_close_t)(int ch); + + static pipe_client_set_simple_helper_cb_t helper_cb; + static pipe_client_set_connect_cb_t connect_cb; + static pipe_client_set_disconnect_cb_t disconnect_cb; + static pipe_client_open_t open_pipe; + static pipe_server_create_t create_pipe; + static pipe_server_write_t write_pipe; + static pipe_server_set_control_cb_t set_control_cb; + static pipe_server_close_t close_pipe; + + static bool initialized; + static void *handle; + + static int current_client; + static int current_server; + + static const int MAX_MPA_CLIENTS{8}; + static mpa_data_cb_t data_cb[MAX_MPA_CLIENTS]; +}; diff --git a/boards/modalai/voxl2/src/modules/sensor_baro_bridge/CMakeLists.txt b/boards/modalai/voxl2/src/modules/sensor_baro_bridge/CMakeLists.txt new file mode 100644 index 0000000000..1377ce00cf --- /dev/null +++ b/boards/modalai/voxl2/src/modules/sensor_baro_bridge/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2025-2026 ModalAI, Inc. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_module( + MODULE modules__sensor_baro_bridge + MAIN sensor_baro_bridge + INCLUDES + ${CMAKE_CURRENT_SOURCE_DIR}/../../lib/mpa + SRCS + sensor_baro_bridge.cpp + DEPENDS + mpa + ) diff --git a/boards/modalai/voxl2/src/modules/sensor_baro_bridge/sensor_baro_bridge.cpp b/boards/modalai/voxl2/src/modules/sensor_baro_bridge/sensor_baro_bridge.cpp new file mode 100644 index 0000000000..51ba8cf968 --- /dev/null +++ b/boards/modalai/voxl2/src/modules/sensor_baro_bridge/sensor_baro_bridge.cpp @@ -0,0 +1,183 @@ +/**************************************************************************** + * + * Copyright (c) 2025-2026 ModalAI, inc. 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 "mpa.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class SensorBaroBridge : public ModuleBase, public px4::WorkItem +{ +public: + static Descriptor desc; + + SensorBaroBridge(); + ~SensorBaroBridge() override = default; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + +private: + void Run() override; + + uORB::SubscriptionCallbackWorkItem _sensor_baro_sub{this, ORB_ID(sensor_baro)}; + + sensor_baro_s _sensor_baro{}; + + int baro_pipe_ch{0}; + +}; + +ModuleBase::Descriptor SensorBaroBridge::desc{task_spawn, custom_command, print_usage}; + +SensorBaroBridge::SensorBaroBridge() : + WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) +{ +} + +bool SensorBaroBridge::init() +{ + if (MPA::Initialize() == -1) { + PX4_ERR("MPA init failed"); + return false; + } + + char baro_pipe_name[] = "px4_sensor_baro"; + baro_pipe_ch = MPA::PipeCreate(baro_pipe_name); + + if (baro_pipe_ch == -1) { + PX4_ERR("Pipe create failed for %s", baro_pipe_name); + return false; + } + + if (!_sensor_baro_sub.registerCallback()) { + PX4_ERR("callback registration failed"); + return false; + } + + return true; +} + +void SensorBaroBridge::Run() +{ + if (should_exit()) { + _sensor_baro_sub.unregisterCallback(); + exit_and_cleanup(desc); + return; + } + + if (_sensor_baro_sub.updated()) { + if (_sensor_baro_sub.update(&_sensor_baro)) { + baro_data_t baro; + memset(&baro, 0, sizeof(baro)); + + baro.magic_number = BARO_MAGIC_NUMBER; + baro.pressure_pa = _sensor_baro.pressure; + baro.temp_c = _sensor_baro.temperature; + baro.alt_amsl_m = 0.0f; // sensor_baro does not include altitude + baro.timestamp_ns = _sensor_baro.timestamp * 1000; // Convert µs to ns + baro.reserved_1 = 0; + baro.reserved_2 = 0; + + if (MPA::PipeWrite(baro_pipe_ch, (void *)&baro, sizeof(baro_data_t)) == -1) { + PX4_ERR("Pipe %d write failed!", baro_pipe_ch); + } + } + } +} + +int SensorBaroBridge::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int SensorBaroBridge::task_spawn(int argc, char *argv[]) +{ + SensorBaroBridge *instance = new SensorBaroBridge(); + + if (instance) { + desc.object.store(instance); + desc.task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + desc.object.store(nullptr); + desc.task_id = -1; + + return PX4_ERROR; +} + +int SensorBaroBridge::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Sensor baro bridge + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("sensor_baro_bridge", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int sensor_baro_bridge_main(int argc, char *argv[]) +{ + return ModuleBase::main(SensorBaroBridge::desc, argc, argv); +} diff --git a/boards/modalai/voxl2/src/modules/vehicle_air_data_bridge/CMakeLists.txt b/boards/modalai/voxl2/src/modules/vehicle_air_data_bridge/CMakeLists.txt new file mode 100644 index 0000000000..a6dc08bc08 --- /dev/null +++ b/boards/modalai/voxl2/src/modules/vehicle_air_data_bridge/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2025-2026 ModalAI, Inc. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_module( + MODULE modules__vehicle_air_data_bridge + MAIN vehicle_air_data_bridge + INCLUDES + ${CMAKE_CURRENT_SOURCE_DIR}/../../lib/mpa + SRCS + vehicle_air_data_bridge.cpp + DEPENDS + mpa + ) diff --git a/boards/modalai/voxl2/src/modules/vehicle_air_data_bridge/vehicle_air_data_bridge.cpp b/boards/modalai/voxl2/src/modules/vehicle_air_data_bridge/vehicle_air_data_bridge.cpp new file mode 100644 index 0000000000..7943db6e5b --- /dev/null +++ b/boards/modalai/voxl2/src/modules/vehicle_air_data_bridge/vehicle_air_data_bridge.cpp @@ -0,0 +1,183 @@ +/**************************************************************************** + * + * Copyright (c) 2025-2026 ModalAI, inc. 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 "mpa.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class VehicleAirDataBridge : public ModuleBase, public px4::WorkItem +{ +public: + static Descriptor desc; + + VehicleAirDataBridge(); + ~VehicleAirDataBridge() override = default; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + +private: + void Run() override; + + uORB::SubscriptionCallbackWorkItem _vehicle_air_data_sub{this, ORB_ID(vehicle_air_data)}; + + vehicle_air_data_s _vehicle_air_data{}; + + int baro_pipe_ch{0}; + +}; + +ModuleBase::Descriptor VehicleAirDataBridge::desc{task_spawn, custom_command, print_usage}; + +VehicleAirDataBridge::VehicleAirDataBridge() : + WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) +{ +} + +bool VehicleAirDataBridge::init() +{ + if (MPA::Initialize() == -1) { + PX4_ERR("MPA init failed"); + return false; + } + + char baro_pipe_name[] = "px4_vehicle_air_data"; + baro_pipe_ch = MPA::PipeCreate(baro_pipe_name); + + if (baro_pipe_ch == -1) { + PX4_ERR("Pipe create failed for %s", baro_pipe_name); + return false; + } + + if (!_vehicle_air_data_sub.registerCallback()) { + PX4_ERR("callback registration failed"); + return false; + } + + return true; +} + +void VehicleAirDataBridge::Run() +{ + if (should_exit()) { + _vehicle_air_data_sub.unregisterCallback(); + exit_and_cleanup(desc); + return; + } + + if (_vehicle_air_data_sub.updated()) { + if (_vehicle_air_data_sub.update(&_vehicle_air_data)) { + baro_data_t baro; + memset(&baro, 0, sizeof(baro)); + + baro.magic_number = BARO_MAGIC_NUMBER; + baro.pressure_pa = _vehicle_air_data.baro_pressure_pa; + baro.temp_c = _vehicle_air_data.ambient_temperature; + baro.alt_amsl_m = _vehicle_air_data.baro_alt_meter; + baro.timestamp_ns = _vehicle_air_data.timestamp * 1000; // Convert µs to ns + baro.reserved_1 = 0; + baro.reserved_2 = 0; + + if (MPA::PipeWrite(baro_pipe_ch, (void *)&baro, sizeof(baro_data_t)) == -1) { + PX4_ERR("Pipe %d write failed!", baro_pipe_ch); + } + } + } +} + +int VehicleAirDataBridge::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int VehicleAirDataBridge::task_spawn(int argc, char *argv[]) +{ + VehicleAirDataBridge *instance = new VehicleAirDataBridge(); + + if (instance) { + desc.object.store(instance); + desc.task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + desc.object.store(nullptr); + desc.task_id = -1; + + return PX4_ERROR; +} + +int VehicleAirDataBridge::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Vehicle air data bridge + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("vehicle_air_data_bridge", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int vehicle_air_data_bridge_main(int argc, char *argv[]) +{ + return ModuleBase::main(VehicleAirDataBridge::desc, argc, argv); +} diff --git a/boards/modalai/voxl2/src/modules/vehicle_local_position_bridge/CMakeLists.txt b/boards/modalai/voxl2/src/modules/vehicle_local_position_bridge/CMakeLists.txt new file mode 100644 index 0000000000..378cc56d82 --- /dev/null +++ b/boards/modalai/voxl2/src/modules/vehicle_local_position_bridge/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2025-2026 ModalAI, Inc. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_module( + MODULE modules__vehicle_local_position_bridge + MAIN vehicle_local_position_bridge + INCLUDES + ${CMAKE_CURRENT_SOURCE_DIR}/../../lib/mpa + SRCS + vehicle_local_position_bridge.cpp + DEPENDS + mpa + ) diff --git a/boards/modalai/voxl2/src/modules/vehicle_local_position_bridge/vehicle_local_position_bridge.cpp b/boards/modalai/voxl2/src/modules/vehicle_local_position_bridge/vehicle_local_position_bridge.cpp new file mode 100644 index 0000000000..d39f55dee1 --- /dev/null +++ b/boards/modalai/voxl2/src/modules/vehicle_local_position_bridge/vehicle_local_position_bridge.cpp @@ -0,0 +1,238 @@ +/**************************************************************************** + * + * Copyright (c) 2025-2026 ModalAI, inc. 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 "mpa.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class VehicleLocalPositionBridge : public ModuleBase, public px4::WorkItem +{ +public: + static Descriptor desc; + + VehicleLocalPositionBridge(); + ~VehicleLocalPositionBridge() override = default; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + +private: + void Run() override; + + uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)}; + + vehicle_local_position_s _vehicle_local_position{}; + + int _pipe_ch{0}; + +}; + +ModuleBase::Descriptor VehicleLocalPositionBridge::desc{task_spawn, custom_command, print_usage}; + +VehicleLocalPositionBridge::VehicleLocalPositionBridge() : + WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) +{ +} + +bool VehicleLocalPositionBridge::init() +{ + if (MPA::Initialize() == -1) { + PX4_ERR("MPA init failed"); + return false; + } + + char pipe_name[] = "px4_vehicle_local_position"; + _pipe_ch = MPA::PipeCreate(pipe_name); + + if (_pipe_ch == -1) { + PX4_ERR("Pipe create failed for %s", pipe_name); + return false; + } + + if (!_vehicle_local_position_sub.registerCallback()) { + PX4_ERR("callback registration failed"); + return false; + } + + return true; +} + +void VehicleLocalPositionBridge::Run() +{ + if (should_exit()) { + _vehicle_local_position_sub.unregisterCallback(); + exit_and_cleanup(desc); + return; + } + + if (_vehicle_local_position_sub.updated()) { + if (_vehicle_local_position_sub.update(&_vehicle_local_position)) { + // Only publish if we have valid position data + if (!_vehicle_local_position.xy_valid && !_vehicle_local_position.z_valid) { + return; + } + + pose_vel_6dof_t pose; + + pose.magic_number = POSE_VEL_6DOF_MAGIC_NUMBER; + pose.timestamp_ns = _vehicle_local_position.timestamp * 1000; // Convert µs to ns + + // Position (NED frame) + if (_vehicle_local_position.xy_valid) { + pose.T_child_wrt_parent[0] = _vehicle_local_position.x; + pose.T_child_wrt_parent[1] = _vehicle_local_position.y; + + } else { + pose.T_child_wrt_parent[0] = NAN; + pose.T_child_wrt_parent[1] = NAN; + } + + if (_vehicle_local_position.z_valid) { + pose.T_child_wrt_parent[2] = _vehicle_local_position.z; + + } else { + pose.T_child_wrt_parent[2] = NAN; + } + + // Rotation matrix from heading (yaw rotation around Z axis) + // R_z(heading) = [cos(h) -sin(h) 0] + // [sin(h) cos(h) 0] + // [0 0 1] + float cos_h = cosf(_vehicle_local_position.heading); + float sin_h = sinf(_vehicle_local_position.heading); + pose.R_child_to_parent[0][0] = cos_h; + pose.R_child_to_parent[0][1] = -sin_h; + pose.R_child_to_parent[0][2] = 0.0f; + pose.R_child_to_parent[1][0] = sin_h; + pose.R_child_to_parent[1][1] = cos_h; + pose.R_child_to_parent[1][2] = 0.0f; + pose.R_child_to_parent[2][0] = 0.0f; + pose.R_child_to_parent[2][1] = 0.0f; + pose.R_child_to_parent[2][2] = 1.0f; + + // Velocity (NED frame) + if (_vehicle_local_position.v_xy_valid) { + pose.v_child_wrt_parent[0] = _vehicle_local_position.vx; + pose.v_child_wrt_parent[1] = _vehicle_local_position.vy; + + } else { + pose.v_child_wrt_parent[0] = NAN; + pose.v_child_wrt_parent[1] = NAN; + } + + if (_vehicle_local_position.v_z_valid) { + pose.v_child_wrt_parent[2] = _vehicle_local_position.vz; + + } else { + pose.v_child_wrt_parent[2] = NAN; + } + + // Angular velocity not available in vehicle_local_position + pose.w_child_wrt_child[0] = NAN; + pose.w_child_wrt_child[1] = NAN; + pose.w_child_wrt_child[2] = NAN; + + if (MPA::PipeWrite(_pipe_ch, (void *)&pose, sizeof(pose_vel_6dof_t)) == -1) { + PX4_ERR("Pipe %d write failed!", _pipe_ch); + } + } + } +} + +int VehicleLocalPositionBridge::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int VehicleLocalPositionBridge::task_spawn(int argc, char *argv[]) +{ + VehicleLocalPositionBridge *instance = new VehicleLocalPositionBridge(); + + if (instance) { + desc.object.store(instance); + desc.task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + desc.object.store(nullptr); + desc.task_id = -1; + + return PX4_ERROR; +} + +int VehicleLocalPositionBridge::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Vehicle local position bridge - publishes vehicle_local_position to MPA pipe as pose_vel_6dof_t + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("vehicle_local_position_bridge", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int vehicle_local_position_bridge_main(int argc, char *argv[]) +{ + return ModuleBase::main(VehicleLocalPositionBridge::desc, argc, argv); +} diff --git a/boards/modalai/voxl2/src/spi.cpp b/boards/modalai/voxl2/src/spi.cpp new file mode 100644 index 0000000000..6e0efd2cf3 --- /dev/null +++ b/boards/modalai/voxl2/src/spi.cpp @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * Copyright (C) 2025-2026 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(1, {initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, 0), }), +}; diff --git a/boards/modalai/voxl2/target/voxl-px4 b/boards/modalai/voxl2/target/voxl-px4 index 514fc026bb..a065612118 100755 --- a/boards/modalai/voxl2/target/voxl-px4 +++ b/boards/modalai/voxl2/target/voxl-px4 @@ -2,10 +2,12 @@ CONFIG_FILE="/etc/modalai/voxl-px4.conf" +AIRFRAME=MULTICOPTER GPS=NONE RC=SPEKTRUM ESC=VOXL_ESC POWER_MANAGER=VOXLPM +AIRSPEED_SENSOR=NONE DISTANCE_SENSOR=NONE OSD=DISABLE DAEMON_MODE=DISABLE @@ -39,12 +41,14 @@ else fi print_usage() { - echo -e "\nUsage: voxl-px4 [-b (Specify Holybro GPS unit)]" + echo -e "\nUsage: voxl-px4 [-a (Specify Airspeed Sensor)]" + echo " [-b (Specify Holybro GPS unit)]" echo " [-c delete configuration file and exit]" echo " [-d start px4 without daemon mode]" echo " [-f (Use fake rc input instead of from a real transmitter)]" echo " [-m (Specify Matek GPS unit)]" echo " [-o (Start OSD module on the apps processor)]" + echo " [-p (Specify Fixed Wing airframe selected)]" echo " [-r (Specify TBS Crossfire RC receiver, MAVLINK)]" echo " [-w (Specify TBS Crossfire RC receiver, raw)]" echo " [-z (Use fake sensor calibration values)]" @@ -55,10 +59,12 @@ print_usage() { print_config_settings(){ echo -e "\n*************************" + echo "AIRFRAME=$AIRFRAME" echo "GPS=$GPS" echo "RC=$RC" echo "ESC=$ESC" echo "POWER MANAGER=$POWER_MANAGER" + echo "AIRSPEED SENSOR=$AIRSPEED_SENSOR" echo "DISTANCE SENSOR=$DISTANCE_SENSOR" echo "OSD=$OSD" echo "DAEMON_MODE=$DAEMON_MODE" @@ -71,9 +77,13 @@ print_config_settings(){ echo -e "*************************\n" } -while getopts "bcdhfmorwz" flag +while getopts "abcdhfmoprwz" flag do case "${flag}" in + a) + echo "[INFO] MRO AIRSPEED Sensor selected" + AIRSPEED_SENSOR=MS4525DO + ;; b) echo "[INFO] Holybro GPS selected" GPS=HOLYBRO @@ -104,6 +114,10 @@ do echo "[INFO] OSD module selected" OSD=ENABLE ;; + p) + echo "[INFO] Airframe Selected as Fixed Wing" + AIRFRAME=FIXED_WING + ;; r) echo "[INFO] TBS Crossfire RC receiver, MAVLINK selected" RC=CRSF_MAV @@ -137,5 +151,6 @@ fi print_config_settings -GPS=$GPS RC=$RC ESC=$ESC POWER_MANAGER=$POWER_MANAGER DISTANCE_SENSOR=$DISTANCE_SENSOR \ -OSD=$OSD EXTRA_STEPS=$EXTRA_STEPS px4 $DAEMON -s /usr/bin/voxl-px4-start +AIRFRAME=$AIRFRAME GPS=$GPS RC=$RC ESC=$ESC POWER_MANAGER=$POWER_MANAGER DISTANCE_SENSOR=$DISTANCE_SENSOR \ +AIRSPEED_SENSOR=$AIRSPEED_SENSOR OSD=$OSD EXTRA_STEPS=$EXTRA_STEPS \ +px4 $DAEMON -s /usr/bin/voxl-px4-start diff --git a/boards/modalai/voxl2/target/voxl-px4-start b/boards/modalai/voxl2/target/voxl-px4-start index 9d31466b56..b23c24247d 100755 --- a/boards/modalai/voxl2/target/voxl-px4-start +++ b/boards/modalai/voxl2/target/voxl-px4-start @@ -4,10 +4,12 @@ . px4-alias.sh echo -e "\n*************************" +echo "AIRFRAME: $AIRFRAME" echo "GPS: $GPS" echo "RC: $RC" echo "ESC: $ESC" echo "POWER MANAGER: $POWER_MANAGER" +echo "AIRSPEED SENSOR: $AIRSPEED_SENSOR" echo "DISTANCE SENSOR: $DISTANCE_SENSOR" echo "OSD: $OSD" echo "EXTRA STEPS:" @@ -23,6 +25,8 @@ echo -e "*************************\n" # and modules manually from the px4 command shell if [ ! -z $MINIMAL_PX4 ]; then /bin/echo "Running minimal script" + param select /data/px4/param/parameters + param load exit 0 fi @@ -33,16 +37,21 @@ if [ $RETURNCODE -ne 0 ]; then # If we couldn't get the platform from the voxl-platform utility then check # /etc/version to see if there is an M0052 substring in the version string. If so, # then we assume that we are on M0052. - VERSIONSTRING=$(