dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

SIH in SITL with lockstep (#19028) 

* sih: Move sih out of work queue
This reverts commit bb7dd0cf00.

* sih-sim: Enable sih in sitl, together with lockstep

* sih-sim: new files for sih: quadx and airplane

* sih: Added tailsitter for sih-sitl simulation

* sitl_target: Added seperate target loop for sih

* jmavsim_run: Allow jmavsim to run in UDP mode

* lockstep: Post semaphore on last lockstep component removed

* sih-sim: Added display for effectively achieved speed

* sih: increase stack size

* sih-sim: Improved sleep time computation, fixes bug of running too fast

* sitl_target: place omnicopter in alphabethic order

Co-authored-by: romain-chiap <romain.chiap@gmail.com>
Co-authored-by: Matthias Grob <maetugr@gmail.com>
This commit is contained in:
Thomas Debrunner 2022-06-09 09:52:34 +02:00 committed by GitHub
parent a7e11464c1
commit 46c9d1e288
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
16 changed files with 379 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
ROMFS/px4fmu_common/init.d-posix/airframes/10040_quadx Normal file
View File

@ -0,0 +1,23 @@
#!/bin/sh
#
# @name QuadrotorX SITL for SIH
#
# @type Quadrotor
#
# @maintainer Romain Chiappinelli <romain.chiap@gmail.com>
#
. ${R}etc/init.d/rc.mc_defaults
set MIXER quad_x
# disable some checks to allow to fly:
# - with usb
param set-default CBRK_USB_CHK 197848
# - without real battery
param set-default CBRK_SUPPLY_CHK 894281
# - without safety switch
param set-default COM_PREARM_MODE 0
param set-default CBRK_IO_SAFETY 22027
param set SIH_VEHICLE_TYPE 0

33
ROMFS/px4fmu_common/init.d-posix/airframes/10041_airplane Normal file
View File

@ -0,0 +1,33 @@
#!/bin/sh
#
# @name Plane SITL for SIH
#
# @type Plane
#
# @maintainer Romain Chiappinelli <romain.chiap@gmail.com>
. ${R}etc/init.d/rc.fw_defaults
set MIXER AERT
# disable some checks to allow to fly:
# - with usb
param set-default CBRK_USB_CHK 197848
# - without real battery
param set-default CBRK_SUPPLY_CHK 894281
# - without safety switch
param set-default COM_PREARM_MODE 0
param set-default CBRK_IO_SAFETY 22027
param set-default BAT_N_CELLS 3
param set SIH_T_MAX 6.0
param set SIH_MASS 0.3
param set SIH_IXX 0.00402
param set SIH_IYY 0.0144
param set SIH_IZZ 0.0177
param set SIH_IXZ 0.00046
param set SIH_KDV 0.2
param set SIH_VEHICLE_TYPE 1 # sih as fixed wing
param set RWTO_TKOFF 1 # enable takeoff from runway (as opposed to launched)

45
ROMFS/px4fmu_common/init.d-posix/airframes/10042_xvert Normal file
View File

@ -0,0 +1,45 @@
#!/bin/sh
#
# @name SIH Tailsitter Duo
#
# @type VTOL
#
# @maintainer Romain Chiappinelli <romain.chiap@gmail.com>
. ${R}etc/init.d/rc.vtol_defaults
param set-default VT_ELEV_MC_LOCK 0
param set-default VT_TYPE 0
param set-default VT_FW_DIFTHR_EN 1
param set-default VT_FW_DIFTHR_SC 0.3
param set-default MPC_MAN_Y_MAX 60
param set-default MC_PITCH_P 5
param set-default MAV_TYPE 19
set MAV_TYPE 19
set MIXER vtol_tailsitter_duo_sat
# disable some checks to allow to fly:
# - with usb
param set-default CBRK_USB_CHK 197848
# - without real battery
param set-default CBRK_SUPPLY_CHK 894281
# - without safety switch
param set-default COM_PREARM_MODE 0
param set-default CBRK_IO_SAFETY 22027
param set-default BAT_N_CELLS 3
param set SIH_T_MAX 2.0
param set SIH_Q_MAX 0.0165
param set SIH_MASS 0.2
# IXX and IZZ are inverted from the thesis as the body frame is pitched by 90 deg
param set SIH_IXX 0.00354
param set SIH_IYY 0.000625
param set SIH_IZZ 0.00300
param set SIH_IXZ 0.0
param set SIH_KDV 0.2
param set SIH_L_ROLL 0.145
# sih as tailsitter
param set SIH_VEHICLE_TYPE 2

3
ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt
View File

@ -38,6 +38,9 @@ px4_add_romfs_files(
10019_omnicopter
10020_if750a
10030_px4vision
10040_quadx
10041_airplane
10042_xvert
1010_iris_opt_flow
1010_iris_opt_flow.post
1011_iris_irlock

9
ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink
View File

@ -30,3 +30,12 @@ mavlink start -x -u $udp_onboard_payload_port_local -r 4000 -f -m onboard -o $ud
# Onboard link to gimbal
mavlink start -x -u $udp_onboard_gimbal_port_local -r 400000 -m gimbal -o $udp_onboard_gimbal_port_remote
# To display for SIH sitl
if [ "$SIM_MODE" = "sihsim" ]; then
udp_sihsim_port_local=$((19450+px4_instance))
udp_sihsim_port_remote=$((19410+px4_instance))
mavlink start -x -u $udp_sihsim_port_local -r 400000 -m custom -o $udp_sihsim_port_remote
mavlink stream -r 200 -s HIL_ACTUATOR_CONTROLS -u $udp_sihsim_port_local
mavlink stream -r 25 -s HIL_STATE_QUATERNION -u $udp_sihsim_port_local
fi

5
ROMFS/px4fmu_common/init.d-posix/rcS
View File

@ -204,8 +204,11 @@ param set IMU_INTEG_RATE 250
. px4-rc.params
dataman start
# start sih in sih_sim mode, otherwise simulator module
if [ "$SIM_MODE" = "sihsim" ]; then
sih start
# only start the simulator if not in replay mode, as both control the lockstep time
if ! replay tryapplyparams
elif ! replay tryapplyparams
then
. px4-rc.simulator
fi

16
Tools/jmavsim_run.sh
View File

@ -5,12 +5,13 @@ set -e
SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
cd "$SCRIPT_DIR/jMAVSim"
tcp_port=4560
port=4560
extra_args=
baudrate=921600
device=
ip="127.0.0.1"
while getopts ":b:d:p:qsr:f:i:loat" opt; do
protocol="tcp"
while getopts ":b:d:u:p:qsr:f:i:loat" opt; do
case $opt in
b)
baudrate=$OPTARG
@ -18,11 +19,14 @@ while getopts ":b:d:p:qsr:f:i:loat" opt; do
d)
device="$OPTARG"
;;
u)
protocol="udp"
;;
i)
ip="$OPTARG"
;;
p)
tcp_port=$OPTARG
port=$OPTARG
;;
q)
extra_args="$extra_args -qgc"
@ -53,7 +57,11 @@ while getopts ":b:d:p:qsr:f:i:loat" opt; do
done
if [ "$device" == "" ]; then
device="-tcp $ip:$tcp_port"
if [ "$protocol" == "tcp" ]; then
device="-tcp $ip:$port"
else
device="-udp $port"
fi
else
device="-serial $device $baudrate"
fi

