From 46c9d1e2885eca6e3ea095afcfb6a6583260fd95 Mon Sep 17 00:00:00 2001 From: Thomas Debrunner Date: Thu, 9 Jun 2022 09:52:34 +0200 Subject: [PATCH] SIH in SITL with lockstep (#19028) * sih: Move sih out of work queue This reverts commit bb7dd0cf0016ac1998a79814ec52b15079a74c1a. * 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 Co-authored-by: Matthias Grob --- .../init.d-posix/airframes/10040_quadx | 23 +++ .../init.d-posix/airframes/10041_airplane | 33 ++++ .../init.d-posix/airframes/10042_xvert | 45 +++++ .../init.d-posix/airframes/CMakeLists.txt | 3 + .../px4fmu_common/init.d-posix/px4-rc.mavlink | 9 + ROMFS/px4fmu_common/init.d-posix/rcS | 5 +- Tools/jmavsim_run.sh | 16 +- Tools/sitl_run.sh | 9 + boards/px4/sitl/default.px4board | 1 + platforms/posix/cmake/sitl_target.cmake | 67 ++++++- .../src/lockstep_components.cpp | 2 +- src/modules/sih/CMakeLists.txt | 3 +- src/modules/sih/aero.hpp | 16 +- src/modules/sih/sih.cpp | 164 ++++++++++++++---- src/modules/sih/sih.hpp | 41 ++++- src/modules/sih/sih_params.c | 2 +- 16 files changed, 378 insertions(+), 61 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/10040_quadx create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/10041_airplane create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/10042_xvert diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10040_quadx b/ROMFS/px4fmu_common/init.d-posix/airframes/10040_quadx new file mode 100644 index 0000000000..1586ea7814 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10040_quadx @@ -0,0 +1,23 @@ +#!/bin/sh +# +# @name QuadrotorX SITL for SIH +# +# @type Quadrotor +# +# @maintainer Romain Chiappinelli +# + +. ${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 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10041_airplane b/ROMFS/px4fmu_common/init.d-posix/airframes/10041_airplane new file mode 100644 index 0000000000..e5e33fef9f --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10041_airplane @@ -0,0 +1,33 @@ +#!/bin/sh +# +# @name Plane SITL for SIH +# +# @type Plane +# +# @maintainer Romain Chiappinelli + +. ${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) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10042_xvert b/ROMFS/px4fmu_common/init.d-posix/airframes/10042_xvert new file mode 100644 index 0000000000..7d0d056caa --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10042_xvert @@ -0,0 +1,45 @@ +#!/bin/sh +# +# @name SIH Tailsitter Duo +# +# @type VTOL +# +# @maintainer Romain Chiappinelli + +. ${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 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index 6343f511a2..fba035b61e 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -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 diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink b/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink index 88aa1fe722..0867fa5c64 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink @@ -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 diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index d9e4d76693..25dc9c2fcf 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -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 diff --git a/Tools/jmavsim_run.sh b/Tools/jmavsim_run.sh index 7413e592d6..0fae398286 100755 --- a/Tools/jmavsim_run.sh +++ b/Tools/jmavsim_run.sh @@ -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 diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index d2d9299432..1ae1267b3d 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -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 diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index dd504480d6..fc401133bc 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -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 diff --git a/platforms/posix/cmake/sitl_target.cmake b/platforms/posix/cmake/sitl_target.cmake index bc512f6675..a71e3efbcf 100644 --- a/platforms/posix/cmake/sitl_target.cmake +++ b/platforms/posix/cmake/sitl_target.cmake @@ -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 $ ${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 $ ${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 $ ${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 diff --git a/platforms/posix/src/px4/common/lockstep_scheduler/src/lockstep_components.cpp b/platforms/posix/src/px4/common/lockstep_scheduler/src/lockstep_components.cpp index 2875ae298f..5492f72b28 100644 --- a/platforms/posix/src/px4/common/lockstep_scheduler/src/lockstep_components.cpp +++ b/platforms/posix/src/px4/common/lockstep_scheduler/src/lockstep_components.cpp @@ -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); } diff --git a/src/modules/sih/CMakeLists.txt b/src/modules/sih/CMakeLists.txt index 4608563a44..02bd2d81d7 100644 --- a/src/modules/sih/CMakeLists.txt +++ b/src/modules/sih/CMakeLists.txt @@ -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 ) diff --git a/src/modules/sih/aero.hpp b/src/modules/sih/aero.hpp index e2905e2dc5..498acd9f23 100644 --- a/src/modules/sih/aero.hpp +++ b/src/modules/sih/aero.hpp @@ -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) diff --git a/src/modules/sih/sih.cpp b/src/modules/sih/sih.cpp index 77b3e13819..45137bc722 100644 --- a/src/modules/sih/sih.cpp +++ b/src/modules/sih/sih.cpp @@ -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; +} + +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 -bool Sih::init() +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[]) { - Sih *instance = new Sih(); + _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; + } - if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; + return 0; +} - if (instance->init()) { - return PX4_OK; - } +Sih *Sih::instantiate(int argc, char *argv[]) +{ + Sih *instance = new Sih(); - } 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[]) diff --git a/src/modules/sih/sih.hpp b/src/modules/sih/sih.hpp index 281fdc3e4e..05b11a8014 100644 --- a/src/modules/sih/sih.hpp +++ b/src/modules/sih/sih.hpp @@ -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 #include #include -#include #include // matrix, vectors, dcm, quaterions #include // math::radians, @@ -67,7 +66,7 @@ #include #include #include -#include +#include #include #include #include @@ -81,17 +80,27 @@ #include #include +#if defined(ENABLE_LOCKSTEP_SCHEDULER) +#include +#endif + using namespace time_literals; -class Sih : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +extern "C" __EXPORT int sih_main(int argc, char *argv[]); + +class Sih : public ModuleBase, 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) _imu_gyro_ratemax, - + (ParamInt) _imu_integration_rate, (ParamFloat) _sih_mass, (ParamFloat) _sih_ixx, (ParamFloat) _sih_iyy, diff --git a/src/modules/sih/sih_params.c b/src/modules/sih/sih_params.c index 60619885af..a174d78c34 100644 --- a/src/modules/sih/sih_params.c +++ b/src/modules/sih/sih_params.c @@ -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