feat(boards/modalai/voxl2): Add SIH simulator to the build and start scripts

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
 )
 

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"

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

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()

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
+	)

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 <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_local_position.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
+
+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);
+}

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

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

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