diff --git a/boards/modalai/voxl2/cmake/install.cmake b/boards/modalai/voxl2/cmake/install.cmake index 9e144f932e..5a20dbbb9b 100644 --- a/boards/modalai/voxl2/cmake/install.cmake +++ b/boards/modalai/voxl2/cmake/install.cmake @@ -49,6 +49,7 @@ install(PROGRAMS ${PX4_BOARD_DIR}/target/voxl-px4-start ${PX4_BOARD_DIR}/target/voxl-px4-hitl ${PX4_BOARD_DIR}/target/voxl-px4-hitl-start + ${PX4_BOARD_DIR}/target/voxl-px4-sih-start ${PX4_BOARD_DIR}/scripts/voxl-configure-px4 DESTINATION bin ) @@ -62,6 +63,7 @@ install(FILES ${VOXL2_SLPI_BUILD_DIR}/platforms/qurt/libpx4.so install(FILES ${PX4_BOARD_DIR}/target/voxl-px4-fake-imu-calibration.config ${PX4_BOARD_DIR}/target/voxl-px4-hitl-set-default-parameters.config + ${PX4_BOARD_DIR}/target/voxl-px4-sih-set-default-parameters.config DESTINATION ../etc/modalai ) diff --git a/boards/modalai/voxl2/scripts/install-voxl.sh b/boards/modalai/voxl2/scripts/install-voxl.sh index fc08e44213..e3ba736a3b 100755 --- a/boards/modalai/voxl2/scripts/install-voxl.sh +++ b/boards/modalai/voxl2/scripts/install-voxl.sh @@ -12,16 +12,19 @@ adb push boards/modalai/voxl2/target/voxl-px4 /usr/bin adb push boards/modalai/voxl2/target/voxl-px4-start /usr/bin adb push boards/modalai/voxl2/target/voxl-px4-hitl /usr/bin adb push boards/modalai/voxl2/target/voxl-px4-hitl-start /usr/bin +adb push boards/modalai/voxl2/target/voxl-px4-sih-start /usr/bin adb shell chmod a+x /usr/bin/px4-alias.sh adb shell chmod a+x /usr/bin/voxl-px4 adb shell chmod a+x /usr/bin/voxl-px4-start adb shell chmod a+x /usr/bin/voxl-px4-hitl adb shell chmod a+x /usr/bin/voxl-px4-hitl-start +adb shell chmod a+x /usr/bin/voxl-px4-sih-start # Push configuration file adb shell mkdir -p /etc/modalai adb push boards/modalai/voxl2/target/voxl-px4-fake-imu-calibration.config /etc/modalai adb push boards/modalai/voxl2/target/voxl-px4-hitl-set-default-parameters.config /etc/modalai +adb push boards/modalai/voxl2/target/voxl-px4-sih-set-default-parameters.config /etc/modalai # Make sure to setup all of the needed px4 aliases. adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-accelsim" @@ -132,6 +135,7 @@ 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" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-sih_vio_bridge" # Make sure any required directories exist adb shell "/bin/mkdir -p /data/px4/param" diff --git a/boards/modalai/voxl2/slpi.px4board b/boards/modalai/voxl2/slpi.px4board index 2ca84442a6..62dfebe2ff 100644 --- a/boards/modalai/voxl2/slpi.px4board +++ b/boards/modalai/voxl2/slpi.px4board @@ -64,6 +64,7 @@ CONFIG_MODULES_MUORB_SLPI=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y +CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y CONFIG_SYSTEMCMDS_PARAM=y CONFIG_SYSTEMCMDS_UORB=y CONFIG_ORB_COMMUNICATOR=y diff --git a/boards/modalai/voxl2/src/CMakeLists.txt b/boards/modalai/voxl2/src/CMakeLists.txt index 95f9296dd2..43ea73dbf5 100644 --- a/boards/modalai/voxl2/src/CMakeLists.txt +++ b/boards/modalai/voxl2/src/CMakeLists.txt @@ -108,4 +108,5 @@ elseif("${PX4_PLATFORM}" STREQUAL "posix") add_subdirectory(${PX4_BOARD_DIR}/src/modules/vehicle_air_data_bridge vehicle_air_data_bridge) add_subdirectory(${PX4_BOARD_DIR}/src/modules/sensor_baro_bridge sensor_baro_bridge) add_subdirectory(${PX4_BOARD_DIR}/src/modules/vehicle_local_position_bridge vehicle_local_position_bridge) + add_subdirectory(${PX4_BOARD_DIR}/src/modules/sih_vio_bridge sih_vio_bridge) endif() diff --git a/boards/modalai/voxl2/src/modules/sih_vio_bridge/CMakeLists.txt b/boards/modalai/voxl2/src/modules/sih_vio_bridge/CMakeLists.txt new file mode 100644 index 0000000000..ee380231d0 --- /dev/null +++ b/boards/modalai/voxl2/src/modules/sih_vio_bridge/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 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__sih_vio_bridge + MAIN sih_vio_bridge + INCLUDES + ${CMAKE_CURRENT_SOURCE_DIR}/../../lib/mpa + SRCS + sih_vio_bridge.cpp + DEPENDS + mpa + ) diff --git a/boards/modalai/voxl2/src/modules/sih_vio_bridge/sih_vio_bridge.cpp b/boards/modalai/voxl2/src/modules/sih_vio_bridge/sih_vio_bridge.cpp new file mode 100644 index 0000000000..6132a60e3f --- /dev/null +++ b/boards/modalai/voxl2/src/modules/sih_vio_bridge/sih_vio_bridge.cpp @@ -0,0 +1,283 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ + +/** + * @file sih_vio_bridge.cpp + * + * Bridges SIH ground truth pose data to MPA VIO pipes so that other VOXL2 + * services (e.g. voxl-vision-hub) can consume simulated pose as if it were + * real VIO data. + * + * Subscribes to vehicle_local_position_groundtruth and + * vehicle_attitude_groundtruth published by the SIH simulator module, + * converts the data to vio_data_t, and writes to the "hitl_vio" and "state" + * MPA pipes. + */ + +#include "mpa.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class SihVioBridge : public ModuleBase, public px4::WorkItem +{ +public: + static Descriptor desc; + + SihVioBridge(); + ~SihVioBridge() override = default; + + static int task_spawn(int argc, char *argv[]); + static int custom_command(int argc, char *argv[]); + static int print_usage(const char *reason = nullptr); + + bool init(); + +private: + void Run() override; + + int64_t TimeMonotonic_ns(); + + uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position_groundtruth)}; + uORB::Subscription _attitude_sub{ORB_ID(vehicle_attitude_groundtruth)}; + uORB::Subscription _angular_vel_sub{ORB_ID(vehicle_angular_velocity_groundtruth)}; + + int _vio_pipe_ch{0}; + int _flow_pipe_ch{0}; +}; + +ModuleBase::Descriptor SihVioBridge::desc{task_spawn, custom_command, print_usage}; + +SihVioBridge::SihVioBridge() : + WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) +{ +} + +bool SihVioBridge::init() +{ + if (MPA::Initialize() == -1) { + return false; + } + + char vio_pipe_name[] = "hitl_vio"; + _vio_pipe_ch = MPA::PipeCreate(vio_pipe_name); + + if (_vio_pipe_ch == -1) { + PX4_ERR("Pipe create failed for %s", vio_pipe_name); + return false; + } + + char flow_pipe_name[] = "state"; + _flow_pipe_ch = MPA::PipeCreate(flow_pipe_name); + + if (_flow_pipe_ch == -1) { + PX4_ERR("Pipe create failed for %s", flow_pipe_name); + return false; + } + + if (!_local_pos_sub.registerCallback()) { + PX4_ERR("callback registration failed"); + return false; + } + + return true; +} + +int64_t SihVioBridge::TimeMonotonic_ns() +{ + struct timespec ts; + + if (clock_gettime(CLOCK_MONOTONIC, &ts)) { + PX4_ERR("ERROR calling clock_gettime"); + return -1; + } + + return (int64_t)ts.tv_sec * 1000000000 + (int64_t)ts.tv_nsec; +} + +void SihVioBridge::Run() +{ + if (should_exit()) { + _local_pos_sub.unregisterCallback(); + exit_and_cleanup(desc); + return; + } + + if (_local_pos_sub.updated()) { + vehicle_local_position_s lpos{}; + + if (_local_pos_sub.copy(&lpos)) { + vio_data_t s; + memset(&s, 0, sizeof(s)); + + s.magic_number = VIO_MAGIC_NUMBER; + s.error_code = 0; + s.state = VIO_STATE_OK; + s.timestamp_ns = TimeMonotonic_ns(); + + // Position + s.T_imu_wrt_vio[0] = lpos.x; + s.T_imu_wrt_vio[1] = lpos.y; + s.T_imu_wrt_vio[2] = lpos.z; + + // Velocity + s.vel_imu_wrt_vio[0] = lpos.vx; + s.vel_imu_wrt_vio[1] = lpos.vy; + s.vel_imu_wrt_vio[2] = lpos.vz; + + // Attitude: convert quaternion to rotation matrix + vehicle_attitude_s att{}; + + if (_attitude_sub.copy(&att)) { + float w = att.q[0], x = att.q[1], y = att.q[2], z = att.q[3]; + float xx = x * x, yy = y * y, zz = z * z; + float xy = x * y, xz = x * z, yz = y * z; + float wx = w * x, wy = w * y, wz = w * z; + + s.R_imu_to_vio[0][0] = 1 - 2 * (yy + zz); + s.R_imu_to_vio[0][1] = 2 * (xy - wz); + s.R_imu_to_vio[0][2] = 2 * (xz + wy); + + s.R_imu_to_vio[1][0] = 2 * (xy + wz); + s.R_imu_to_vio[1][1] = 1 - 2 * (xx + zz); + s.R_imu_to_vio[1][2] = 2 * (yz - wx); + + s.R_imu_to_vio[2][0] = 2 * (xz - wy); + s.R_imu_to_vio[2][1] = 2 * (yz + wx); + s.R_imu_to_vio[2][2] = 1 - 2 * (xx + yy); + } + + // Angular velocity + vehicle_angular_velocity_s ang_vel{}; + + if (_angular_vel_sub.copy(&ang_vel)) { + s.imu_angular_vel[0] = ang_vel.xyz[0]; + s.imu_angular_vel[1] = ang_vel.xyz[1]; + s.imu_angular_vel[2] = ang_vel.xyz[2]; + } + + // Gravity vector in VIO frame (NED: gravity is +Z) + s.gravity_vector[0] = 0.f; + s.gravity_vector[1] = 0.f; + s.gravity_vector[2] = 1.f; + + // Ground truth has perfect quality + s.quality = 100; + s.n_feature_points = 25; + + // Small fixed covariance for ground truth + s.pose_covariance[0] = 1e-6f; // x + s.pose_covariance[6] = 1e-6f; // y + s.pose_covariance[11] = 1e-6f; // z + s.pose_covariance[15] = 1e-6f; // roll + s.pose_covariance[18] = 1e-6f; // pitch + s.pose_covariance[20] = 1e-6f; // yaw + + s.velocity_covariance[0] = 1e-6f; // vx + s.velocity_covariance[6] = 1e-6f; // vy + s.velocity_covariance[11] = 1e-6f; // vz + + if (MPA::PipeWrite(_vio_pipe_ch, (void *)&s, sizeof(vio_data_t)) == -1) { + PX4_ERR("Pipe %d write failed!", _vio_pipe_ch); + } + + if (MPA::PipeWrite(_flow_pipe_ch, (void *)&s, sizeof(vio_data_t)) == -1) { + PX4_ERR("Pipe %d write failed!", _flow_pipe_ch); + } + } + } +} + +int SihVioBridge::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int SihVioBridge::task_spawn(int argc, char *argv[]) +{ + SihVioBridge *instance = new SihVioBridge(); + + 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 SihVioBridge::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Bridges SIH ground truth pose data to MPA VIO pipes. + +When running SIH simulation on VOXL2, this module publishes the simulated +pose (position, attitude, velocity, angular velocity) to the "hitl_vio" and +"state" MPA pipes in vio_data_t format, allowing other VOXL2 services to +consume simulated VIO data. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("sih_vio_bridge", "simulation"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int sih_vio_bridge_main(int argc, char *argv[]) +{ + return ModuleBase::main(SihVioBridge::desc, argc, argv); +} diff --git a/boards/modalai/voxl2/target/voxl-px4 b/boards/modalai/voxl2/target/voxl-px4 index 951d7cb198..861a4a8f2e 100755 --- a/boards/modalai/voxl2/target/voxl-px4 +++ b/boards/modalai/voxl2/target/voxl-px4 @@ -52,6 +52,7 @@ print_usage() { 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 " [-S (Start in SIH simulation mode)]" echo " [-w (Specify TBS Crossfire RC receiver, raw)]" echo " [-z (Use fake sensor calibration values)]" echo " [-h (show help)]" @@ -79,7 +80,7 @@ print_config_settings(){ echo -e "*************************\n" } -while getopts "abcdhfmoprwz" flag +while getopts "abcdhfmoprSwz" flag do case "${flag}" in a) @@ -124,6 +125,11 @@ do echo "[INFO] TBS Crossfire RC receiver, MAVLINK selected" RC=CRSF_MAV ;; + S) + echo "[INFO] SIH simulation mode selected" + px4 -s /usr/bin/voxl-px4-sih-start + exit 0 + ;; w) echo "[INFO] TBS Crossfire RC receiver, raw selected" RC=CRSF_RAW diff --git a/boards/modalai/voxl2/target/voxl-px4-sih-set-default-parameters.config b/boards/modalai/voxl2/target/voxl-px4-sih-set-default-parameters.config new file mode 100644 index 0000000000..8515b7c927 --- /dev/null +++ b/boards/modalai/voxl2/target/voxl-px4-sih-set-default-parameters.config @@ -0,0 +1,71 @@ +# Default parameters for SIH (Simulator in Hardware) mode + +param select /data/px4/param/sih_parameters + +# Load in all of the current parameters that have been saved in the file +param load + +# Set airframe type +param set SYS_AUTOCONFIG 1 +param set SYS_AUTOSTART 4001 + +# Simulated accelerometer calibration (device ID 1310988 = DRV_IMU_DEVTYPE_SIM) +param set CAL_ACC0_ID 1310988 +param set CAL_ACC0_PRIO 75 +param set CAL_ACC0_ROT -1 +param set CAL_ACC0_XOFF 0.0 +param set CAL_ACC0_XSCALE 1.0 +param set CAL_ACC0_YOFF 0.0 +param set CAL_ACC0_YSCALE 1.0 +param set CAL_ACC0_ZOFF 0.0 +param set CAL_ACC0_ZSCALE 1.0 + +# Simulated gyroscope calibration (device ID 1310988 = DRV_IMU_DEVTYPE_SIM) +param set CAL_GYRO0_ID 1310988 +param set CAL_GYRO0_PRIO 75 +param set CAL_GYRO0_ROT -1 +param set CAL_GYRO0_XOFF 0.0 +param set CAL_GYRO0_YOFF 0.0 +param set CAL_GYRO0_ZOFF 0.0 + +# Simulated magnetometer calibration +param set CAL_MAG0_ID 197388 +param set CAL_MAG0_PRIO 75 +param set CAL_MAG0_ROT -1 +param set CAL_MAG0_XOFF 0.0 +param set CAL_MAG0_XSCALE 1.0 +param set CAL_MAG0_YOFF 0.0 +param set CAL_MAG0_YSCALE 1.0 +param set CAL_MAG0_ZOFF 0.0 +param set CAL_MAG0_ZSCALE 1.0 + +# Control allocation: map HIL_ACT outputs to motor functions +# pwm_out_sim uses HIL_ACT prefix on non-SITL boards +param set HIL_ACT_FUNC1 101 +param set HIL_ACT_FUNC2 102 +param set HIL_ACT_FUNC3 103 +param set HIL_ACT_FUNC4 104 + +# Rotor configuration for control allocator (quadrotor X) +param set CA_ROTOR_COUNT 4 +param set CA_AIRFRAME 0 +param set CA_ROTOR0_PX 1 +param set CA_ROTOR0_PY 1 +param set CA_ROTOR1_PX -1 +param set CA_ROTOR1_PY -1 +param set CA_ROTOR2_PX 1 +param set CA_ROTOR2_PY -1 +param set CA_ROTOR2_KM -0.05 +param set CA_ROTOR3_PX -1 +param set CA_ROTOR3_PY 1 +param set CA_ROTOR3_KM -0.05 + +# Vehicle type: quadrotor (required for multicopter position control) +param set MAV_TYPE 2 + +# Disable power supply check (no real battery in SIH mode) +param set CBRK_SUPPLY_CHK 894281 + +param save + +sleep 2 diff --git a/boards/modalai/voxl2/target/voxl-px4-sih-start b/boards/modalai/voxl2/target/voxl-px4-sih-start new file mode 100755 index 0000000000..b0ea470547 --- /dev/null +++ b/boards/modalai/voxl2/target/voxl-px4-sih-start @@ -0,0 +1,124 @@ +#!/bin/sh +# PX4 commands need the 'px4-' prefix in bash. +# (px4-alias.sh is expected to be in the PATH) +. px4-alias.sh + +echo -e "\n*************************" +echo "SIH Simulation Mode" +echo -e "*************************\n" + +# Sleep a little here. A lot happens when the uorb and muorb start +# and we need to make sure that it all completes successfully to avoid +# any possible race conditions. +/bin/sleep 1 + +# Set up the default sih parameters if the dedicated sih parameter file doesn't exist yet. +if [ ! -f /data/px4/param/sih_parameters ]; then + echo "[INFO] Setting default parameters for PX4 SIH on voxl" + . /etc/modalai/voxl-px4-sih-set-default-parameters.config + /bin/sync +# Otherwise load existing sih parameters +else + param select /data/px4/param/sih_parameters + param load +fi + +# Start simulated sensor drivers on DSP +/bin/echo "Starting simulated sensor drivers on DSP" +qshell simulator_sih start +qshell sensor_baro_sim start +qshell sensor_gps_sim start +qshell sensor_mag_sim start +qshell pwm_out_sim start + +# We do not change the value of SYS_AUTOCONFIG but if it does not +# show up as used then it is not reported to QGC and we get a +# missing parameter error. Also, we don't use SYS_AUTOSTART but QGC +# complains about it as well. +param touch SYS_AUTOCONFIG +param touch SYS_AUTOSTART + +# Start all of the processing modules on DSP +qshell sensors start +qshell ekf2 start + +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 + +qshell manual_control start +qshell control_allocator start +qshell load_mon start +qshell rc_update start + +qshell commander start + +# This is needed for altitude and position hold modes +qshell flight_mode_manager start + +# Start all of the processing modules on the applications processor +dataman start +navigator start +vehicle_air_data_bridge start +sensor_baro_bridge start +vehicle_local_position_bridge start +sih_vio_bridge start + +# 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 + +# start the onboard fast link to connect to voxl-mavlink-server +mavlink start -x -u 14556 -o 14557 -r 100000 -n lo -m onboard + +# slow down some of the fastest streams +mavlink stream -u 14556 -s HIGHRES_IMU -r 10 +mavlink stream -u 14556 -s ATTITUDE -r 10 +mavlink stream -u 14556 -s ATTITUDE_QUATERNION -r 10 +mavlink stream -u 14556 -s GLOBAL_POSITION_INT -r 30 +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 +mavlink start -x -u 14558 -o 14559 -r 100000 -n lo + +# Start the jmavsim visualization stream. Change -t ip address to your desired endpoint +# TODO: Should go through voxl-mavlink-server, not just direct like this +# mavlink start -x -u 14560 -o 14560 -r 4000000 -t 192.168.101.1 -m minimal +# mavlink stream -u 14560 -s HIL_STATE_QUATERNION -r 200 + +mavlink boot_complete + +# 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. +# +# Set logger mode based on SDLOG_MODE parameter: +# 0: log when armed until disarm (default) +# 1: log from boot until disarm +# 2: log from boot until shutdown +# 3: log based on AUX1 RC channel +# 4: log from first armed until shutdown +LOGGER_ARGS="" +if param compare SDLOG_MODE 1 +then + LOGGER_ARGS="-e" +fi +if param compare SDLOG_MODE 2 +then + LOGGER_ARGS="-f" +fi +if param compare SDLOG_MODE 3 +then + LOGGER_ARGS="-x" +fi +if param compare SDLOG_MODE 4 +then + LOGGER_ARGS="-a" +fi +logger start $LOGGER_ARGS