mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-09 22:08:56 +08:00
feat(boards/modalai/voxl2): Add SIH simulator to the build and start scripts
This commit is contained in:
Eric Katzfey
committed by
Eric Katzfey
Eric Katzfey
parent
3ac9b267ca
commit
a779925618
@@ -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
|
||||
)
|
||||
|
||||
|
||||
@@ -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"
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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()
|
||||
|
||||
@@ -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
|
||||
)
|
||||
@@ -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);
|
||||
}
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
+124
@@ -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
|
||||
Reference in New Issue
Block a user