fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-30 00:04:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

* refactor sensor simulation modules to one module called SensorSimManager. allows better failure handling

Browse Source 
* still all sensors can be enabled/disabled through parameters
* check airspeed simulation (sih.cpp has seperate calc.)
This commit is contained in:
Marco Hauswirth 2025-09-18 00:19:32 +02:00
parent 7ee9ba0c48
commit 8bf68e91e2
34 changed files with 873 additions and 2017 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
ROMFS/px4fmu_common/init.d-posix/px4-rc.gzsim
View File

@ -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

17
ROMFS/px4fmu_common/init.d-posix/px4-rc.sihsim
View File

@ -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"

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

@ -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

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

@ -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

5
src/modules/simulation/Kconfig
View File

@ -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

43
src/modules/simulation/sensor_agp_sim/CMakeLists.txt
View File

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

5
src/modules/simulation/sensor_agp_sim/Kconfig
View File

@ -1,5 +0,0 @@
menuconfig MODULES_SIMULATION_SENSOR_AGP_SIM
bool "sensor_agp_sim"
default n
---help---
Enable support for sensor_agp_sim

202
src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp
View File

@ -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 <drivers/drv_sensor.h>
#include <lib/drivers/device/Device.hpp>
#include <lib/geo/geo.h>
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(&param_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<int32_t>(FailureMode::Stuck))) {
_measured_lla = LatLonAlt(gpos.lat, gpos.lon, gpos.alt_ellipsoid);
}
if (_param_sim_agp_fail.get() & static_cast<int32_t>(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);
}

93
src/modules/simulation/sensor_agp_sim/SensorAgpSim.hpp
View File

@ -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 <lib/lat_lon_alt/lat_lon_alt.hpp>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_global_position.h>
using namespace time_literals;
class SensorAgpSim : public ModuleBase<SensorAgpSim>, 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<vehicle_global_position_s> _aux_global_position_pub{ORB_ID(aux_global_position)};
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
DEFINE_PARAMETERS(
(ParamInt<px4::params::SIM_AGP_FAIL>) _param_sim_agp_fail
)
};

57
src/modules/simulation/sensor_agp_sim/parameters.c
View File

@ -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);

43
src/modules/simulation/sensor_airspeed_sim/CMakeLists.txt
View File

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

5
src/modules/simulation/sensor_airspeed_sim/Kconfig
View File

@ -1,5 +0,0 @@
menuconfig MODULES_SIMULATION_SENSOR_AIRSPEED_SIM
bool "sensor_airspeed_sim"
default n
---help---
Enable support for sensor_airspeed_sim

208
src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp
View File

@ -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 <drivers/drv_sensor.h>
#include <lib/drivers/device/Device.hpp>
#include <lib/geo/geo.h>
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(&param_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);
}

96
src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp
View File

@ -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 <lib/perf/perf_counter.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
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<SensorAirspeedSim>, 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_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
DEFINE_PARAMETERS(
(ParamInt<px4::params::SIM_ARSPD_FAIL>) _sim_failure
)
};

56
src/modules/simulation/sensor_airspeed_sim/parameters.c
View File

@ -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);

44
src/modules/simulation/sensor_baro_sim/CMakeLists.txt
View File

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

5
src/modules/simulation/sensor_baro_sim/Kconfig
View File

@ -1,5 +0,0 @@
menuconfig MODULES_SIMULATION_SENSOR_BARO_SIM
bool "sensor_baro_sim"
default n
---help---
Enable support for sensor_baro_sim

232
src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp
View File

@ -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 <drivers/drv_sensor.h>
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(&param_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);
}

92
src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp
View File

@ -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 <lib/perf/perf_counter.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/vehicle_global_position.h>
using namespace time_literals;
class SensorBaroSim : public ModuleBase<SensorBaroSim>, 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_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::SIM_BARO_OFF_P>) _sim_baro_off_p,
(ParamFloat<px4::params::SIM_BARO_OFF_T>) _sim_baro_off_t
)
};

59
src/modules/simulation/sensor_baro_sim/parameters.c
View File

@ -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);

43
src/modules/simulation/sensor_gps_sim/CMakeLists.txt
View File

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

5
src/modules/simulation/sensor_gps_sim/Kconfig
View File

@ -1,5 +0,0 @@
menuconfig MODULES_SIMULATION_SENSOR_GPS_SIM
bool "sensor_gps_sim"
default n
---help---
Enable support for sensor_gps_sim

236
src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp
View File

@ -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 <drivers/drv_sensor.h>
#include <lib/drivers/device/Device.hpp>
#include <lib/geo/geo.h>
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(&param_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);
}

88
src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp
View File

@ -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 <lib/perf/perf_counter.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
using namespace time_literals;
class SensorGpsSim : public ModuleBase<SensorGpsSim>, 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_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
DEFINE_PARAMETERS(
(ParamInt<px4::params::SIM_GPS_USED>) _sim_gps_used
)
};

52
src/modules/simulation/sensor_gps_sim/parameters.c
View File

@ -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);