9
Tools/sitl_run.sh
View File

@ -74,6 +74,9 @@ if [ "$model" == "" ] || [ "$model" == "none" ]; then
if [ "$program" == "jsbsim" ]; then
echo "empty model, setting rascal as default for jsbsim"
model="rascal"
elif [ "$program" == "sihsim" ]; then
echo "empty model, setting quadx as default for sihsim"
model="quadx"
else
echo "empty model, setting iris as default"
model="iris"
@ -214,6 +217,12 @@ elif [ "$program" == "jsbsim" ] && [ -z "$no_sim" ]; then
fi
"${build_path}/build_jsbsim_bridge/jsbsim_bridge" ${model} -s "${src_path}/Tools/jsbsim_bridge/scene/${world}.xml" 2> /dev/null &
JSBSIM_PID=$!
elif [ "$program" == "sihsim" ] && [ ! -n "$no_sim" ]; then
export SIM_MODE="sihsim"
if [ "$model" != "airplane" ] && [ "$model" != "quadx" ] && [ "$model" != "xvert" ]; then
echo "Model ${model} not compatible with with sih. sih supports [quadx,airplane,xvert]."
exit 1
fi
fi
pushd "$rootfs" >/dev/null

1
boards/px4/sitl/default.px4board
View File

@ -39,6 +39,7 @@ CONFIG_MODULES_RC_UPDATE=y
CONFIG_MODULES_REPLAY=y
CONFIG_MODULES_ROVER_POS_CONTROL=y
CONFIG_MODULES_SENSORS=y
CONFIG_MODULES_SIH=y
CONFIG_MODULES_SIMULATOR=y
CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
CONFIG_MODULES_UUV_ATT_CONTROL=y

