mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
voxl2: Added components to the board build that are in the ModalAI fork but missing in mainline
This commit is contained in:
parent
6e418096b7
commit
bd76832f34
6
.gitmodules
vendored
6
.gitmodules
vendored
@ -109,3 +109,9 @@
|
|||||||
[submodule "src/lib/rl_tools/rl_tools"]
|
[submodule "src/lib/rl_tools/rl_tools"]
|
||||||
path = src/lib/rl_tools/rl_tools
|
path = src/lib/rl_tools/rl_tools
|
||||||
url = https://github.com/rl-tools/rl-tools.git
|
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
|
||||||
|
|||||||
@ -39,6 +39,8 @@ exec find boards msg src platforms test \
|
|||||||
-path src/lib/cdrstream/rosidl -prune -o \
|
-path src/lib/cdrstream/rosidl -prune -o \
|
||||||
-path src/modules/zenoh/zenoh-pico -prune -o \
|
-path src/modules/zenoh/zenoh-pico -prune -o \
|
||||||
-path boards/modalai/voxl2/libfc-sensor-api -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/drivers/actuators/vertiq_io/iq-module-communication-cpp -prune -o \
|
||||||
-path src/lib/tensorflow_lite_micro/tflite_micro -prune -o \
|
-path src/lib/tensorflow_lite_micro/tflite_micro -prune -o \
|
||||||
-path src/drivers/ins/sbgecom/sbgECom -prune -o \
|
-path src/drivers/ins/sbgecom/sbgECom -prune -o \
|
||||||
|
|||||||
@ -5,6 +5,7 @@ CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP101XX=y
|
|||||||
CONFIG_DRIVERS_BAROMETER_MS5611=y
|
CONFIG_DRIVERS_BAROMETER_MS5611=y
|
||||||
CONFIG_DRIVERS_BAROMETER_BMP280=y
|
CONFIG_DRIVERS_BAROMETER_BMP280=y
|
||||||
CONFIG_DRIVERS_BAROMETER_BMP388=y
|
CONFIG_DRIVERS_BAROMETER_BMP388=y
|
||||||
|
CONFIG_DRIVERS_BAROMETER_DPS310=y
|
||||||
CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_MS4525DO=y
|
CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_MS4525DO=y
|
||||||
CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L0X=y
|
CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L0X=y
|
||||||
CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L1X=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_IST8308=y
|
||||||
CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
|
CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
|
||||||
CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y
|
CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y
|
||||||
|
CONFIG_DRIVERS_MAGNETOMETER_ST_IIS2MDC=y
|
||||||
CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
|
CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
|
||||||
CONFIG_DRIVERS_QSHELL_QURT=y
|
CONFIG_DRIVERS_QSHELL_QURT=y
|
||||||
CONFIG_DRIVERS_RC_CRSF_RC=y
|
CONFIG_DRIVERS_RC_CRSF_RC=y
|
||||||
@ -29,6 +31,11 @@ CONFIG_MODULES_LOAD_MON=y
|
|||||||
CONFIG_MODULES_MANUAL_CONTROL=y
|
CONFIG_MODULES_MANUAL_CONTROL=y
|
||||||
CONFIG_MODULES_MC_ATT_CONTROL=y
|
CONFIG_MODULES_MC_ATT_CONTROL=y
|
||||||
CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_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_HOVER_THRUST_ESTIMATOR=y
|
||||||
CONFIG_MODULES_MC_POS_CONTROL=y
|
CONFIG_MODULES_MC_POS_CONTROL=y
|
||||||
CONFIG_MODULES_MC_RATE_CONTROL=y
|
CONFIG_MODULES_MC_RATE_CONTROL=y
|
||||||
|
|||||||
@ -3,6 +3,8 @@ CONFIG_BOARD_LINUX_TARGET=y
|
|||||||
CONFIG_BOARD_TOOLCHAIN="aarch64-linux-gnu"
|
CONFIG_BOARD_TOOLCHAIN="aarch64-linux-gnu"
|
||||||
CONFIG_BOARD_ROOT_PATH="/data/px4"
|
CONFIG_BOARD_ROOT_PATH="/data/px4"
|
||||||
CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y
|
CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y
|
||||||
|
CONFIG_DRIVERS_BAROMETER_DPS310=y
|
||||||
|
CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP101XX=y
|
||||||
CONFIG_DRIVERS_GPS=y
|
CONFIG_DRIVERS_GPS=y
|
||||||
CONFIG_DRIVERS_OSD_MSP_OSD=y
|
CONFIG_DRIVERS_OSD_MSP_OSD=y
|
||||||
CONFIG_DRIVERS_QSHELL_POSIX=y
|
CONFIG_DRIVERS_QSHELL_POSIX=y
|
||||||
|
|||||||
@ -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-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-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-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
|
# Make sure any required directories exist
|
||||||
adb shell "/bin/mkdir -p /data/px4/param"
|
adb shell "/bin/mkdir -p /data/px4/param"
|
||||||
|
|||||||
@ -39,12 +39,20 @@ set(DISABLE_PARAMS_MODULE_SCOPING TRUE PARENT_SCOPE)
|
|||||||
|
|
||||||
add_library(drivers_board
|
add_library(drivers_board
|
||||||
board_config.h
|
board_config.h
|
||||||
|
i2c.cpp
|
||||||
init.c
|
init.c
|
||||||
boardctl.c
|
boardctl.c
|
||||||
|
spi.cpp
|
||||||
)
|
)
|
||||||
|
|
||||||
# Add custom drivers
|
# Add custom drivers
|
||||||
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/apps_sbus)
|
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/apps_sbus)
|
||||||
|
|
||||||
|
# Add custom libraries
|
||||||
|
add_subdirectory(${PX4_BOARD_DIR}/src/lib/mpa)
|
||||||
|
|
||||||
# Add custom modules
|
# Add custom modules
|
||||||
add_subdirectory(${PX4_BOARD_DIR}/src/modules/voxl_save_cal_params)
|
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)
|
||||||
|
|||||||
@ -42,9 +42,21 @@
|
|||||||
#define CONFIG_BOARDCTL_RESET
|
#define CONFIG_BOARDCTL_RESET
|
||||||
#define BOARD_HAS_NO_BOOTLOADER
|
#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
|
#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 <system_config.h>
|
#include <system_config.h>
|
||||||
#include <px4_platform_common/board_common.h>
|
#include <px4_platform_common/board_common.h>
|
||||||
|
|
||||||
|
|||||||
40
boards/modalai/voxl2/src/i2c.cpp
Normal file
40
boards/modalai/voxl2/src/i2c.cpp
Normal file
@ -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 <px4_platform_common/i2c.h>
|
||||||
|
#include <px4_arch/i2c_hw_description.h>
|
||||||
|
#include <drivers/drv_sensor.h>
|
||||||
|
|
||||||
|
constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = {
|
||||||
|
initI2CBusExternal(0)
|
||||||
|
};
|
||||||
44
boards/modalai/voxl2/src/lib/mpa/CMakeLists.txt
Normal file
44
boards/modalai/voxl2/src/lib/mpa/CMakeLists.txt
Normal file
@ -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
|
||||||
|
)
|
||||||
1
boards/modalai/voxl2/src/lib/mpa/libmodal-json
Submodule
1
boards/modalai/voxl2/src/lib/mpa/libmodal-json
Submodule
@ -0,0 +1 @@
|
|||||||
|
Subproject commit a18d9eee62465b8eef52251515f7195874afa260
|
||||||
1
boards/modalai/voxl2/src/lib/mpa/libmodal-pipe
Submodule
1
boards/modalai/voxl2/src/lib/mpa/libmodal-pipe
Submodule
@ -0,0 +1 @@
|
|||||||
|
Subproject commit be51027375ca4071a2c2ba7410fb2ddd4bbead0c
|
||||||
262
boards/modalai/voxl2/src/lib/mpa/mpa.cpp
Normal file
262
boards/modalai/voxl2/src/lib/mpa/mpa.cpp
Normal file
@ -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 <dlfcn.h>
|
||||||
|
#include <px4_log.h>
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
|
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;
|
||||||
|
}
|
||||||
83
boards/modalai/voxl2/src/lib/mpa/mpa.hpp
Normal file
83
boards/modalai/voxl2/src/lib/mpa/mpa.hpp
Normal file
@ -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 <modal_pipe.h>
|
||||||
|
|
||||||
|
#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];
|
||||||
|
};
|
||||||
@ -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
|
||||||
|
)
|
||||||
@ -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 <sys/types.h>
|
||||||
|
#include <sys/stat.h>
|
||||||
|
#include <time.h>
|
||||||
|
#include <px4_log.h>
|
||||||
|
#include <px4_platform_common/defines.h>
|
||||||
|
#include <px4_platform_common/module.h>
|
||||||
|
#include <uORB/uORB.h>
|
||||||
|
#include <uORB/SubscriptionCallback.hpp>
|
||||||
|
#include <uORB/topics/sensor_baro.h>
|
||||||
|
|
||||||
|
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);
|
||||||
|
}
|
||||||
@ -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
|
||||||
|
)
|
||||||
@ -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 <sys/types.h>
|
||||||
|
#include <sys/stat.h>
|
||||||
|
#include <time.h>
|
||||||
|
#include <px4_log.h>
|
||||||
|
#include <px4_platform_common/defines.h>
|
||||||
|
#include <px4_platform_common/module.h>
|
||||||
|
#include <uORB/uORB.h>
|
||||||
|
#include <uORB/SubscriptionCallback.hpp>
|
||||||
|
#include <uORB/topics/vehicle_air_data.h>
|
||||||
|
|
||||||
|
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);
|
||||||
|
}
|
||||||
@ -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
|
||||||
|
)
|
||||||
@ -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 <sys/types.h>
|
||||||
|
#include <sys/stat.h>
|
||||||
|
#include <time.h>
|
||||||
|
#include <math.h>
|
||||||
|
#include <px4_log.h>
|
||||||
|
#include <px4_platform_common/defines.h>
|
||||||
|
#include <px4_platform_common/module.h>
|
||||||
|
#include <uORB/uORB.h>
|
||||||
|
#include <uORB/SubscriptionCallback.hpp>
|
||||||
|
#include <uORB/topics/vehicle_local_position.h>
|
||||||
|
|
||||||
|
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);
|
||||||
|
}
|
||||||
40
boards/modalai/voxl2/src/spi.cpp
Normal file
40
boards/modalai/voxl2/src/spi.cpp
Normal file
@ -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 <px4_arch/spi_hw_description.h>
|
||||||
|
#include <px4_platform_common/spi.h>
|
||||||
|
#include <drivers/drv_sensor.h>
|
||||||
|
|
||||||
|
constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
|
||||||
|
initSPIBus(1, {initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, 0), }),
|
||||||
|
};
|
||||||
@ -2,10 +2,12 @@
|
|||||||
|
|
||||||
CONFIG_FILE="/etc/modalai/voxl-px4.conf"
|
CONFIG_FILE="/etc/modalai/voxl-px4.conf"
|
||||||
|
|
||||||
|
AIRFRAME=MULTICOPTER
|
||||||
GPS=NONE
|
GPS=NONE
|
||||||
RC=SPEKTRUM
|
RC=SPEKTRUM
|
||||||
ESC=VOXL_ESC
|
ESC=VOXL_ESC
|
||||||
POWER_MANAGER=VOXLPM
|
POWER_MANAGER=VOXLPM
|
||||||
|
AIRSPEED_SENSOR=NONE
|
||||||
DISTANCE_SENSOR=NONE
|
DISTANCE_SENSOR=NONE
|
||||||
OSD=DISABLE
|
OSD=DISABLE
|
||||||
DAEMON_MODE=DISABLE
|
DAEMON_MODE=DISABLE
|
||||||
@ -39,12 +41,14 @@ else
|
|||||||
fi
|
fi
|
||||||
|
|
||||||
print_usage() {
|
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 " [-c delete configuration file and exit]"
|
||||||
echo " [-d start px4 without daemon mode]"
|
echo " [-d start px4 without daemon mode]"
|
||||||
echo " [-f (Use fake rc input instead of from a real transmitter)]"
|
echo " [-f (Use fake rc input instead of from a real transmitter)]"
|
||||||
echo " [-m (Specify Matek GPS unit)]"
|
echo " [-m (Specify Matek GPS unit)]"
|
||||||
echo " [-o (Start OSD module on the apps processor)]"
|
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 " [-r (Specify TBS Crossfire RC receiver, MAVLINK)]"
|
||||||
echo " [-w (Specify TBS Crossfire RC receiver, raw)]"
|
echo " [-w (Specify TBS Crossfire RC receiver, raw)]"
|
||||||
echo " [-z (Use fake sensor calibration values)]"
|
echo " [-z (Use fake sensor calibration values)]"
|
||||||
@ -55,10 +59,12 @@ print_usage() {
|
|||||||
|
|
||||||
print_config_settings(){
|
print_config_settings(){
|
||||||
echo -e "\n*************************"
|
echo -e "\n*************************"
|
||||||
|
echo "AIRFRAME=$AIRFRAME"
|
||||||
echo "GPS=$GPS"
|
echo "GPS=$GPS"
|
||||||
echo "RC=$RC"
|
echo "RC=$RC"
|
||||||
echo "ESC=$ESC"
|
echo "ESC=$ESC"
|
||||||
echo "POWER MANAGER=$POWER_MANAGER"
|
echo "POWER MANAGER=$POWER_MANAGER"
|
||||||
|
echo "AIRSPEED SENSOR=$AIRSPEED_SENSOR"
|
||||||
echo "DISTANCE SENSOR=$DISTANCE_SENSOR"
|
echo "DISTANCE SENSOR=$DISTANCE_SENSOR"
|
||||||
echo "OSD=$OSD"
|
echo "OSD=$OSD"
|
||||||
echo "DAEMON_MODE=$DAEMON_MODE"
|
echo "DAEMON_MODE=$DAEMON_MODE"
|
||||||
@ -71,9 +77,13 @@ print_config_settings(){
|
|||||||
echo -e "*************************\n"
|
echo -e "*************************\n"
|
||||||
}
|
}
|
||||||
|
|
||||||
while getopts "bcdhfmorwz" flag
|
while getopts "abcdhfmoprwz" flag
|
||||||
do
|
do
|
||||||
case "${flag}" in
|
case "${flag}" in
|
||||||
|
a)
|
||||||
|
echo "[INFO] MRO AIRSPEED Sensor selected"
|
||||||
|
AIRSPEED_SENSOR=MS4525DO
|
||||||
|
;;
|
||||||
b)
|
b)
|
||||||
echo "[INFO] Holybro GPS selected"
|
echo "[INFO] Holybro GPS selected"
|
||||||
GPS=HOLYBRO
|
GPS=HOLYBRO
|
||||||
@ -104,6 +114,10 @@ do
|
|||||||
echo "[INFO] OSD module selected"
|
echo "[INFO] OSD module selected"
|
||||||
OSD=ENABLE
|
OSD=ENABLE
|
||||||
;;
|
;;
|
||||||
|
p)
|
||||||
|
echo "[INFO] Airframe Selected as Fixed Wing"
|
||||||
|
AIRFRAME=FIXED_WING
|
||||||
|
;;
|
||||||
r)
|
r)
|
||||||
echo "[INFO] TBS Crossfire RC receiver, MAVLINK selected"
|
echo "[INFO] TBS Crossfire RC receiver, MAVLINK selected"
|
||||||
RC=CRSF_MAV
|
RC=CRSF_MAV
|
||||||
@ -137,5 +151,6 @@ fi
|
|||||||
|
|
||||||
print_config_settings
|
print_config_settings
|
||||||
|
|
||||||
GPS=$GPS RC=$RC ESC=$ESC POWER_MANAGER=$POWER_MANAGER DISTANCE_SENSOR=$DISTANCE_SENSOR \
|
AIRFRAME=$AIRFRAME 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
|
AIRSPEED_SENSOR=$AIRSPEED_SENSOR OSD=$OSD EXTRA_STEPS=$EXTRA_STEPS \
|
||||||
|
px4 $DAEMON -s /usr/bin/voxl-px4-start
|
||||||
|
|||||||
@ -4,10 +4,12 @@
|
|||||||
. px4-alias.sh
|
. px4-alias.sh
|
||||||
|
|
||||||
echo -e "\n*************************"
|
echo -e "\n*************************"
|
||||||
|
echo "AIRFRAME: $AIRFRAME"
|
||||||
echo "GPS: $GPS"
|
echo "GPS: $GPS"
|
||||||
echo "RC: $RC"
|
echo "RC: $RC"
|
||||||
echo "ESC: $ESC"
|
echo "ESC: $ESC"
|
||||||
echo "POWER MANAGER: $POWER_MANAGER"
|
echo "POWER MANAGER: $POWER_MANAGER"
|
||||||
|
echo "AIRSPEED SENSOR: $AIRSPEED_SENSOR"
|
||||||
echo "DISTANCE SENSOR: $DISTANCE_SENSOR"
|
echo "DISTANCE SENSOR: $DISTANCE_SENSOR"
|
||||||
echo "OSD: $OSD"
|
echo "OSD: $OSD"
|
||||||
echo "EXTRA STEPS:"
|
echo "EXTRA STEPS:"
|
||||||
@ -23,6 +25,8 @@ echo -e "*************************\n"
|
|||||||
# and modules manually from the px4 command shell
|
# and modules manually from the px4 command shell
|
||||||
if [ ! -z $MINIMAL_PX4 ]; then
|
if [ ! -z $MINIMAL_PX4 ]; then
|
||||||
/bin/echo "Running minimal script"
|
/bin/echo "Running minimal script"
|
||||||
|
param select /data/px4/param/parameters
|
||||||
|
param load
|
||||||
exit 0
|
exit 0
|
||||||
fi
|
fi
|
||||||
|
|
||||||
@ -33,16 +37,21 @@ if [ $RETURNCODE -ne 0 ]; then
|
|||||||
# If we couldn't get the platform from the voxl-platform utility then check
|
# 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,
|
# /etc/version to see if there is an M0052 substring in the version string. If so,
|
||||||
# then we assume that we are on M0052.
|
# then we assume that we are on M0052.
|
||||||
VERSIONSTRING=$(</etc/version)
|
VERSIONSTRING=""
|
||||||
|
if [ -f /etc/version ]; then
|
||||||
|
VERSIONSTRING=$(</etc/version)
|
||||||
|
fi
|
||||||
M0052SUBSTRING="M0052"
|
M0052SUBSTRING="M0052"
|
||||||
if [[ "$VERSIONSTRING" == *"$M0052SUBSTRING"* ]]; then
|
if [[ "$VERSIONSTRING" == *"$M0052SUBSTRING"* ]]; then
|
||||||
PLATFORM="M0052"
|
PLATFORM="M0052"
|
||||||
fi
|
fi
|
||||||
fi
|
fi
|
||||||
|
|
||||||
# We can only run on M0052, M0054, M0104, or M0197 so exit with error if that is not the case
|
# We can only run on M0054, M0104, or M0197 so exit with error if that is not the case
|
||||||
if [ $PLATFORM = "M0052" ]; then
|
if [ $PLATFORM = "M0052" ]; then
|
||||||
/bin/echo "Running on M0052"
|
/bin/echo "Running on M0052"
|
||||||
|
/bin/echo "Error M0052 is no longer supported!!!"
|
||||||
|
exit 1
|
||||||
elif [ $PLATFORM = "M0054" ]; then
|
elif [ $PLATFORM = "M0054" ]; then
|
||||||
/bin/echo "Running on M0054"
|
/bin/echo "Running on M0054"
|
||||||
elif [ $PLATFORM = "M0104" ]; then
|
elif [ $PLATFORM = "M0104" ]; then
|
||||||
@ -51,7 +60,7 @@ elif [ $PLATFORM = "M0197" ]; then
|
|||||||
/bin/echo "Running on M0197"
|
/bin/echo "Running on M0197"
|
||||||
else
|
else
|
||||||
/bin/echo "Error, cannot determine platform!"
|
/bin/echo "Error, cannot determine platform!"
|
||||||
exit 0
|
exit 1
|
||||||
fi
|
fi
|
||||||
|
|
||||||
# Sleep a little here. A lot happens when the uorb and muorb start
|
# Sleep a little here. A lot happens when the uorb and muorb start
|
||||||
@ -87,13 +96,33 @@ else
|
|||||||
qshell icm42688p start -s -C 32768
|
qshell icm42688p start -s -C 32768
|
||||||
fi
|
fi
|
||||||
|
|
||||||
# Start onboard barometer
|
# First look for any external barometers connected to the apps proc
|
||||||
if [ "$PLATFORM" == "M0197" ]; then
|
EXTERNAL_BAROMETER=0
|
||||||
/bin/echo "Starting dps368 barometer on M0197"
|
|
||||||
qshell dps310 start -I -b 5
|
/bin/echo "Looking for external DPS368 barometer"
|
||||||
else
|
if dps310 start -X -b /dev/i2c-0; then
|
||||||
# Start Invensense ICP 101xx barometer built on to VOXL 2
|
/bin/echo "Detected external DPS368 barometer"
|
||||||
qshell icp101xx start -I -b 5
|
EXTERNAL_BAROMETER=1
|
||||||
|
fi
|
||||||
|
|
||||||
|
if (( EXTERNAL_BAROMETER == 0 )); then
|
||||||
|
/bin/echo "Looking for external ICP10100 barometer"
|
||||||
|
if icp101xx start -X -b /dev/i2c-0; then
|
||||||
|
/bin/echo "Detected external ICP10100 barometer"
|
||||||
|
EXTERNAL_BAROMETER=1
|
||||||
|
fi
|
||||||
|
fi
|
||||||
|
|
||||||
|
# If no external barometers found, use the builtin barometer
|
||||||
|
if (( EXTERNAL_BAROMETER == 0 )); then
|
||||||
|
/bin/echo "Did not detect an external barometer, starting onboard barometer"
|
||||||
|
if [ "$PLATFORM" == "M0197" ]; then
|
||||||
|
/bin/echo "Starting dps368 barometer on M0197"
|
||||||
|
qshell dps310 start -I -b 5
|
||||||
|
else
|
||||||
|
# Start Invensense ICP 101xx barometer built on to VOXL 2
|
||||||
|
qshell icp101xx start -I -b 5
|
||||||
|
fi
|
||||||
fi
|
fi
|
||||||
|
|
||||||
# Auto detect the magnetometer. If one or both of these devices
|
# Auto detect the magnetometer. If one or both of these devices
|
||||||
@ -102,18 +131,19 @@ fi
|
|||||||
qshell qmc5883l start -R 10 -X -b 1
|
qshell qmc5883l start -R 10 -X -b 1
|
||||||
/bin/echo "Looking for ist8310 magnetometer"
|
/bin/echo "Looking for ist8310 magnetometer"
|
||||||
qshell ist8310 start -R 10 -X -b 1
|
qshell ist8310 start -R 10 -X -b 1
|
||||||
|
/bin/echo "Looking for ist8308 magnetometer"
|
||||||
|
# Rotation 12 = PITCH_180
|
||||||
|
qshell ist8308 start -R 12 -X -b 1
|
||||||
|
/bin/echo "Looking for iis2mdc magnetometer"
|
||||||
|
# Note: Rotation may not be correct
|
||||||
|
qshell iis2mdc start -R 10 -X -b 1
|
||||||
|
|
||||||
# GPS and magnetometer
|
# GPS and magnetometer
|
||||||
if [ "$GPS" != "NONE" ]; then
|
if [ "$GPS" != "NONE" ]; then
|
||||||
# On M0052 the GPS driver runs on the apps processor
|
if [ "$PLATFORM" == "M0197" ]; then
|
||||||
if [ $PLATFORM = "M0052" ]; then
|
gps start -d /dev/ttyHS7
|
||||||
gps start -d /dev/ttyHS2
|
|
||||||
# On M0197 the GPS driver runs on the apps processor
|
|
||||||
elif [ $PLATFORM = "M0197" ]; then
|
|
||||||
gps start -d /dev/ttyHS7
|
|
||||||
# On M0054 and M0104 the GPS driver runs on SLPI DSP
|
|
||||||
else
|
else
|
||||||
qshell gps start -d 6
|
qshell gps start
|
||||||
fi
|
fi
|
||||||
fi
|
fi
|
||||||
|
|
||||||
@ -158,20 +188,12 @@ elif [ "$RC" == "CRSF_MAV" ]; then
|
|||||||
qshell mavlink_rc_in start -m -p 7 -b 115200
|
qshell mavlink_rc_in start -m -p 7 -b 115200
|
||||||
elif [ "$RC" == "SPEKTRUM" ]; then
|
elif [ "$RC" == "SPEKTRUM" ]; then
|
||||||
/bin/echo "Starting Spektrum RC"
|
/bin/echo "Starting Spektrum RC"
|
||||||
# On M0052 the RC driver runs on the apps processor
|
qshell spektrum_rc start
|
||||||
if [ $PLATFORM = "M0052" ]; then
|
|
||||||
rc_input start -d /dev/ttyHS1
|
|
||||||
# On M0054 and M0104 the RC driver runs on SLPI DSP
|
|
||||||
else
|
|
||||||
qshell spektrum_rc start
|
|
||||||
fi
|
|
||||||
elif [ "$RC" == "GHST" ]; then
|
elif [ "$RC" == "GHST" ]; then
|
||||||
/bin/echo "Starting GHST RC driver"
|
/bin/echo "Starting GHST RC driver"
|
||||||
qshell ghst_rc start -d 7
|
qshell ghst_rc start -d 7
|
||||||
elif [ "$RC" == "M0065_SBUS" ]; then
|
elif [ "$RC" == "M0065_SBUS" ]; then
|
||||||
if [ $PLATFORM = "M0052" ]; then
|
if [ "$ESC" != "VOXL2_IO_PWM_ESC" ]; then
|
||||||
apps_sbus start
|
|
||||||
elif [ "$ESC" != "VOXL2_IO_PWM_ESC" ]; then
|
|
||||||
/bin/echo "Attempting to start M0065 SBUS RC driver for original M0065 FW"
|
/bin/echo "Attempting to start M0065 SBUS RC driver for original M0065 FW"
|
||||||
qshell dsp_sbus start
|
qshell dsp_sbus start
|
||||||
retVal=$?
|
retVal=$?
|
||||||
@ -195,6 +217,10 @@ if [ "$POWER_MANAGER" == "VOXLPM" ]; then
|
|||||||
qshell voxlpm start -X -b 2
|
qshell voxlpm start -X -b 2
|
||||||
fi
|
fi
|
||||||
|
|
||||||
|
if [ "$AIRSPEED_SENSOR" == "MS4525DO" ]; then
|
||||||
|
qshell ms4525do start -X -b 4
|
||||||
|
fi
|
||||||
|
|
||||||
# Optional distance sensor on spare i2c
|
# Optional distance sensor on spare i2c
|
||||||
# qshell vl53l0x start -X -b 4
|
# qshell vl53l0x start -X -b 4
|
||||||
# qshell vl53l1x start -X -b 4
|
# qshell vl53l1x start -X -b 4
|
||||||
@ -202,22 +228,27 @@ fi
|
|||||||
# Start all of the processing modules on DSP
|
# Start all of the processing modules on DSP
|
||||||
qshell sensors start
|
qshell sensors start
|
||||||
qshell ekf2 start
|
qshell ekf2 start
|
||||||
qshell mc_pos_control start
|
|
||||||
qshell mc_att_control start
|
if [ "$AIRFRAME" == "FIXED_WING" ]; then
|
||||||
qshell mc_rate_control start
|
qshell fw_pos_control start
|
||||||
qshell mc_hover_thrust_estimator start
|
qshell fw_att_control start
|
||||||
qshell mc_autotune_attitude_control start
|
qshell fw_rate_control start
|
||||||
qshell land_detector start multicopter
|
qshell airspeed_selector start
|
||||||
|
qshell fw_autotune_attitude_control start
|
||||||
|
qshell land_detector start fixedwing
|
||||||
|
elif [ $AIRFRAME = "MULTICOPTER" ]; then
|
||||||
|
qshell mc_pos_control start
|
||||||
|
qshell mc_att_control start
|
||||||
|
qshell mc_rate_control start
|
||||||
|
qshell mc_hover_thrust_estimator start
|
||||||
|
qshell mc_autotune_attitude_control start
|
||||||
|
qshell land_detector start multicopter
|
||||||
|
fi
|
||||||
|
|
||||||
qshell manual_control start
|
qshell manual_control start
|
||||||
qshell control_allocator start
|
qshell control_allocator start
|
||||||
qshell load_mon start
|
qshell load_mon start
|
||||||
|
qshell rc_update start
|
||||||
# Only start the rc_update module if an actual RC driver
|
|
||||||
# is publishing input_rc topics. Otherwise for external RC
|
|
||||||
# over Mavlink this isn't needed.
|
|
||||||
if [ "$RC" != "EXTERNAL" ]; then
|
|
||||||
qshell rc_update start
|
|
||||||
fi
|
|
||||||
|
|
||||||
qshell commander start
|
qshell commander start
|
||||||
|
|
||||||
@ -227,21 +258,15 @@ qshell flight_mode_manager start
|
|||||||
# Start all of the processing modules on the applications processor
|
# Start all of the processing modules on the applications processor
|
||||||
dataman start
|
dataman start
|
||||||
navigator start
|
navigator start
|
||||||
|
vehicle_air_data_bridge start
|
||||||
# This bridge allows raw data packets to be sent over UART to the ESC
|
sensor_baro_bridge start
|
||||||
# voxl2_io_bridge start
|
vehicle_local_position_bridge start
|
||||||
|
|
||||||
# Start uxrce_dds_client for ros2 offboard messages from agent over localhost
|
# Start uxrce_dds_client for ros2 offboard messages from agent over localhost
|
||||||
uxrce_dds_client start -t udp -h 127.0.0.1 -p 8888
|
uxrce_dds_client start -t udp -h 127.0.0.1 -p 8888
|
||||||
|
|
||||||
voxl_save_cal_params start
|
voxl_save_cal_params start
|
||||||
|
|
||||||
# On M0052 there is only one IMU. So, PX4 needs to
|
|
||||||
# publish IMU samples externally for VIO to use.
|
|
||||||
if [ $PLATFORM = "M0052" ]; then
|
|
||||||
imu_server start
|
|
||||||
fi
|
|
||||||
|
|
||||||
# start the onboard fast link to connect to voxl-mavlink-server
|
# start the onboard fast link to connect to voxl-mavlink-server
|
||||||
mavlink start -x -u 14556 -o 14557 -r 100000 -n lo -m onboard
|
mavlink start -x -u 14556 -o 14557 -r 100000 -n lo -m onboard
|
||||||
|
|
||||||
@ -252,17 +277,12 @@ mavlink stream -u 14556 -s ATTITUDE_QUATERNION -r 10
|
|||||||
mavlink stream -u 14556 -s GLOBAL_POSITION_INT -r 30
|
mavlink stream -u 14556 -s GLOBAL_POSITION_INT -r 30
|
||||||
mavlink stream -u 14556 -s SCALED_PRESSURE -r 10
|
mavlink stream -u 14556 -s SCALED_PRESSURE -r 10
|
||||||
|
|
||||||
|
# Increase heartbeat rate so VFC can get faster mode updates
|
||||||
|
mavlink stream -u 14556 -s HEARTBEAT -r 10
|
||||||
|
|
||||||
# start the slow normal mode for voxl-mavlink-server to forward to GCS
|
# start the slow normal mode for voxl-mavlink-server to forward to GCS
|
||||||
mavlink start -x -u 14558 -o 14559 -r 100000 -n lo
|
mavlink start -x -u 14558 -o 14559 -r 100000 -n lo
|
||||||
|
|
||||||
# Start logging and use timestamps for log files when possible.
|
|
||||||
# Add the "-e" option to start logging immediately. Default is
|
|
||||||
# to log only when armed. Caution must be used with the "-e" option
|
|
||||||
# because if power is removed without stopping the logger gracefully then
|
|
||||||
# the log file may be corrupted. Rather than using "-e" option it's better
|
|
||||||
# to use the SDLOG_MODE to do that.
|
|
||||||
logger start -t -b 256
|
|
||||||
|
|
||||||
mavlink boot_complete
|
mavlink boot_complete
|
||||||
|
|
||||||
# Optional MSP OSD driver for DJI goggles
|
# Optional MSP OSD driver for DJI goggles
|
||||||
@ -277,3 +297,9 @@ for i in "${EXTRA_STEPS[@]}"
|
|||||||
do
|
do
|
||||||
$i
|
$i
|
||||||
done
|
done
|
||||||
|
|
||||||
|
# Start logging module. This is done as the last step because any topics
|
||||||
|
# marked as optional will only be logged if they have been advertised when
|
||||||
|
# this is started. By starting it last it makes sure to see those
|
||||||
|
# advertisements as the other modules are starting before it.
|
||||||
|
logger start
|
||||||
|
|||||||
@ -40,5 +40,6 @@ px4_add_module(
|
|||||||
DPS310_SPI.cpp
|
DPS310_SPI.cpp
|
||||||
dps310_main.cpp
|
dps310_main.cpp
|
||||||
DEPENDS
|
DEPENDS
|
||||||
|
drivers__device
|
||||||
px4_work_queue
|
px4_work_queue
|
||||||
)
|
)
|
||||||
|
|||||||
Loading…
x
Reference in New Issue
Block a user