5
src/modules/simulation/sensor_mag_sim/Kconfig
View File

@ -1,5 +0,0 @@
menuconfig MODULES_SIMULATION_SENSOR_MAG_SIM
bool "sensor_mag_sim"
default n
---help---
Enable support for sensor_mag_sim

196
src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp
View File

@ -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 <drivers/drv_sensor.h>
#include <lib/world_magnetic_model/geo_mag_declination.h>
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(&param_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);
}

94
src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp
View File

@ -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 <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
using namespace time_literals;
class SensorMagSim : public ModuleBase<SensorMagSim>, 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<px4::params::SIM_MAG_OFFSET_X>) _sim_mag_offset_x,
(ParamFloat<px4::params::SIM_MAG_OFFSET_Y>) _sim_mag_offset_y,
(ParamFloat<px4::params::SIM_MAG_OFFSET_Z>) _sim_mag_offset_z
)
};

17
src/modules/simulation/sensor_mag_sim/CMakeLists.txt → src/modules/simulation/sensor_sim_manager/CMakeLists.txt
View File

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

5
src/modules/simulation/sensor_sim_manager/Kconfig Normal file
View File

@ -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

578
src/modules/simulation/sensor_sim_manager/SensorSimManager.cpp Normal file
View File

@ -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 <drivers/drv_sensor.h>
#include <lib/drivers/device/Device.hpp>
#include <lib/geo/geo.h>
#include <lib/mathlib/mathlib.h>
#include <lib/matrix/matrix/math.hpp>
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<hrt_abstime>(_uniform_dist(_gen) * 125000),
.enabled = false,
};
// Barometer: 20 Hz
_baro_timing = {
.interval_us = 50000,
.next_update_time = now,
.offset_us = static_cast<hrt_abstime>(_uniform_dist(_gen) * 50000),
.enabled = false,
};
// Magnetometer: 50 Hz
_mag_timing = {
.interval_us = 20000,
.next_update_time = now,
.offset_us = static_cast<hrt_abstime>(_uniform_dist(_gen) * 20000),
.enabled = false,
};
// Airspeed: 8 Hz
_airspeed_timing = {
.interval_us = 125000,
.next_update_time = now,
.offset_us = static_cast<hrt_abstime>(_uniform_dist(_gen) * 125000),
.enabled = false,
};
// AGP: 2 Hz
_agp_timing = {
.interval_us = 500000,
.next_update_time = now,
.offset_us = static_cast<hrt_abstime>(_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(&parameter_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<int32_t>(FailureMode::Stuck))) {
_agp_measured_lla = LatLonAlt(gpos.lat, gpos.lon, gpos.alt_ellipsoid);
}
if (_sim_agp_fail.get() & static_cast<int32_t>(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);
}

179
src/modules/simulation/sensor_sim_manager/SensorSimManager.hpp Normal file
View File

@ -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 <lib/perf/perf_counter.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/sensor_mag.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/airspeed_validated.h>
#include <simulation/failure_injection/failureInjection.hpp>
#include <drivers/drv_sensor.h>
#include <lib/drivers/device/Device.hpp>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <lib/world_magnetic_model/geo_mag_declination.h>
#include <lib/geo/geo.h>
#include <lib/mathlib/mathlib.h>
#include <lib/lat_lon_alt/lat_lon_alt.hpp>
#include <random>
using namespace time_literals;
class SensorSimManager : public ModuleBase<SensorSimManager>, 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_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
uORB::PublicationMulti<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
uORB::PublicationMulti<vehicle_global_position_s> _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<float> _uniform_dist;
// Parameters
DEFINE_PARAMETERS(
(ParamInt<px4::params::SENS_EN_GPSSIM>) _param_sens_en_gpssim,
(ParamInt<px4::params::SENS_EN_BAROSIM>) _param_sens_en_barosim,
(ParamInt<px4::params::SENS_EN_MAGSIM>) _param_sens_en_magsim,
(ParamInt<px4::params::SENS_EN_ARSPDSIM>) _param_sens_en_arspdsim,
(ParamInt<px4::params::SENS_EN_AGPSIM>) _param_sens_en_agpsim,
(ParamInt<px4::params::SIM_GPS_USED>) _sim_gps_used,
(ParamFloat<px4::params::SIM_BARO_OFF_P>) _sim_baro_off_p,
(ParamFloat<px4::params::SIM_BARO_OFF_T>) _sim_baro_off_t,
(ParamFloat<px4::params::SIM_MAG_OFFSET_X>) _sim_mag_offset_x,
(ParamFloat<px4::params::SIM_MAG_OFFSET_Y>) _sim_mag_offset_y,
(ParamFloat<px4::params::SIM_MAG_OFFSET_Z>) _sim_mag_offset_z,
(ParamInt<px4::params::SIM_AGP_FAIL>) _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]
};

95
src/modules/simulation/sensor_mag_sim/parameters.c → src/modules/simulation/sensor_sim_manager/parameters.c
View File

@ -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);

4
src/modules/simulation/simulator_sih/Kconfig
View File

@ -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