diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.gzsim b/ROMFS/px4fmu_common/init.d-posix/px4-rc.gzsim index ee24d195a7..ba32f6284d 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.gzsim +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.gzsim @@ -193,14 +193,16 @@ else exit 1 fi -# NOTE: Only for rover_mecanum and spacecraft_2d. All other models have -# the magnetometer sensor in the model.sdf. -if param compare -s SENS_EN_MAGSIM 1 -then - sensor_mag_sim start -fi -# NOTE: new gz has airspeed sensor, remove once added -if param compare -s SENS_EN_ARSPDSIM 1 -then - sensor_airspeed_sim start -fi +# TODO: + +# # NOTE: Only for rover_mecanum and spacecraft_2d. All other models have +# # the magnetometer sensor in the model.sdf. +# if param compare -s SENS_EN_MAGSIM 1 +# then +# sensor_mag_sim start +# fi +# # NOTE: new gz has airspeed sensor, remove once added +# if param compare -s SENS_EN_ARSPDSIM 1 +# then +# sensor_airspeed_sim start +# fi diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.sihsim b/ROMFS/px4fmu_common/init.d-posix/px4-rc.sihsim index 3e10bd631d..18928de581 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.sihsim +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.sihsim @@ -15,22 +15,7 @@ fi if simulator_sih start; then - if param compare -s SENS_EN_BAROSIM 1 - then - sensor_baro_sim start - fi - if param compare -s SENS_EN_GPSSIM 1 - then - sensor_gps_sim start - fi - if param compare -s SENS_EN_MAGSIM 1 - then - sensor_mag_sim start - fi - if param compare -s SENS_EN_AGPSIM 1 - then - sensor_agp_sim start - fi + sensor_sim_manager start else echo "ERROR [init] simulator_sih failed to start" diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f747966e03..88dcf6c9c7 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -336,10 +336,7 @@ else if param compare SYS_HITL 2 then simulator_sih start - sensor_baro_sim start - sensor_mag_sim start - sensor_gps_sim start - sensor_agp_sim start + sensor_sim_manager start fi else diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index ebe4f4ca5b..176d0f8221 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -55,7 +55,7 @@ CONFIG_COMMON_SIMULATION=y CONFIG_MODULES_SIMULATION_GZ_MSGS=y CONFIG_MODULES_SIMULATION_GZ_BRIDGE=y CONFIG_MODULES_SIMULATION_GZ_PLUGINS=y -CONFIG_MODULES_SIMULATION_SENSOR_AGP_SIM=y +CONFIG_MODULES_SIMULATION_SENSOR_SIM_MANAGER=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_UUV_ATT_CONTROL=y CONFIG_MODULES_UUV_POS_CONTROL=y diff --git a/src/modules/simulation/Kconfig b/src/modules/simulation/Kconfig index 576834846b..eafed00573 100644 --- a/src/modules/simulation/Kconfig +++ b/src/modules/simulation/Kconfig @@ -5,10 +5,7 @@ menu "Simulation" depends on PLATFORM_POSIX select MODULES_SIMULATION_BATTERY_SIMULATOR select MODULES_SIMULATION_PWM_OUT_SIM - select MODULES_SIMULATION_SENSOR_AIRSPEED_SIM - select MODULES_SIMULATION_SENSOR_BARO_SIM - select MODULES_SIMULATION_SENSOR_GPS_SIM - select MODULES_SIMULATION_SENSOR_MAG_SIM + select MODULES_SIMULATION_SENSOR_SIM_MANAGER select MODULES_SIMULATION_SYSTEM_POWER_SIMULATOR select MODULES_SIMULATION_SIMULATOR_MAVLINK select MODULES_SIMULATION_SIMULATOR_SIH diff --git a/src/modules/simulation/sensor_agp_sim/CMakeLists.txt b/src/modules/simulation/sensor_agp_sim/CMakeLists.txt deleted file mode 100644 index e1390068eb..0000000000 --- a/src/modules/simulation/sensor_agp_sim/CMakeLists.txt +++ /dev/null @@ -1,43 +0,0 @@ -############################################################################ -# -# Copyright (c) 2024 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 -# 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__simulation__sensor_agp_sim - MAIN sensor_agp_sim - COMPILE_FLAGS - SRCS - SensorAgpSim.cpp - SensorAgpSim.hpp - DEPENDS - px4_work_queue - ) diff --git a/src/modules/simulation/sensor_agp_sim/Kconfig b/src/modules/simulation/sensor_agp_sim/Kconfig deleted file mode 100644 index 43c6885246..0000000000 --- a/src/modules/simulation/sensor_agp_sim/Kconfig +++ /dev/null @@ -1,5 +0,0 @@ -menuconfig MODULES_SIMULATION_SENSOR_AGP_SIM - bool "sensor_agp_sim" - default n - ---help--- - Enable support for sensor_agp_sim diff --git a/src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp b/src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp deleted file mode 100644 index 4de5d46c2d..0000000000 --- a/src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp +++ /dev/null @@ -1,202 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2024 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 - * 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. - * - ****************************************************************************/ - -#include "SensorAgpSim.hpp" - -#include -#include -#include - -using namespace matrix; - -SensorAgpSim::SensorAgpSim() : - ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) -{ -} - -SensorAgpSim::~SensorAgpSim() -{ - perf_free(_loop_perf); -} - -bool SensorAgpSim::init() -{ - ScheduleOnInterval(500_ms); // 2 Hz - return true; -} - -float SensorAgpSim::generate_wgn() -{ - // generate white Gaussian noise sample with std=1 - // from BlockRandGauss.hpp - static float V1, V2, S; - static bool phase = true; - float X; - - if (phase) { - do { - 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; - } while (S >= 1.0f || fabsf(S) < 1e-8f); - - X = V1 * float(sqrtf(-2.0f * float(logf(S)) / S)); - - } else { - X = V2 * float(sqrtf(-2.0f * float(logf(S)) / S)); - } - - phase = !phase; - return X; -} - -void SensorAgpSim::Run() -{ - if (should_exit()) { - ScheduleClear(); - exit_and_cleanup(); - return; - } - - perf_begin(_loop_perf); - - // Check if parameters have changed - if (_parameter_update_sub.updated()) { - // clear update - parameter_update_s param_update; - _parameter_update_sub.copy(¶m_update); - - updateParams(); - } - - if (_vehicle_global_position_sub.updated()) { - - vehicle_global_position_s gpos{}; - _vehicle_global_position_sub.copy(&gpos); - - const uint64_t now = gpos.timestamp; - const float dt = (now - _time_last_update) * 1e-6f; - _time_last_update = now; - - if (!(_param_sim_agp_fail.get() & static_cast(FailureMode::Stuck))) { - _measured_lla = LatLonAlt(gpos.lat, gpos.lon, gpos.alt_ellipsoid); - } - - if (_param_sim_agp_fail.get() & static_cast(FailureMode::Drift)) { - _position_bias += Vector3f(1.5f, -5.f, 0.f) * dt; - _measured_lla += _position_bias; - - } else { - _position_bias.zero(); - } - - const double latitude = _measured_lla.latitude_deg() + math::degrees((double)generate_wgn() * 2.0 / - CONSTANTS_RADIUS_OF_EARTH); - const double longitude = _measured_lla.longitude_deg() + math::degrees((double)generate_wgn() * 2.0 / - CONSTANTS_RADIUS_OF_EARTH); - const double altitude = (double)(_measured_lla.altitude() + (generate_wgn() * 0.5f)); - - vehicle_global_position_s sample{}; - - sample.timestamp_sample = gpos.timestamp_sample; - sample.lat = latitude; - sample.lon = longitude; - sample.alt = altitude; - sample.lat_lon_valid = true; - sample.alt_ellipsoid = altitude; - sample.alt_valid = true; - sample.eph = 20.f; - sample.epv = 5.f; - - sample.timestamp = hrt_absolute_time(); - _aux_global_position_pub.publish(sample); - } - - perf_end(_loop_perf); -} - -int SensorAgpSim::task_spawn(int argc, char *argv[]) -{ - SensorAgpSim *instance = new SensorAgpSim(); - - if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; - - if (instance->init()) { - return PX4_OK; - } - - } else { - PX4_ERR("alloc failed"); - } - - delete instance; - _object.store(nullptr); - _task_id = -1; - - return PX4_ERROR; -} - -int SensorAgpSim::custom_command(int argc, char *argv[]) -{ - return print_usage("unknown command"); -} - -int SensorAgpSim::print_usage(const char *reason) -{ - if (reason) { - PX4_WARN("%s\n", reason); - } - - PRINT_MODULE_DESCRIPTION( - R"DESCR_STR( -### Description -Module to simulate auxiliary global position measurements with optional failure modes for SIH simulation. - -)DESCR_STR"); - - PRINT_MODULE_USAGE_NAME("sensor_agp_sim", "system"); - PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); - - return 0; -} - -extern "C" __EXPORT int sensor_agp_sim_main(int argc, char *argv[]) -{ - return SensorAgpSim::main(argc, argv); -} diff --git a/src/modules/simulation/sensor_agp_sim/SensorAgpSim.hpp b/src/modules/simulation/sensor_agp_sim/SensorAgpSim.hpp deleted file mode 100644 index 9700780adb..0000000000 --- a/src/modules/simulation/sensor_agp_sim/SensorAgpSim.hpp +++ /dev/null @@ -1,93 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2024 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 - * 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. - * - ****************************************************************************/ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace time_literals; - -class SensorAgpSim : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem -{ -public: - SensorAgpSim(); - ~SensorAgpSim() override; - - /** @see ModuleBase */ - static int task_spawn(int argc, char *argv[]); - - /** @see ModuleBase */ - static int custom_command(int argc, char *argv[]); - - /** @see ModuleBase */ - static int print_usage(const char *reason = nullptr); - - bool init(); - -private: - enum class FailureMode : int32_t { - Stuck = (1 << 0), - Drift = (1 << 1) - }; - - void Run() override; - - // generate white Gaussian noise sample with std=1 - static float generate_wgn(); - - LatLonAlt _measured_lla{}; - matrix::Vector3f _position_bias{}; - - uint64_t _time_last_update{}; - - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)}; - - uORB::PublicationMulti _aux_global_position_pub{ORB_ID(aux_global_position)}; - - perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; - - DEFINE_PARAMETERS( - (ParamInt) _param_sim_agp_fail - ) -}; diff --git a/src/modules/simulation/sensor_agp_sim/parameters.c b/src/modules/simulation/sensor_agp_sim/parameters.c deleted file mode 100644 index 6690255f09..0000000000 --- a/src/modules/simulation/sensor_agp_sim/parameters.c +++ /dev/null @@ -1,57 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2024 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 - * 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. - * - ****************************************************************************/ -/** - * Simulate Aux Global Position (AGP) - * - * @reboot_required true - * @min 0 - * @max 1 - * @group Sensors - * @value 0 Disabled - * @value 1 Enabled - */ -PARAM_DEFINE_INT32(SENS_EN_AGPSIM, 0); - -/** - * AGP failure mode - * - * Stuck: freeze the measurement to the current location - * Drift: add a linearly growing bias to the sensor data - * - * @group Simulator - * @min 0 - * @max 3 - * @bit 0 Stuck - * @bit 1 Drift - */ -PARAM_DEFINE_INT32(SIM_AGP_FAIL, 0); diff --git a/src/modules/simulation/sensor_airspeed_sim/CMakeLists.txt b/src/modules/simulation/sensor_airspeed_sim/CMakeLists.txt deleted file mode 100644 index d993fe49af..0000000000 --- a/src/modules/simulation/sensor_airspeed_sim/CMakeLists.txt +++ /dev/null @@ -1,43 +0,0 @@ -############################################################################ -# -# Copyright (c) 2021-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 -# 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__simulation__sensor_airspeed_sim - MAIN sensor_airspeed_sim - COMPILE_FLAGS - SRCS - SensorAirspeedSim.cpp - SensorAirspeedSim.hpp - DEPENDS - px4_work_queue - ) diff --git a/src/modules/simulation/sensor_airspeed_sim/Kconfig b/src/modules/simulation/sensor_airspeed_sim/Kconfig deleted file mode 100644 index c485cb4676..0000000000 --- a/src/modules/simulation/sensor_airspeed_sim/Kconfig +++ /dev/null @@ -1,5 +0,0 @@ -menuconfig MODULES_SIMULATION_SENSOR_AIRSPEED_SIM - bool "sensor_airspeed_sim" - default n - ---help--- - Enable support for sensor_airspeed_sim diff --git a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp deleted file mode 100644 index 5566efbb1c..0000000000 --- a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp +++ /dev/null @@ -1,208 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2023 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 - * 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. - * - ****************************************************************************/ - -#include "SensorAirspeedSim.hpp" - -#include -#include -#include - -using namespace matrix; - -SensorAirspeedSim::SensorAirspeedSim() : - ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) -{ -} - -SensorAirspeedSim::~SensorAirspeedSim() -{ - perf_free(_loop_perf); -} - -bool SensorAirspeedSim::init() -{ - ScheduleOnInterval(125_ms); // 8 Hz - return true; -} - -float SensorAirspeedSim::generate_wgn() -{ - // generate white Gaussian noise sample with std=1 - - // algorithm 1: - // float temp=((float)(rand()+1))/(((float)RAND_MAX+1.0f)); - // return sqrtf(-2.0f*logf(temp))*cosf(2.0f*M_PI_F*rand()/RAND_MAX); - // algorithm 2: from BlockRandGauss.hpp - static float V1, V2, S; - static bool phase = true; - float X; - - if (phase) { - do { - 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; - } while (S >= 1.0f || fabsf(S) < 1e-8f); - - X = V1 * float(sqrtf(-2.0f * float(logf(S)) / S)); - - } else { - X = V2 * float(sqrtf(-2.0f * float(logf(S)) / S)); - } - - phase = !phase; - return X; -} - -void SensorAirspeedSim::Run() -{ - if (should_exit()) { - ScheduleClear(); - exit_and_cleanup(); - return; - } - - perf_begin(_loop_perf); - - // Check if parameters have changed - if (_parameter_update_sub.updated()) { - // clear update - parameter_update_s param_update; - _parameter_update_sub.copy(¶m_update); - - updateParams(); - } - - if (_sim_failure.get() == 0) { - if (_vehicle_local_position_sub.updated() && _vehicle_global_position_sub.updated() - && _vehicle_attitude_sub.updated()) { - - vehicle_local_position_s lpos{}; - _vehicle_local_position_sub.copy(&lpos); - - vehicle_global_position_s gpos{}; - _vehicle_global_position_sub.copy(&gpos); - - vehicle_attitude_s attitude{}; - _vehicle_attitude_sub.copy(&attitude); - - Vector3f local_velocity = Vector3f{lpos.vx, lpos.vy, lpos.vz}; - Vector3f body_velocity = Dcmf{Quatf{attitude.q}} .transpose() * local_velocity; - - // device id - device::Device::DeviceId device_id; - device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; - device_id.devid_s.bus = 0; - device_id.devid_s.address = 0; - device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_SIM; - - const float alt_amsl = gpos.alt; - const float temperature_local = TEMPERATURE_MSL - LAPSE_RATE * alt_amsl; - const float density_ratio = powf(TEMPERATURE_MSL / temperature_local, 4.256f); - const float air_density = AIR_DENSITY_MSL / density_ratio; - - // calculate differential pressure + noise in hPa - const float diff_pressure_noise = (float)generate_wgn() * 0.01f; - float diff_pressure = sign(body_velocity(0)) * 0.005f * air_density * body_velocity(0) * body_velocity( - 0) + diff_pressure_noise; - - - differential_pressure_s differential_pressure{}; - // report.timestamp_sample = time; - differential_pressure.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION - differential_pressure.differential_pressure_pa = (double)diff_pressure * 100.0; // hPa to Pa; - differential_pressure.temperature = temperature_local + ABSOLUTE_ZERO_C; // K to C - differential_pressure.timestamp = hrt_absolute_time(); - _differential_pressure_pub.publish(differential_pressure); - - } - } - - perf_end(_loop_perf); -} - -int SensorAirspeedSim::task_spawn(int argc, char *argv[]) -{ - SensorAirspeedSim *instance = new SensorAirspeedSim(); - - if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; - - if (instance->init()) { - return PX4_OK; - } - - } else { - PX4_ERR("alloc failed"); - } - - delete instance; - _object.store(nullptr); - _task_id = -1; - - return PX4_ERROR; -} - -int SensorAirspeedSim::custom_command(int argc, char *argv[]) -{ - return print_usage("unknown command"); -} - -int SensorAirspeedSim::print_usage(const char *reason) -{ - if (reason) { - PX4_WARN("%s\n", reason); - } - - PRINT_MODULE_DESCRIPTION( - R"DESCR_STR( -### Description - - -)DESCR_STR"); - - PRINT_MODULE_USAGE_NAME("sensor_arispeed_sim", "system"); - PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); - - return 0; -} - -extern "C" __EXPORT int sensor_airspeed_sim_main(int argc, char *argv[]) -{ - return SensorAirspeedSim::main(argc, argv); -} diff --git a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp deleted file mode 100644 index 3aafc9ca93..0000000000 --- a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp +++ /dev/null @@ -1,96 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2023 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 - * 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. - * - ****************************************************************************/ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace time_literals; - -static constexpr float ABSOLUTE_ZERO_C = -273.15; // absolute 0 temperature [C] -static constexpr float TEMPERATURE_MSL = 288.15; // temperature at MSL [K] (15 [C]) -static constexpr float PRESSURE_MSL = 101325.0; // pressure at MSL [Pa] -static constexpr float LAPSE_RATE = 0.0065; // reduction in temperature with altitude for troposphere [K/m] -static constexpr float AIR_DENSITY_MSL = 1.225; // air density at MSL [kg/m^3] - -class SensorAirspeedSim : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem -{ -public: - SensorAirspeedSim(); - ~SensorAirspeedSim() override; - - /** @see ModuleBase */ - static int task_spawn(int argc, char *argv[]); - - /** @see ModuleBase */ - static int custom_command(int argc, char *argv[]); - - /** @see ModuleBase */ - static int print_usage(const char *reason = nullptr); - - bool init(); - -private: - void Run() override; - - // generate white Gaussian noise sample with std=1 - static float generate_wgn(); - - // generate white Gaussian noise sample as a 3D vector with specified std - matrix::Vector3f noiseGauss3f(float stdx, float stdy, float stdz) { return matrix::Vector3f(generate_wgn() * stdx, generate_wgn() * stdy, generate_wgn() * stdz); } - - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; - uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)}; - uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position_groundtruth)}; - - uORB::PublicationMulti _differential_pressure_pub{ORB_ID(differential_pressure)}; - - perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; - - DEFINE_PARAMETERS( - (ParamInt) _sim_failure - ) -}; diff --git a/src/modules/simulation/sensor_airspeed_sim/parameters.c b/src/modules/simulation/sensor_airspeed_sim/parameters.c deleted file mode 100644 index 99b1b64f7f..0000000000 --- a/src/modules/simulation/sensor_airspeed_sim/parameters.c +++ /dev/null @@ -1,56 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2023 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 - * 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. - * - ****************************************************************************/ - -/** - * Enable simulated airspeed sensor instance - * - * @reboot_required true - * @min 0 - * @max 1 - * @group Sensors - * @value 0 Disabled - * @value 1 Enabled - */ -PARAM_DEFINE_INT32(SENS_EN_ARSPDSIM, 0); - -/** - * Dynamically simulate failure of airspeed sensor instance - * - * @reboot_required true - * @min 0 - * @max 1 - * @group Sensors - * @value 0 Disabled - * @value 1 Enabled - */ -PARAM_DEFINE_INT32(SIM_ARSPD_FAIL, 0); diff --git a/src/modules/simulation/sensor_baro_sim/CMakeLists.txt b/src/modules/simulation/sensor_baro_sim/CMakeLists.txt deleted file mode 100644 index 2a21973698..0000000000 --- a/src/modules/simulation/sensor_baro_sim/CMakeLists.txt +++ /dev/null @@ -1,44 +0,0 @@ -############################################################################ -# -# Copyright (c) 2021-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 -# 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__simulation__sensor_baro_sim - MAIN sensor_baro_sim - COMPILE_FLAGS - SRCS - SensorBaroSim.cpp - SensorBaroSim.hpp - DEPENDS - geo - px4_work_queue - ) diff --git a/src/modules/simulation/sensor_baro_sim/Kconfig b/src/modules/simulation/sensor_baro_sim/Kconfig deleted file mode 100644 index 59898c31a8..0000000000 --- a/src/modules/simulation/sensor_baro_sim/Kconfig +++ /dev/null @@ -1,5 +0,0 @@ -menuconfig MODULES_SIMULATION_SENSOR_BARO_SIM - bool "sensor_baro_sim" - default n - ---help--- - Enable support for sensor_baro_sim diff --git a/src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp b/src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp deleted file mode 100644 index fc70998e9a..0000000000 --- a/src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp +++ /dev/null @@ -1,232 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2021 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 - * 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. - * - ****************************************************************************/ - -#include "SensorBaroSim.hpp" - -#include - -using namespace matrix; - -SensorBaroSim::SensorBaroSim() : - ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) -{ - srand(1234); // initialize the random seed once before calling generate_wgn() -} - -SensorBaroSim::~SensorBaroSim() -{ - perf_free(_loop_perf); -} - -bool SensorBaroSim::init() -{ - ScheduleOnInterval(50_ms); // 20 Hz - return true; -} - -float SensorBaroSim::generate_wgn() -{ - // generate white Gaussian noise sample with std=1 - - // algorithm 1: - // float temp=((float)(rand()+1))/(((float)RAND_MAX+1.0f)); - // return sqrtf(-2.0f*logf(temp))*cosf(2.0f*M_PI_F*rand()/RAND_MAX); - // algorithm 2: from BlockRandGauss.hpp - static float V1, V2, S; - static bool phase = true; - float X; - - if (phase) { - do { - 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; - } while (S >= 1.0f || fabsf(S) < 1e-8f); - - X = V1 * float(sqrtf(-2.0f * float(logf(S)) / S)); - - } else { - X = V2 * float(sqrtf(-2.0f * float(logf(S)) / S)); - } - - phase = !phase; - return X; -} - -void SensorBaroSim::Run() -{ - if (should_exit()) { - ScheduleClear(); - exit_and_cleanup(); - return; - } - - perf_begin(_loop_perf); - - // Check if parameters have changed - if (_parameter_update_sub.updated()) { - // clear update - parameter_update_s param_update; - _parameter_update_sub.copy(¶m_update); - - updateParams(); - } - - if (_vehicle_global_position_sub.updated()) { - vehicle_global_position_s gpos; - - if (_vehicle_global_position_sub.copy(&gpos)) { - - const float dt = math::constrain((gpos.timestamp - _last_update_time) * 1e-6f, 0.001f, 0.1f); - - const float alt_msl = gpos.alt; - - // calculate abs_pressure using an ISA model for the tropsphere (valid up to 11km above MSL) - const float lapse_rate = 0.0065f; // reduction in temperature with altitude (Kelvin/m) - const float temperature_msl = 288.0f; // temperature at MSL (Kelvin) - - const float temperature_local = temperature_msl - lapse_rate * alt_msl; - const float pressure_ratio = powf(temperature_msl / temperature_local, 5.256f); - const float pressure_msl = 101325.0f; // pressure at MSL - const float absolute_pressure = pressure_msl / pressure_ratio; - - // generate Gaussian noise sequence using polar form of Box-Muller transformation - double y1; - { - double x1; - double x2; - double w; - - if (!_baro_rnd_use_last) { - do { - x1 = 2. * (double)generate_wgn() - 1.; - x2 = 2. * (double)generate_wgn() - 1.; - w = x1 * x1 + x2 * x2; - } while (w >= 1.0); - - w = sqrt((-2.0 * log(w)) / w); - // calculate two values - the second value can be used next time because it is uncorrelated - y1 = x1 * w; - _baro_rnd_y2 = x2 * w; - _baro_rnd_use_last = true; - - } else { - // no need to repeat the calculation - use the second value from last update - y1 = _baro_rnd_y2; - _baro_rnd_use_last = false; - } - } - - // Apply noise and drift - const float abs_pressure_noise = 1.f * (float)y1; // 1 Pa RMS noise - _baro_drift_pa += _baro_drift_pa_per_sec * dt; - const float absolute_pressure_noisy = absolute_pressure + abs_pressure_noise + _baro_drift_pa; - - // convert to hPa - float pressure = absolute_pressure_noisy + _sim_baro_off_p.get(); - - // calculate temperature in Celsius - float temperature = temperature_local - 273.0f + _sim_baro_off_t.get(); - - // publish - sensor_baro_s sensor_baro{}; - sensor_baro.timestamp_sample = gpos.timestamp; - sensor_baro.device_id = 6620172; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION - sensor_baro.pressure = pressure; - sensor_baro.temperature = temperature; - sensor_baro.timestamp = hrt_absolute_time(); - _sensor_baro_pub.publish(sensor_baro); - - - _last_update_time = gpos.timestamp; - } - } - - perf_end(_loop_perf); -} - -int SensorBaroSim::task_spawn(int argc, char *argv[]) -{ - SensorBaroSim *instance = new SensorBaroSim(); - - if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; - - if (instance->init()) { - return PX4_OK; - } - - } else { - PX4_ERR("alloc failed"); - } - - delete instance; - _object.store(nullptr); - _task_id = -1; - - return PX4_ERROR; -} - -int SensorBaroSim::custom_command(int argc, char *argv[]) -{ - return print_usage("unknown command"); -} - -int SensorBaroSim::print_usage(const char *reason) -{ - if (reason) { - PX4_WARN("%s\n", reason); - } - - PRINT_MODULE_DESCRIPTION( - R"DESCR_STR( -### Description - - -)DESCR_STR"); - - PRINT_MODULE_USAGE_NAME("sensor_baro_sim", "system"); - PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); - - return 0; -} - -extern "C" __EXPORT int sensor_baro_sim_main(int argc, char *argv[]) -{ - return SensorBaroSim::main(argc, argv); -} diff --git a/src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp b/src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp deleted file mode 100644 index d103845378..0000000000 --- a/src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp +++ /dev/null @@ -1,92 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2021 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 - * 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. - * - ****************************************************************************/ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace time_literals; - -class SensorBaroSim : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem -{ -public: - SensorBaroSim(); - ~SensorBaroSim() override; - - /** @see ModuleBase */ - static int task_spawn(int argc, char *argv[]); - - /** @see ModuleBase */ - static int custom_command(int argc, char *argv[]); - - /** @see ModuleBase */ - static int print_usage(const char *reason = nullptr); - - bool init(); - -private: - void Run() override; - - // generate white Gaussian noise sample with std=1 - static float generate_wgn(); - - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)}; - - bool _baro_rnd_use_last{false}; - double _baro_rnd_y2{0.0}; - float _baro_drift_pa_per_sec{0.0}; - float _baro_drift_pa{0.0}; - - hrt_abstime _last_update_time{0}; - - uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; - - perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; - - DEFINE_PARAMETERS( - (ParamFloat) _sim_baro_off_p, - (ParamFloat) _sim_baro_off_t - ) -}; diff --git a/src/modules/simulation/sensor_baro_sim/parameters.c b/src/modules/simulation/sensor_baro_sim/parameters.c deleted file mode 100644 index 33adf72019..0000000000 --- a/src/modules/simulation/sensor_baro_sim/parameters.c +++ /dev/null @@ -1,59 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2021 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 - * 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. - * - ****************************************************************************/ - -/** - * Enable simulated barometer sensor instance - * - * @reboot_required true - * @min 0 - * @max 1 - * @group Sensors - * @value 0 Disabled - * @value 1 Enabled - */ -PARAM_DEFINE_INT32(SENS_EN_BAROSIM, 0); - -/** - * simulated barometer pressure offset - * - * @group Simulator - */ -PARAM_DEFINE_FLOAT(SIM_BARO_OFF_P, 0.0f); - -/** - * simulated barometer temperature offset - * - * @group Simulator - * @unit celcius - */ -PARAM_DEFINE_FLOAT(SIM_BARO_OFF_T, 0.0f); diff --git a/src/modules/simulation/sensor_gps_sim/CMakeLists.txt b/src/modules/simulation/sensor_gps_sim/CMakeLists.txt deleted file mode 100644 index 46e62f79ff..0000000000 --- a/src/modules/simulation/sensor_gps_sim/CMakeLists.txt +++ /dev/null @@ -1,43 +0,0 @@ -############################################################################ -# -# Copyright (c) 2021-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 -# 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__simulation__sensor_gps_sim - MAIN sensor_gps_sim - COMPILE_FLAGS - SRCS - SensorGpsSim.cpp - SensorGpsSim.hpp - DEPENDS - px4_work_queue - ) diff --git a/src/modules/simulation/sensor_gps_sim/Kconfig b/src/modules/simulation/sensor_gps_sim/Kconfig deleted file mode 100644 index 621429fa75..0000000000 --- a/src/modules/simulation/sensor_gps_sim/Kconfig +++ /dev/null @@ -1,5 +0,0 @@ -menuconfig MODULES_SIMULATION_SENSOR_GPS_SIM - bool "sensor_gps_sim" - default n - ---help--- - Enable support for sensor_gps_sim diff --git a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp deleted file mode 100644 index 5641706345..0000000000 --- a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp +++ /dev/null @@ -1,236 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2021 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 - * 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. - * - ****************************************************************************/ - -#include "SensorGpsSim.hpp" - -#include -#include -#include - -using namespace matrix; - -SensorGpsSim::SensorGpsSim() : - ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) -{ -} - -SensorGpsSim::~SensorGpsSim() -{ - perf_free(_loop_perf); -} - -bool SensorGpsSim::init() -{ - ScheduleOnInterval(125_ms); // 8 Hz - return true; -} - -float SensorGpsSim::generate_wgn() -{ - // generate white Gaussian noise sample with std=1 - - // algorithm 1: - // float temp=((float)(rand()+1))/(((float)RAND_MAX+1.0f)); - // return sqrtf(-2.0f*logf(temp))*cosf(2.0f*M_PI_F*rand()/RAND_MAX); - // algorithm 2: from BlockRandGauss.hpp - static float V1, V2, S; - static bool phase = true; - float X; - - if (phase) { - do { - 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; - } while (S >= 1.0f || fabsf(S) < 1e-8f); - - X = V1 * float(sqrtf(-2.0f * float(logf(S)) / S)); - - } else { - X = V2 * float(sqrtf(-2.0f * float(logf(S)) / S)); - } - - phase = !phase; - return X; -} - -void SensorGpsSim::Run() -{ - if (should_exit()) { - ScheduleClear(); - exit_and_cleanup(); - return; - } - - perf_begin(_loop_perf); - - // Check if parameters have changed - if (_parameter_update_sub.updated()) { - // clear update - parameter_update_s param_update; - _parameter_update_sub.copy(¶m_update); - - updateParams(); - } - - if (_vehicle_local_position_sub.updated() && _vehicle_global_position_sub.updated()) { - - vehicle_local_position_s lpos{}; - _vehicle_local_position_sub.copy(&lpos); - - vehicle_global_position_s gpos{}; - _vehicle_global_position_sub.copy(&gpos); - - double latitude = gpos.lat + math::degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH); - double longitude = gpos.lon + math::degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH); - double altitude = (double)(gpos.alt + (generate_wgn() * 0.5f)); - - Vector3f gps_vel = Vector3f{lpos.vx, lpos.vy, lpos.vz} + noiseGauss3f(0.06f, 0.077f, 0.158f); - - // device id - device::Device::DeviceId device_id; - device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; - device_id.devid_s.bus = 0; - device_id.devid_s.address = 0; - device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM; - - sensor_gps_s sensor_gps{}; - - if (_sim_gps_used.get() >= 4) { - // fix - sensor_gps.fix_type = 3; // 3D fix - sensor_gps.s_variance_m_s = 0.4f; - sensor_gps.c_variance_rad = 0.1f; - sensor_gps.eph = 0.9f; - sensor_gps.epv = 1.78f; - sensor_gps.hdop = 0.7f; - sensor_gps.vdop = 1.1f; - - } else { - // no fix - sensor_gps.fix_type = 0; // No fix - sensor_gps.s_variance_m_s = 100.f; - sensor_gps.c_variance_rad = 100.f; - sensor_gps.eph = 100.f; - sensor_gps.epv = 100.f; - sensor_gps.hdop = 100.f; - sensor_gps.vdop = 100.f; - } - - sensor_gps.timestamp_sample = gpos.timestamp_sample; - sensor_gps.time_utc_usec = 0; - sensor_gps.device_id = device_id.devid; - sensor_gps.latitude_deg = latitude; // Latitude in degrees - sensor_gps.longitude_deg = longitude; // Longitude in degrees - sensor_gps.altitude_msl_m = altitude; // Altitude in meters above MSL - sensor_gps.altitude_ellipsoid_m = altitude; - sensor_gps.noise_per_ms = 0; - sensor_gps.jamming_indicator = 0; - sensor_gps.vel_m_s = sqrtf(gps_vel(0) * gps_vel(0) + gps_vel(1) * gps_vel(1)); // GPS ground speed, (metres/sec) - sensor_gps.vel_n_m_s = gps_vel(0); - sensor_gps.vel_e_m_s = gps_vel(1); - sensor_gps.vel_d_m_s = gps_vel(2); - sensor_gps.cog_rad = atan2(gps_vel(1), - gps_vel(0)); // Course over ground (NOT heading, but direction of movement), -PI..PI, (radians) - sensor_gps.timestamp_time_relative = 0; - sensor_gps.heading = NAN; - sensor_gps.heading_offset = NAN; - sensor_gps.heading_accuracy = 0; - sensor_gps.automatic_gain_control = 0; - sensor_gps.jamming_state = 0; - sensor_gps.spoofing_state = 0; - sensor_gps.vel_ned_valid = true; - sensor_gps.satellites_used = _sim_gps_used.get(); - - sensor_gps.timestamp = hrt_absolute_time(); - _sensor_gps_pub.publish(sensor_gps); - } - - perf_end(_loop_perf); -} - -int SensorGpsSim::task_spawn(int argc, char *argv[]) -{ - SensorGpsSim *instance = new SensorGpsSim(); - - if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; - - if (instance->init()) { - return PX4_OK; - } - - } else { - PX4_ERR("alloc failed"); - } - - delete instance; - _object.store(nullptr); - _task_id = -1; - - return PX4_ERROR; -} - -int SensorGpsSim::custom_command(int argc, char *argv[]) -{ - return print_usage("unknown command"); -} - -int SensorGpsSim::print_usage(const char *reason) -{ - if (reason) { - PX4_WARN("%s\n", reason); - } - - PRINT_MODULE_DESCRIPTION( - R"DESCR_STR( -### Description - - -)DESCR_STR"); - - PRINT_MODULE_USAGE_NAME("sensor_gps_sim", "system"); - PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); - - return 0; -} - -extern "C" __EXPORT int sensor_gps_sim_main(int argc, char *argv[]) -{ - return SensorGpsSim::main(argc, argv); -} diff --git a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp deleted file mode 100644 index fac4e5201e..0000000000 --- a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp +++ /dev/null @@ -1,88 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2021 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 - * 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. - * - ****************************************************************************/ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace time_literals; - -class SensorGpsSim : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem -{ -public: - SensorGpsSim(); - ~SensorGpsSim() override; - - /** @see ModuleBase */ - static int task_spawn(int argc, char *argv[]); - - /** @see ModuleBase */ - static int custom_command(int argc, char *argv[]); - - /** @see ModuleBase */ - static int print_usage(const char *reason = nullptr); - - bool init(); - -private: - void Run() override; - - // generate white Gaussian noise sample with std=1 - static float generate_wgn(); - - // generate white Gaussian noise sample as a 3D vector with specified std - matrix::Vector3f noiseGauss3f(float stdx, float stdy, float stdz) { return matrix::Vector3f(generate_wgn() * stdx, generate_wgn() * stdy, generate_wgn() * stdz); } - - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)}; - uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position_groundtruth)}; - - uORB::PublicationMulti _sensor_gps_pub{ORB_ID(sensor_gps)}; - - perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; - - DEFINE_PARAMETERS( - (ParamInt) _sim_gps_used - ) -}; diff --git a/src/modules/simulation/sensor_gps_sim/parameters.c b/src/modules/simulation/sensor_gps_sim/parameters.c deleted file mode 100644 index c9d9c74555..0000000000 --- a/src/modules/simulation/sensor_gps_sim/parameters.c +++ /dev/null @@ -1,52 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2021 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 - * 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. - * - ****************************************************************************/ -/** - * Enable simulated GPS sinstance - * - * @reboot_required true - * @min 0 - * @max 1 - * @group Sensors - * @value 0 Disabled - * @value 1 Enabled - */ -PARAM_DEFINE_INT32(SENS_EN_GPSSIM, 0); - -/** - * simulated GPS number of satellites used - * - * @min 0 - * @max 50 - * @group Simulator - */ -PARAM_DEFINE_INT32(SIM_GPS_USED, 10); diff --git a/src/modules/simulation/sensor_mag_sim/Kconfig b/src/modules/simulation/sensor_mag_sim/Kconfig deleted file mode 100644 index b1e0e6dd82..0000000000 --- a/src/modules/simulation/sensor_mag_sim/Kconfig +++ /dev/null @@ -1,5 +0,0 @@ -menuconfig MODULES_SIMULATION_SENSOR_MAG_SIM - bool "sensor_mag_sim" - default n - ---help--- - Enable support for sensor_mag_sim diff --git a/src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp b/src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp deleted file mode 100644 index 97c2626f75..0000000000 --- a/src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp +++ /dev/null @@ -1,196 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2021 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 - * 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. - * - ****************************************************************************/ - -#include "SensorMagSim.hpp" - -#include -#include - -using namespace matrix; - -SensorMagSim::SensorMagSim() : - ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) -{ - _px4_mag.set_device_type(DRV_MAG_DEVTYPE_MAGSIM); -} - -SensorMagSim::~SensorMagSim() -{ - perf_free(_loop_perf); -} - -bool SensorMagSim::init() -{ - ScheduleOnInterval(20_ms); // 50 Hz - return true; -} - -float SensorMagSim::generate_wgn() -{ - // generate white Gaussian noise sample with std=1 - - // algorithm 1: - // float temp=((float)(rand()+1))/(((float)RAND_MAX+1.0f)); - // return sqrtf(-2.0f*logf(temp))*cosf(2.0f*M_PI_F*rand()/RAND_MAX); - // algorithm 2: from BlockRandGauss.hpp - static float V1, V2, S; - static bool phase = true; - float X; - - if (phase) { - do { - 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; - } while (S >= 1.0f || fabsf(S) < 1e-8f); - - X = V1 * float(sqrtf(-2.0f * float(logf(S)) / S)); - - } else { - X = V2 * float(sqrtf(-2.0f * float(logf(S)) / S)); - } - - phase = !phase; - return X; -} - -void SensorMagSim::Run() -{ - if (should_exit()) { - ScheduleClear(); - exit_and_cleanup(); - return; - } - - perf_begin(_loop_perf); - - // Check if parameters have changed - if (_parameter_update_sub.updated()) { - // clear update - parameter_update_s param_update; - _parameter_update_sub.copy(¶m_update); - - updateParams(); - } - - if (_vehicle_global_position_sub.updated()) { - vehicle_global_position_s gpos; - - if (_vehicle_global_position_sub.copy(&gpos)) { - if (gpos.eph < 1000) { - - // magnetic field data returned by the geo library using the current GPS position - const float declination_rad = math::radians(get_mag_declination_degrees(gpos.lat, gpos.lon)); - const float inclination_rad = math::radians(get_mag_inclination_degrees(gpos.lat, gpos.lon)); - const float field_strength_gauss = get_mag_strength_gauss(gpos.lat, gpos.lon); - - _mag_earth_pred = Dcmf(Eulerf(0, -inclination_rad, declination_rad)) * Vector3f(field_strength_gauss, 0, 0); - - _mag_earth_available = true; - } - } - } - - if (_mag_earth_available) { - vehicle_attitude_s attitude; - - if (_vehicle_attitude_sub.update(&attitude)) { - Vector3f expected_field = Dcmf{Quatf{attitude.q}} .transpose() * _mag_earth_pred; - - expected_field += noiseGauss3f(0.02f, 0.02f, 0.03f); - - _px4_mag.update(attitude.timestamp, - expected_field(0) + _sim_mag_offset_x.get(), - expected_field(1) + _sim_mag_offset_y.get(), - expected_field(2) + _sim_mag_offset_z.get()); - } - } - - perf_end(_loop_perf); -} - -int SensorMagSim::task_spawn(int argc, char *argv[]) -{ - SensorMagSim *instance = new SensorMagSim(); - - if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; - - if (instance->init()) { - return PX4_OK; - } - - } else { - PX4_ERR("alloc failed"); - } - - delete instance; - _object.store(nullptr); - _task_id = -1; - - return PX4_ERROR; -} - -int SensorMagSim::custom_command(int argc, char *argv[]) -{ - return print_usage("unknown command"); -} - -int SensorMagSim::print_usage(const char *reason) -{ - if (reason) { - PX4_WARN("%s\n", reason); - } - - PRINT_MODULE_DESCRIPTION( - R"DESCR_STR( -### Description - - -)DESCR_STR"); - - PRINT_MODULE_USAGE_NAME("sensor_mag_sim", "system"); - PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); - - return 0; -} - -extern "C" __EXPORT int sensor_mag_sim_main(int argc, char *argv[]) -{ - return SensorMagSim::main(argc, argv); -} diff --git a/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp b/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp deleted file mode 100644 index 18c4adbf1d..0000000000 --- a/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp +++ /dev/null @@ -1,94 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2021 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 - * 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. - * - ****************************************************************************/ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace time_literals; - -class SensorMagSim : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem -{ -public: - SensorMagSim(); - ~SensorMagSim() override; - - /** @see ModuleBase */ - static int task_spawn(int argc, char *argv[]); - - /** @see ModuleBase */ - static int custom_command(int argc, char *argv[]); - - /** @see ModuleBase */ - static int print_usage(const char *reason = nullptr); - - bool init(); - -private: - void Run() override; - - // generate white Gaussian noise sample with std=1 - static float generate_wgn(); - - // generate white Gaussian noise sample as a 3D vector with specified std - matrix::Vector3f noiseGauss3f(float stdx, float stdy, float stdz) { return matrix::Vector3f(generate_wgn() * stdx, generate_wgn() * stdy, generate_wgn() * stdz); } - - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude_groundtruth)}; - uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)}; - - PX4Magnetometer _px4_mag{197388, ROTATION_NONE}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 1, ADDR: 3, TYPE: SIMULATION - - bool _mag_earth_available{false}; - - matrix::Vector3f _mag_earth_pred{}; - - perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; - - DEFINE_PARAMETERS( - (ParamFloat) _sim_mag_offset_x, - (ParamFloat) _sim_mag_offset_y, - (ParamFloat) _sim_mag_offset_z - ) -}; diff --git a/src/modules/simulation/sensor_mag_sim/CMakeLists.txt b/src/modules/simulation/sensor_sim_manager/CMakeLists.txt similarity index 83% rename from src/modules/simulation/sensor_mag_sim/CMakeLists.txt rename to src/modules/simulation/sensor_sim_manager/CMakeLists.txt index 1508a84ec2..5dc5af39f7 100644 --- a/src/modules/simulation/sensor_mag_sim/CMakeLists.txt +++ b/src/modules/simulation/sensor_sim_manager/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2021 PX4 Development Team. All rights reserved. +# Copyright (c) 2025 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 @@ -32,14 +32,17 @@ ############################################################################ px4_add_module( - MODULE modules__simulation__senosr_mag_sim - MAIN sensor_mag_sim + MODULE modules__simulation__sensor_sim_manager + MAIN sensor_sim_manager COMPILE_FLAGS + INCLUDES + ${CMAKE_CURRENT_SOURCE_DIR}/../failure_injection SRCS - SensorMagSim.cpp - SensorMagSim.hpp + SensorSimManager.cpp + SensorSimManager.hpp + ${CMAKE_CURRENT_SOURCE_DIR}/../failure_injection/failureInjection.cpp DEPENDS - drivers_magnetometer geo + mathlib px4_work_queue - ) +) diff --git a/src/modules/simulation/sensor_sim_manager/Kconfig b/src/modules/simulation/sensor_sim_manager/Kconfig new file mode 100644 index 0000000000..6290035ad3 --- /dev/null +++ b/src/modules/simulation/sensor_sim_manager/Kconfig @@ -0,0 +1,5 @@ +menuconfig MODULES_SIMULATION_SENSOR_SIM_MANAGER + bool "sensor_sim_manager" + default n + ---help--- + Enable support for sensor_sim_manager - Unified sensor simulation manager for SIH \ No newline at end of file diff --git a/src/modules/simulation/sensor_sim_manager/SensorSimManager.cpp b/src/modules/simulation/sensor_sim_manager/SensorSimManager.cpp new file mode 100644 index 0000000000..5e06940ba8 --- /dev/null +++ b/src/modules/simulation/sensor_sim_manager/SensorSimManager.cpp @@ -0,0 +1,578 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 + * 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. + * + ****************************************************************************/ + +#include "SensorSimManager.hpp" + +#include +#include +#include +#include +#include + +using matrix::sign; + +using namespace matrix; + +SensorSimManager::SensorSimManager() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), + _gen(_rd()), + _uniform_dist(0.0f, 1.0f) +{ + _px4_mag.set_device_type(DRV_MAG_DEVTYPE_MAGSIM); + srand(1234); + + // Initialize sensor timings with base intervals and random offsets + const hrt_abstime now = hrt_absolute_time(); + + // GPS: 8 Hz + _gps_timing = { + .interval_us = 125000, + .next_update_time = now, + .offset_us = static_cast(_uniform_dist(_gen) * 125000), + .enabled = false, + }; + + // Barometer: 20 Hz + _baro_timing = { + .interval_us = 50000, + .next_update_time = now, + .offset_us = static_cast(_uniform_dist(_gen) * 50000), + .enabled = false, + }; + + // Magnetometer: 50 Hz + _mag_timing = { + .interval_us = 20000, + .next_update_time = now, + .offset_us = static_cast(_uniform_dist(_gen) * 20000), + .enabled = false, + }; + + // Airspeed: 8 Hz + _airspeed_timing = { + .interval_us = 125000, + .next_update_time = now, + .offset_us = static_cast(_uniform_dist(_gen) * 125000), + .enabled = false, + }; + + // AGP: 2 Hz + _agp_timing = { + .interval_us = 500000, + .next_update_time = now, + .offset_us = static_cast(_uniform_dist(_gen) * 500000), + .enabled = false, + }; + + _gps_timing.next_update_time += _gps_timing.offset_us; + _baro_timing.next_update_time += _baro_timing.offset_us; + _mag_timing.next_update_time += _mag_timing.offset_us; + _airspeed_timing.next_update_time += _airspeed_timing.offset_us; + _agp_timing.next_update_time += _agp_timing.offset_us; + +} + +SensorSimManager::~SensorSimManager() +{ + perf_free(_loop_perf); + perf_free(_gps_perf); + perf_free(_baro_perf); + perf_free(_mag_perf); + perf_free(_airspeed_perf); + perf_free(_agp_perf); +} + +bool SensorSimManager::init() +{ + ScheduleOnInterval(10_ms); + return true; +} + +float SensorSimManager::generate_wgn() +{ + // Generate white Gaussian noise sample with std=1 + // Using Box-Muller transform + static float V1, V2, S; + static bool phase = true; + float X; + + if (phase) { + do { + 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; + } while (S >= 1.0f || fabsf(S) < 1e-8f); + + X = V1 * float(sqrtf(-2.0f * float(logf(S)) / S)); + + } else { + X = V2 * float(sqrtf(-2.0f * float(logf(S)) / S)); + } + + phase = !phase; + return X; +} + +matrix::Vector3f SensorSimManager::noiseGauss3f(float stdx, float stdy, float stdz) +{ + return matrix::Vector3f(generate_wgn() * stdx, generate_wgn() * stdy, generate_wgn() * stdz); +} + +void SensorSimManager::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + if (_parameter_update_sub.updated()) { + parameter_update_s parameter_update; + _parameter_update_sub.copy(¶meter_update); + updateParams(); + + _gps_timing.enabled = (_param_sens_en_gpssim.get() != 0); + _baro_timing.enabled = (_param_sens_en_barosim.get() != 0); + _mag_timing.enabled = (_param_sens_en_magsim.get() != 0); + _airspeed_timing.enabled = (_param_sens_en_arspdsim.get() != 0); + _agp_timing.enabled = (_param_sens_en_agpsim.get() != 0); + } + + _failure_injection.check_failure_injections(); + + const hrt_abstime now = hrt_absolute_time(); + + if (_gps_timing.enabled && now >= _gps_timing.next_update_time) { + updateGPS(); + _gps_timing.next_update_time = now + _gps_timing.interval_us; + } + + if (_baro_timing.enabled && now >= _baro_timing.next_update_time) { + updateBarometer(); + _baro_timing.next_update_time = now + _baro_timing.interval_us; + } + + if (_mag_timing.enabled && now >= _mag_timing.next_update_time) { + updateMagnetometer(); + _mag_timing.next_update_time = now + _mag_timing.interval_us; + } + + if (_airspeed_timing.enabled && now >= _airspeed_timing.next_update_time) { + updateAirspeed(); + _airspeed_timing.next_update_time = now + _airspeed_timing.interval_us; + } + + if (_agp_timing.enabled && now >= _agp_timing.next_update_time) { + updateAGP(); + _agp_timing.next_update_time = now + _agp_timing.interval_us; + } + + perf_end(_loop_perf); +} + +void SensorSimManager::updateGPS() +{ + perf_begin(_gps_perf); + + _failure_injection.check_failure_injections(); + + vehicle_local_position_s lpos{}; + _vehicle_local_position_sub.copy(&lpos); + + vehicle_global_position_s gpos{}; + _vehicle_global_position_sub.copy(&gpos); + + // Check if data is recent (within 20ms), TODO + const hrt_abstime now = hrt_absolute_time(); + + if (lpos.timestamp > 0 && gpos.timestamp > 0 && + (now - lpos.timestamp) < 20000 && (now - gpos.timestamp) < 20000) { + + double latitude = gpos.lat + math::degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH); + double longitude = gpos.lon + math::degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH); + double altitude = (double)(gpos.alt + (generate_wgn() * 0.5f)); + + Vector3f gps_vel = Vector3f{lpos.vx, lpos.vy, lpos.vz} + noiseGauss3f(0.06f, 0.077f, 0.158f); + + // device id + device::Device::DeviceId device_id; + device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; + device_id.devid_s.bus = 0; + device_id.devid_s.address = 0; + device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM; + + sensor_gps_s sensor_gps{}; + + if (_sim_gps_used.get() >= 4) { + // fix + sensor_gps.fix_type = 3; // 3D fix + sensor_gps.s_variance_m_s = 0.4f; + sensor_gps.c_variance_rad = 0.1f; + sensor_gps.eph = 0.9f; + sensor_gps.epv = 1.78f; + sensor_gps.hdop = 0.7f; + sensor_gps.vdop = 1.1f; + + } else { + // no fix + sensor_gps.fix_type = 0; // No fix + sensor_gps.s_variance_m_s = 100.f; + sensor_gps.c_variance_rad = 100.f; + sensor_gps.eph = 100.f; + sensor_gps.epv = 100.f; + sensor_gps.hdop = 100.f; + sensor_gps.vdop = 100.f; + } + + sensor_gps.timestamp_sample = gpos.timestamp_sample; + sensor_gps.time_utc_usec = 0; + sensor_gps.device_id = device_id.devid; + sensor_gps.latitude_deg = latitude; // Latitude in degrees + sensor_gps.longitude_deg = longitude; // Longitude in degrees + sensor_gps.altitude_msl_m = altitude; // Altitude in meters above MSL + sensor_gps.altitude_ellipsoid_m = altitude; + sensor_gps.noise_per_ms = 0; + sensor_gps.jamming_indicator = 0; + sensor_gps.vel_m_s = sqrtf(gps_vel(0) * gps_vel(0) + gps_vel(1) * gps_vel(1)); // GPS ground speed, (metres/sec) + sensor_gps.vel_n_m_s = gps_vel(0); + sensor_gps.vel_e_m_s = gps_vel(1); + sensor_gps.vel_d_m_s = gps_vel(2); + sensor_gps.cog_rad = atan2(gps_vel(1), + gps_vel(0)); // Course over ground (NOT heading, but direction of movement), -PI..PI, (radians) + sensor_gps.timestamp_time_relative = 0; + sensor_gps.heading = NAN; + sensor_gps.heading_offset = NAN; + sensor_gps.heading_accuracy = 0; + sensor_gps.automatic_gain_control = 0; + sensor_gps.jamming_state = 0; + sensor_gps.spoofing_state = 0; + sensor_gps.vel_ned_valid = true; + sensor_gps.satellites_used = _sim_gps_used.get(); + + sensor_gps.timestamp = hrt_absolute_time(); + + // Apply failure injection + if (_failure_injection.handle_gps_failure(sensor_gps) && _failure_injection.handle_gps_alt_failure(sensor_gps)) { + _sensor_gps_pub.publish(sensor_gps); + + } + } + + perf_end(_gps_perf); +} + +void SensorSimManager::updateBarometer() +{ + perf_begin(_baro_perf); + + vehicle_global_position_s gpos; + + if (_vehicle_global_position_sub.copy(&gpos)) { + // Check if data is recent (within 20ms), TODO + const hrt_abstime now = hrt_absolute_time(); + + if (gpos.timestamp > 0 && (now - gpos.timestamp) < 20000) { + const float dt = math::constrain((gpos.timestamp - _last_baro_update_time) * 1e-6f, 0.001f, 0.1f); + + const float alt_msl = gpos.alt; + + const float temperature_local = TEMPERATURE_MSL - LAPSE_RATE * alt_msl; + const float pressure_ratio = powf(TEMPERATURE_MSL / temperature_local, 5.256f); + const float absolute_pressure = PRESSURE_MSL / pressure_ratio; + + // generate Gaussian noise sequence using polar form of Box-Muller transformation + double y1; + { + double x1; + double x2; + double w; + + if (!_baro_rnd_use_last) { + do { + x1 = 2. * (double)generate_wgn() - 1.; + x2 = 2. * (double)generate_wgn() - 1.; + w = x1 * x1 + x2 * x2; + } while (w >= 1.0); + + w = sqrt((-2.0 * log(w)) / w); + // calculate two values - the second value can be used next time because it is uncorrelated + y1 = x1 * w; + _baro_rnd_y2 = x2 * w; + _baro_rnd_use_last = true; + + } else { + // no need to repeat the calculation - use the second value from last update + y1 = _baro_rnd_y2; + _baro_rnd_use_last = false; + } + } + + // Apply noise and drift + const float abs_pressure_noise = 1.f * (float)y1; // 1 Pa RMS noise + _baro_drift_pa += _baro_drift_pa_per_sec * dt; + const float absolute_pressure_noisy = absolute_pressure + abs_pressure_noise + _baro_drift_pa; + + // convert to hPa + float pressure = absolute_pressure_noisy + _sim_baro_off_p.get(); + + // calculate temperature in Celsius + float temperature = temperature_local - 273.0f + _sim_baro_off_t.get(); + + // publish + sensor_baro_s sensor_baro{}; + sensor_baro.timestamp_sample = gpos.timestamp; + sensor_baro.device_id = 6620172; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION + sensor_baro.pressure = pressure; + sensor_baro.temperature = temperature; + sensor_baro.timestamp = hrt_absolute_time(); + _sensor_baro_pub.publish(sensor_baro); + + _last_baro_update_time = gpos.timestamp; + } + } + + perf_end(_baro_perf); +} + +void SensorSimManager::updateMagnetometer() +{ + perf_begin(_mag_perf); + + vehicle_global_position_s gpos; + + if (_vehicle_global_position_sub.copy(&gpos)) { + // Check if data is recent (within 20ms), TODO + const hrt_abstime now = hrt_absolute_time(); + + if (gpos.timestamp > 0 && (now - gpos.timestamp) < 20000 && gpos.eph < 1000) { + // magnetic field data returned by the geo library using the current GPS position + const float declination_rad = math::radians(get_mag_declination_degrees(gpos.lat, gpos.lon)); + const float inclination_rad = math::radians(get_mag_inclination_degrees(gpos.lat, gpos.lon)); + const float field_strength_gauss = get_mag_strength_gauss(gpos.lat, gpos.lon); + + _mag_earth_pred = Dcmf(Eulerf(0, -inclination_rad, declination_rad)) * Vector3f(field_strength_gauss, 0, 0); + + _mag_earth_available = true; + } + } + + if (_mag_earth_available) { + vehicle_attitude_s attitude; + + if (_vehicle_attitude_sub.copy(&attitude)) { + Vector3f expected_field = Dcmf{Quatf{attitude.q}} .transpose() * _mag_earth_pred; + + expected_field += noiseGauss3f(0.02f, 0.02f, 0.03f); + + _px4_mag.update(attitude.timestamp, + expected_field(0) + _sim_mag_offset_x.get(), + expected_field(1) + _sim_mag_offset_y.get(), + expected_field(2) + _sim_mag_offset_z.get()); + } + } + + perf_end(_mag_perf); +} + +void SensorSimManager::updateAirspeed() +{ + perf_begin(_airspeed_perf); + + vehicle_local_position_s lpos{}; + _vehicle_local_position_sub.copy(&lpos); + + vehicle_global_position_s gpos{}; + _vehicle_global_position_sub.copy(&gpos); + + vehicle_attitude_s attitude{}; + _vehicle_attitude_sub.copy(&attitude); + + // Check if data is recent (within 20ms), TODO + const hrt_abstime now = hrt_absolute_time(); + + if (lpos.timestamp > 0 && gpos.timestamp > 0 && attitude.timestamp > 0 && + (now - lpos.timestamp) < 20000 && (now - gpos.timestamp) < 20000 && (now - attitude.timestamp) < 20000) { + + Vector3f local_velocity = Vector3f{lpos.vx, lpos.vy, lpos.vz}; + Vector3f body_velocity = Dcmf{Quatf{attitude.q}} .transpose() * local_velocity; + + // device id + device::Device::DeviceId device_id; + device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; + device_id.devid_s.bus = 0; + device_id.devid_s.address = 0; + device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_SIM; + + const float alt_amsl = gpos.alt; + const float temperature_local = TEMPERATURE_MSL - LAPSE_RATE * alt_amsl; + const float density_ratio = powf(TEMPERATURE_MSL / temperature_local, 4.256f); + const float air_density = AIR_DENSITY_MSL / density_ratio; + + // calculate differential pressure + noise in hPa + const float diff_pressure_noise = (float)generate_wgn() * 0.01f; + float diff_pressure = sign(body_velocity(0)) * 0.005f * air_density * body_velocity(0) * body_velocity( + 0) + diff_pressure_noise; + + differential_pressure_s differential_pressure{}; + differential_pressure.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION + differential_pressure.differential_pressure_pa = (double)diff_pressure * 100.0; // hPa to Pa; + differential_pressure.temperature = temperature_local; + differential_pressure.timestamp = hrt_absolute_time(); + _differential_pressure_pub.publish(differential_pressure); + } + + perf_end(_airspeed_perf); +} + +void SensorSimManager::updateAGP() +{ + perf_begin(_agp_perf); + + vehicle_global_position_s gpos{}; + _vehicle_global_position_sub.copy(&gpos); + + // Check if data is recent (within 20ms), TODO + const hrt_abstime now = hrt_absolute_time(); + + if (gpos.timestamp > 0 && (now - gpos.timestamp) < 20000) { + + const uint64_t current_time = gpos.timestamp; + const float dt = (current_time - _agp_time_last_update) * 1e-6f; + _agp_time_last_update = current_time; + + // Handle failure modes + enum class FailureMode : int32_t { + Stuck = (1 << 0), + Drift = (1 << 1) + }; + + if (!(_sim_agp_fail.get() & static_cast(FailureMode::Stuck))) { + _agp_measured_lla = LatLonAlt(gpos.lat, gpos.lon, gpos.alt_ellipsoid); + } + + if (_sim_agp_fail.get() & static_cast(FailureMode::Drift)) { + _agp_position_bias += matrix::Vector3f(1.5f, -5.f, 0.f) * dt; + _agp_measured_lla += _agp_position_bias; + + } else { + _agp_position_bias.zero(); + } + + const double latitude = _agp_measured_lla.latitude_deg() + math::degrees((double)generate_wgn() * 2.0 / + CONSTANTS_RADIUS_OF_EARTH); + const double longitude = _agp_measured_lla.longitude_deg() + math::degrees((double)generate_wgn() * 2.0 / + CONSTANTS_RADIUS_OF_EARTH); + const double altitude = (double)(_agp_measured_lla.altitude() + (generate_wgn() * 0.5f)); + + vehicle_global_position_s agp_sample{}; + + agp_sample.timestamp_sample = gpos.timestamp_sample; + agp_sample.lat = latitude; + agp_sample.lon = longitude; + agp_sample.alt = altitude; + agp_sample.lat_lon_valid = true; + agp_sample.alt_ellipsoid = altitude; + agp_sample.alt_valid = true; + agp_sample.eph = 20.f; + agp_sample.epv = 5.f; + + agp_sample.timestamp = hrt_absolute_time(); + _aux_global_position_pub.publish(agp_sample); + } + + perf_end(_agp_perf); +} + +int SensorSimManager::task_spawn(int argc, char *argv[]) +{ + SensorSimManager *instance = new SensorSimManager(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int SensorSimManager::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int SensorSimManager::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Unified sensor simulation manager for SIH (Simulation in Hardware). +Simulates GPS, barometer, magnetometer, airspeed, and AGP sensors with realistic noise and timing. +Integrates failure injection capabilities for robust testing. + +### Examples +To start the sensor simulation manager: +$ sensor_sim_manager start + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("sensor_sim_manager", "simulation"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int sensor_sim_manager_main(int argc, char *argv[]) +{ + return SensorSimManager::main(argc, argv); +} diff --git a/src/modules/simulation/sensor_sim_manager/SensorSimManager.hpp b/src/modules/simulation/sensor_sim_manager/SensorSimManager.hpp new file mode 100644 index 0000000000..6643b83850 --- /dev/null +++ b/src/modules/simulation/sensor_sim_manager/SensorSimManager.hpp @@ -0,0 +1,179 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 + * 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class SensorSimManager : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + SensorSimManager(); + ~SensorSimManager() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + +private: + void Run() override; + + // Sensor simulation methods + void updateGPS(); + void updateBarometer(); + void updateMagnetometer(); + void updateAirspeed(); + void updateAGP(); + + // Utility methods + static float generate_wgn(); + matrix::Vector3f noiseGauss3f(float stdx, float stdy, float stdz); + + // Sensor timing structure + struct SensorTiming { + hrt_abstime interval_us; + hrt_abstime next_update_time; + hrt_abstime offset_us; + bool enabled; + }; + + // Sensor timings with random offsets + SensorTiming _gps_timing; + SensorTiming _baro_timing; + SensorTiming _mag_timing; + SensorTiming _airspeed_timing; + SensorTiming _agp_timing; + + // Subscriptions + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position_groundtruth)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude_groundtruth)}; + + // Publications + uORB::PublicationMulti _sensor_gps_pub{ORB_ID(sensor_gps)}; + uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + uORB::PublicationMulti _differential_pressure_pub{ORB_ID(differential_pressure)}; + uORB::PublicationMulti _aux_global_position_pub{ORB_ID(aux_global_position)}; + + // Magnetometer uses PX4Magnetometer publisher + PX4Magnetometer _px4_mag{1, ROTATION_NONE}; + + FailureInjection _failure_injection{}; + + // TODO: needed? + perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + perf_counter_t _gps_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": gps")}; + perf_counter_t _baro_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": baro")}; + perf_counter_t _mag_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": mag")}; + perf_counter_t _airspeed_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": airspeed")}; + perf_counter_t _agp_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": agp")}; + + // Random number generator for offsets and noise + std::random_device _rd; + std::mt19937 _gen; + std::uniform_real_distribution _uniform_dist; + + // Parameters + DEFINE_PARAMETERS( + (ParamInt) _param_sens_en_gpssim, + (ParamInt) _param_sens_en_barosim, + (ParamInt) _param_sens_en_magsim, + (ParamInt) _param_sens_en_arspdsim, + (ParamInt) _param_sens_en_agpsim, + (ParamInt) _sim_gps_used, + (ParamFloat) _sim_baro_off_p, + (ParamFloat) _sim_baro_off_t, + (ParamFloat) _sim_mag_offset_x, + (ParamFloat) _sim_mag_offset_y, + (ParamFloat) _sim_mag_offset_z, + (ParamInt) _sim_agp_fail + ) + + // Barometer simulation state + hrt_abstime _last_baro_update_time{0}; + float _baro_drift_pa{0.0f}; + float _baro_drift_pa_per_sec{0.1f}; // TODO, was 0 + bool _baro_rnd_use_last{false}; + double _baro_rnd_y2{0.0}; + + // Magnetometer simulation state + matrix::Vector3f _mag_earth_pred{}; + bool _mag_earth_available{false}; + + // AGP simulation state + LatLonAlt _agp_measured_lla{}; + matrix::Vector3f _agp_position_bias{}; + uint64_t _agp_time_last_update{0}; + + // Air constants + static constexpr float TEMPERATURE_MSL = 288.0f; // [K] + static constexpr float PRESSURE_MSL = 101325.0f; // [Pa] + static constexpr float LAPSE_RATE = 0.0065f; // [K/m] + static constexpr float AIR_DENSITY_MSL = 1.225f; // [kg/m^3] + +}; diff --git a/src/modules/simulation/sensor_mag_sim/parameters.c b/src/modules/simulation/sensor_sim_manager/parameters.c similarity index 58% rename from src/modules/simulation/sensor_mag_sim/parameters.c rename to src/modules/simulation/sensor_sim_manager/parameters.c index 1002a02663..b29e979c3d 100644 --- a/src/modules/simulation/sensor_mag_sim/parameters.c +++ b/src/modules/simulation/sensor_sim_manager/parameters.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2025 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 @@ -31,37 +31,108 @@ * ****************************************************************************/ /** - * Enable simulated magnetometer sensor instance + * Enable simulated GPS sensor * - * @reboot_required true - * @min 0 - * @max 1 - * @group Sensors + * @group Simulation * @value 0 Disabled * @value 1 Enabled - */ -PARAM_DEFINE_INT32(SENS_EN_MAGSIM, 0); + */ +PARAM_DEFINE_INT32(SENS_EN_GPSSIM, 1); + +/** + * Enable simulated barometer sensor + * + * @group Simulation + * @value 0 Disabled + * @value 1 Enabled + */ +PARAM_DEFINE_INT32(SENS_EN_BAROSIM, 1); + +/** + * Enable simulated magnetometer sensor + * + * @group Simulation + * @value 0 Disabled + * @value 1 Enabled + */ +PARAM_DEFINE_INT32(SENS_EN_MAGSIM, 1); + +/** + * Enable simulated airspeed sensor + * + * @group Simulation + * @value 0 Disabled + * @value 1 Enabled + */ +PARAM_DEFINE_INT32(SENS_EN_ARSPDSIM, 1); + +/** + * Enable simulated AGP sensor + * + * @group Simulation + * @value 0 Disabled + * @value 1 Enabled + */ +PARAM_DEFINE_INT32(SENS_EN_AGPSIM, 1); + +/** + * Number of GPS satellites used in simulation + * + * @group Simulator + * @min 0 + * @max 50 + */ +PARAM_DEFINE_INT32(SIM_GPS_USED, 10); + +/** + * simulated barometer pressure offset + * + * @group Simulator + */ +PARAM_DEFINE_FLOAT(SIM_BARO_OFF_P, 0.0f); + +/** + * simulated barometer temperature offset + * + * @group Simulator + * @unit celcius + */ +PARAM_DEFINE_FLOAT(SIM_BARO_OFF_T, 0.0f); /** * simulated magnetometer X offset * - * @unit gauss * @group Simulator + * @unit gauss */ PARAM_DEFINE_FLOAT(SIM_MAG_OFFSET_X, 0.0f); /** * simulated magnetometer Y offset * - * @unit gauss * @group Simulator + * @unit gauss */ PARAM_DEFINE_FLOAT(SIM_MAG_OFFSET_Y, 0.0f); /** * simulated magnetometer Z offset * - * @unit gauss * @group Simulator + * @unit gauss */ -PARAM_DEFINE_FLOAT(SIM_MAG_OFFSET_Z, 0.0f); +PARAM_DEFINE_FLOAT(SIM_MAG_OFFSET_Z, 0.0f); + +/** + * AGP failure mode + * + * Stuck: freeze the measurement to the current location + * Drift: add a linearly growing bias to the sensor data + * + * @group Simulator + * @min 0 + * @max 3 + * @bit 0 Stuck + * @bit 1 Drift + */ +PARAM_DEFINE_INT32(SIM_AGP_FAIL, 0); diff --git a/src/modules/simulation/simulator_sih/Kconfig b/src/modules/simulation/simulator_sih/Kconfig index 6f3be6f637..5de8ff8e13 100644 --- a/src/modules/simulation/simulator_sih/Kconfig +++ b/src/modules/simulation/simulator_sih/Kconfig @@ -2,9 +2,7 @@ menuconfig MODULES_SIMULATION_SIMULATOR_SIH bool "simulator_sih" default n select MODULES_SIMULATION_PWM_OUT_SIM - select MODULES_SIMULATION_SENSOR_BARO_SIM - select MODULES_SIMULATION_SENSOR_GPS_SIM - select MODULES_SIMULATION_SENSOR_MAG_SIM + select MODULES_SIMULATION_SENSOR_SIM_MANAGER ---help--- Enable support for simulator_sih