67
platforms/posix/cmake/sitl_target.cmake
View File

@ -9,7 +9,6 @@ add_custom_command(OUTPUT ${PX4_BINARY_DIR}/logs
)
add_custom_target(logs_symlink DEPENDS ${PX4_BINARY_DIR}/logs)
add_dependencies(px4 logs_symlink)
add_custom_target(run_config
COMMAND Tools/sitl_run.sh $<TARGET_FILE:px4> ${config_sitl_debugger} ${config_sitl_viewer} ${config_sitl_model} ${PX4_SOURCE_DIR} ${PX4_BINARY_DIR}
WORKING_DIRECTORY ${SITL_WORKING_DIR}
@ -159,6 +158,7 @@ set(models
iris_rplidar
iris_vision
nxp_cupcar
omnicopter
plane
plane_cam
plane_catapult
@ -177,7 +177,6 @@ set(models
typhoon_h480_ctrlalloc
uuv_bluerov2_heavy
uuv_hippocampus
omnicopter
)
set(worlds
@ -325,6 +324,70 @@ foreach(debugger ${debuggers})
endforeach()
endforeach()
# create targets for sihsim
set(models_sih
none
airplane
quadx
xvert
)
set(worlds_sih
none
)
foreach(debugger ${debuggers})
foreach(model ${models_sih})
foreach(world ${worlds_sih})
if(world STREQUAL "none")
if(debugger STREQUAL "none")
if(model STREQUAL "none")
set(_targ_name "sihsim")
else()
set(_targ_name "sihsim_${model}")
endif()
else()
if(model STREQUAL "none")
set(_targ_name "sihsim__${debugger}_${world}")
else()
set(_targ_name "sihsim_${model}_${debugger}_${world}")
endif()
endif()
add_custom_target(${_targ_name}
COMMAND ${PX4_SOURCE_DIR}/Tools/sitl_run.sh $<TARGET_FILE:px4> ${debugger} sihsim ${model} ${world} ${PX4_SOURCE_DIR} ${PX4_BINARY_DIR}
WORKING_DIRECTORY ${SITL_WORKING_DIR}
USES_TERMINAL
DEPENDS logs_symlink
)
list(APPEND all_posix_vmd_make_targets ${_targ_name})
else()
if(debugger STREQUAL "none")
if(model STREQUAL "none")
set(_targ_name "sihsim___${world}")
else()
set(_targ_name "sihsim_${model}__${world}")
endif()
else()
if(model STREQUAL "none")
set(_targ_name "sihsim___${debugger}_${world}")
else()
set(_targ_name "sihsim_${model}_${debugger}_${world}")
endif()
endif()
add_custom_target(${_targ_name}
COMMAND ${PX4_SOURCE_DIR}/Tools/sitl_run.sh $<TARGET_FILE:px4> ${debugger} sihsim ${model} ${world} ${PX4_SOURCE_DIR} ${PX4_BINARY_DIR}
WORKING_DIRECTORY ${SITL_WORKING_DIR}
USES_TERMINAL
DEPENDS logs_symlink
)
list(APPEND all_posix_vmd_make_targets ${_targ_name})
endif()
endforeach()
endforeach()
endforeach()
# add flighgear targets
if(ENABLE_LOCKSTEP_SCHEDULER STREQUAL "no")
set(models

2
platforms/posix/src/px4/common/lockstep_scheduler/src/lockstep_components.cpp
View File

@ -84,7 +84,7 @@ void LockstepComponents::unregister_component(int component)
int components_used_bitset = _components_used_bitset;
if (_components_progress_bitset == components_used_bitset && components_used_bitset != 0) {
if (_components_progress_bitset == components_used_bitset) {
_components_progress_bitset = 0;
px4_sem_post(&_components_sem);
}

3
src/modules/sih/CMakeLists.txt
View File

@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
# Copyright (c) 2019-2022 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
@ -45,5 +45,4 @@ px4_add_module(
drivers_accelerometer
drivers_gyroscope
drivers_magnetometer
px4_work_queue
)

16
src/modules/sih/aero.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2019-2022 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
@ -139,12 +139,6 @@ private:
matrix::Vector3f _v_S; // velocity in segment frame
public:
AeroSeg()
{
AeroSeg(1.0f, 0.2f, 0.0f, matrix::Vector3f());
}
/** public constructor
* AeroSeg(float span, float mac, float alpha_0_deg, matrix::Vector3f p_B, float dihedral_deg = 0.0f,
* float AR = -1.0f, float cf = 0.0f, float prop_radius=-1.0f, float cl_alpha=2.0f*M_PI_F, float alpha_max_deg=0.0f, float alpha_min_deg=0.0f)
@ -162,7 +156,7 @@ public:
* alpha_max_deg: maximum angle of attack before stall. Setting to 0 (default) will compute it from a table for flat plate.
* alpha_min_deg: maximum negative angle of attack before stall. Setting to 0 (default) will compute it from a table for flat plate.
*/
AeroSeg(float span, float mac, float alpha_0_deg, matrix::Vector3f p_B, float dihedral_deg = 0.0f,
AeroSeg(float span, float mac, float alpha_0_deg, const matrix::Vector3f &p_B, float dihedral_deg = 0.0f,
float AR = -1.0f, float cf = 0.0f, float prop_radius = -1.0f, float cl_alpha = 2.0f * M_PI_F,
float alpha_max_deg = 0.0f, float alpha_min_deg = 0.0f)
{
@ -176,7 +170,7 @@ public:
_span = span;
_mac = mac;
_alpha_0 = math::radians(alpha_0_deg);
_p_B = matrix::Vector3f(p_B);
_p_B = p_B;
_ar = (AR <= 0.0f) ? _span / _mac : AR; // setting AR<=0 will compute it from _span and _mac
_alpha_eff = 0.0f;
_alpha_eff_old = 0.0f;
@ -208,6 +202,10 @@ public:
_kD = 1.0f / (M_PI_F * K0 * _ar);
}
AeroSeg() : AeroSeg(1.0f, 0.2f, 0.0f, matrix::Vector3f())
{}
/** aerodynamic force and moments of a generic flate plate segment
* void update_aero(matrix::Vector3f v_B, matrix::Vector3f w_B, float alt = 0.0f,
* float def = 0.0f, float thrust=0.0f, float dt = -1.0f)

166
src/modules/sih/sih.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2019-2022 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
@ -54,8 +54,16 @@ using namespace matrix;
using namespace time_literals;
Sih::Sih() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
ModuleParams(nullptr)
{}
Sih::~Sih()
{
perf_free(_loop_perf);
perf_free(_loop_interval_perf);
}
void Sih::run()
{
_px4_accel.set_temperature(T1_C);
_px4_gyro.set_temperature(T1_C);
@ -77,15 +85,86 @@ Sih::Sih() :
if (_sys_ctrl_alloc.get()) {
_actuator_out_sub = uORB::Subscription{ORB_ID(actuator_outputs_sim)};
}
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
lockstep_loop();
#else
realtime_loop();
#endif
exit_and_cleanup();
}
Sih::~Sih()
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
// Get current timestamp in microseconds
uint64_t micros()
{
perf_free(_loop_perf);
perf_free(_loop_interval_perf);
struct timeval t;
gettimeofday(&t, nullptr);
return t.tv_sec * ((uint64_t)1000000) + t.tv_usec;
}
bool Sih::init()
void Sih::lockstep_loop()
{
int rate = math::min(_imu_gyro_ratemax.get(), _imu_integration_rate.get());
// default to 400Hz (2500 us interval)
if (rate <= 0) {
rate = 400;
}
// 200 - 2000 Hz
int sim_interval_us = math::constrain(int(roundf(1e6f / rate)), 500, 5000);
float speed_factor = 1.f;
const char *speedup = getenv("PX4_SIM_SPEED_FACTOR");
if (speedup) {
speed_factor = atof(speedup);
}
int rt_interval_us = int(roundf(sim_interval_us / speed_factor));
PX4_INFO("Simulation loop with %d Hz (%d us sim time interval)", rate, sim_interval_us);
PX4_INFO("Simulation with %.1fx speedup. Loop with (%d us wall time interval)", (double)speed_factor, rt_interval_us);
uint64_t pre_compute_wall_time_us;
while (!should_exit()) {
pre_compute_wall_time_us = micros();
perf_count(_loop_interval_perf);
_current_simulation_time_us += sim_interval_us;
struct timespec ts;
abstime_to_ts(&ts, _current_simulation_time_us);
px4_clock_settime(CLOCK_MONOTONIC, &ts);
perf_begin(_loop_perf);
sensor_step();
perf_end(_loop_perf);
// Only do lock-step once we received the first actuator output
int sleep_time;
uint64_t current_wall_time_us;
if (_last_actuator_output_time <= 0) {
PX4_DEBUG("SIH starting up - no lockstep yet");
current_wall_time_us = micros();
sleep_time = math::max(0, sim_interval_us - (int)(current_wall_time_us - pre_compute_wall_time_us));
} else {
px4_lockstep_wait_for_components();
current_wall_time_us = micros();
sleep_time = math::max(0, rt_interval_us - (int)(current_wall_time_us - pre_compute_wall_time_us));
}
_achieved_speedup = 0.99f * _achieved_speedup + 0.01f * ((float)sim_interval_us / (float)(
current_wall_time_us - pre_compute_wall_time_us + sleep_time));
usleep(sleep_time);
}
}
#endif
void Sih::realtime_loop()
{
int rate = _imu_gyro_ratemax.get();
@ -96,20 +175,29 @@ bool Sih::init()
// 200 - 2000 Hz
int interval_us = math::constrain(int(roundf(1e6f / rate)), 500, 5000);
ScheduleOnInterval(interval_us);
return true;
}
px4_sem_init(&_data_semaphore, 0, 0);
hrt_call_every(&_timer_call, interval_us, interval_us, timer_callback, &_data_semaphore);
void Sih::Run()
{
if (should_exit()) {
exit_and_cleanup();
return;
while (!should_exit()) {
px4_sem_wait(&_data_semaphore); // periodic real time wakeup
perf_begin(_loop_perf);
sensor_step();
perf_end(_loop_perf);
}
perf_count(_loop_interval_perf);
hrt_cancel(&_timer_call);
px4_sem_destroy(&_data_semaphore);
}
void Sih::timer_callback(void *sem)
{
px4_sem_post((px4_sem_t *)sem);
}
void Sih::sensor_step()
{
// check for parameter updates
if (_parameter_update_sub.updated()) {
// clear update
@ -170,7 +258,7 @@ void Sih::Run()
send_gps();
}
if (_vehicle == VehicleType::FW && _now - _airspeed_time >= 50_ms) {
if ((_vehicle == VehicleType::FW || _vehicle == VehicleType::TS) && _now - _airspeed_time >= 50_ms) {
_airspeed_time = _now;
send_airspeed();
}
@ -284,6 +372,7 @@ void Sih::read_motors()
float pwm_middle = 0.5f * (PWM_DEFAULT_MIN + PWM_DEFAULT_MAX);
if (_actuator_out_sub.update(&actuators_out)) {
_last_actuator_output_time = actuators_out.timestamp;
if (_sys_ctrl_alloc.get()) {
for (int i = 0; i < NB_MOTORS; i++) { // saturate the motor signals
@ -598,8 +687,8 @@ float Sih::generate_wgn() // generate white Gaussian noise sample with std=1
if (phase) {
do {
float U1 = (float)rand() / RAND_MAX;
float U2 = (float)rand() / RAND_MAX;
float U1 = (float)rand() / (float)RAND_MAX;
float U2 = (float)rand() / (float)RAND_MAX;
V1 = 2.0f * U1 - 1.0f;
V2 = 2.0f * U2 - 1.0f;
S = V1 * V1 + V2 * V2;
@ -623,6 +712,11 @@ Vector3f Sih::noiseGauss3f(float stdx, float stdy, float stdz)
int Sih::print_status()
{
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
PX4_INFO("Running in lockstep mode");
PX4_INFO("Achieved speedup: %.2fX", (double)_achieved_speedup);
#endif
if (_vehicle == VehicleType::MC) {
PX4_INFO("Running MultiCopter");
@ -658,27 +752,33 @@ int Sih::print_status()
return 0;
}
int Sih::task_spawn(int argc, char *argv[])
{
_task_id = px4_task_spawn_cmd("sih",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX,
1250,
(px4_main_t)&run_trampoline,
(char *const *)argv);
if (_task_id < 0) {
_task_id = -1;
return -errno;
}
return 0;
}
Sih *Sih::instantiate(int argc, char *argv[])
{
Sih *instance = new Sih();
if (instance) {
_object.store(instance);
_task_id = task_id_is_work_queue;
if (instance->init()) {
return PX4_OK;
}
} else {
if (instance == nullptr) {
PX4_ERR("alloc failed");
}
delete instance;
_object.store(nullptr);
_task_id = -1;
return PX4_ERROR;
return instance;
}
int Sih::custom_command(int argc, char *argv[])

41
src/modules/sih/sih.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2019-2022 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
@ -58,7 +58,6 @@
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <matrix/matrix/math.hpp> // matrix, vectors, dcm, quaterions
#include <conversion/rotation.h> // math::radians,
@ -67,7 +66,7 @@
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <lib/perf/perf_counter.h>
#include <perf/perf_counter.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
@ -81,17 +80,27 @@
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/airspeed.h>
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
#include <sys/time.h>
#endif
using namespace time_literals;
class Sih : public ModuleBase<Sih>, public ModuleParams, public px4::ScheduledWorkItem
extern "C" __EXPORT int sih_main(int argc, char *argv[]);
class Sih : public ModuleBase<Sih>, public ModuleParams
{
public:
Sih();
~Sih() override;
virtual ~Sih();
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static Sih *instantiate(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
@ -101,16 +110,18 @@ public:
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
/** @see ModuleBase::run() */
void run() override;
static float generate_wgn(); // generate white Gaussian noise sample
// generate white Gaussian noise sample as a 3D vector with specified std
static matrix::Vector3f noiseGauss3f(float stdx, float stdy, float stdz);
bool init();
// timer called periodically to post the semaphore
static void timer_callback(void *sem);
private:
void Run() override;
void parameters_updated();
// simulated sensor instances
@ -171,11 +182,23 @@ private:
void publish_sih();
void generate_fw_aerodynamics();
void generate_ts_aerodynamics();
void sensor_step();
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
void lockstep_loop();
uint64_t _current_simulation_time_us{0};
float _achieved_speedup{0.f};
#endif
void realtime_loop();
px4_sem_t _data_semaphore;
hrt_call _timer_call;
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")};
hrt_abstime _last_run{0};
hrt_abstime _last_actuator_output_time{0};
hrt_abstime _baro_time{0};
hrt_abstime _gps_time{0};
hrt_abstime _airspeed_time{0};
@ -275,7 +298,7 @@ private:
// parameters defined in sih_params.c
DEFINE_PARAMETERS(
(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _imu_gyro_ratemax,
(ParamInt<px4::params::IMU_INTEG_RATE>) _imu_integration_rate,
(ParamFloat<px4::params::SIH_MASS>) _sih_mass,
(ParamFloat<px4::params::SIH_IXX>) _sih_ixx,
(ParamFloat<px4::params::SIH_IYY>) _sih_iyy,

2
src/modules/sih/sih_params.c
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2022 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