From 2f71fc382c27c1342b8334fb10ab0f553dcef08a Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Thu, 18 Sep 2025 13:03:08 +0200 Subject: [PATCH] remove developing comments and cleanup --- .../init.d-posix/px4-rc.mavlinksim | 2 - .../failure_injection/failureInjection.cpp | 13 ++----- .../failure_injection/failureInjection.hpp | 37 +++++++++++++++++-- .../sensor_sim_manager/SensorSimManager.hpp | 3 +- 4 files changed, 39 insertions(+), 16 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlinksim b/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlinksim index 44a6b77d9c..9041bd6679 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlinksim +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlinksim @@ -27,8 +27,6 @@ else simulator_mavlink start -h "${PX4_SIM_HOSTNAME}" "${simulator_tcp_port}" fi -echo "mvlinksim" - # Start sensor simulation manager for additional sensors not handled by gz if param compare -s SENS_EN_AGPSIM 1 || param compare -s SENS_EN_MAGSIM 1 || param compare -s SENS_EN_ARSPDSIM 1 then diff --git a/src/modules/simulation/failure_injection/failureInjection.cpp b/src/modules/simulation/failure_injection/failureInjection.cpp index 9b413fa47b..6c59ce9ffd 100644 --- a/src/modules/simulation/failure_injection/failureInjection.cpp +++ b/src/modules/simulation/failure_injection/failureInjection.cpp @@ -1,11 +1,6 @@ #include -void FailureInjection::test() -{ - printf("test\n"); -} - bool FailureInjection::handle_gps_failure(sensor_gps_s &gps) { if (_gps_blocked) { @@ -39,7 +34,7 @@ bool FailureInjection::handle_gps_failure(sensor_gps_s &gps) matrix::Vector2f vel_gps(gps.vel_n_m_s, gps.vel_e_m_s); const float vel_norm = vel_gps.norm(); - // directional drift by 5% + // directional drift by RELATIVE_GPS_DRIFT if (vel_norm > 1.f) { vel_gps *= 1.f + RELATIVE_GPS_DRIFT; @@ -50,7 +45,7 @@ bool FailureInjection::handle_gps_failure(sensor_gps_s &gps) vel_gps(0) = 0.5f; } - // perpendicular drift by 5% + // perpendicular drift matrix::Vector2f vel_normal = vel_gps.normalized(); vel_gps(0) += vel_norm * RELATIVE_GPS_DRIFT * vel_normal(1); vel_gps(1) -= vel_norm * RELATIVE_GPS_DRIFT * vel_normal(0); @@ -165,7 +160,7 @@ void FailureInjection::check_failure_injections() // } } else if (failure_type == vehicle_command_s::FAILURE_TYPE_DRIFT) { - PX4_INFO("CMD_INJECT_FAILURE, GPsS drift"); + PX4_INFO("CMD_INJECT_FAILURE, GPS drift"); supported = true; _gps_drift = true; @@ -285,7 +280,7 @@ void FailureInjection::check_failure_injections() // 0 to signal all if (instance == 0) { for (int i = 0; i < GYRO_COUNT_MAX; i++) { - PX4_WARN("CMD_INJECT_FAILUREe, gyro %d stuck", i); + PX4_WARN("CMD_INJECT_FAILURE, gyro %d stuck", i); _gyro_blocked[i] = false; _gyro_stuck[i] = true; } diff --git a/src/modules/simulation/failure_injection/failureInjection.hpp b/src/modules/simulation/failure_injection/failureInjection.hpp index e287ceeb6e..34421886b1 100644 --- a/src/modules/simulation/failure_injection/failureInjection.hpp +++ b/src/modules/simulation/failure_injection/failureInjection.hpp @@ -1,4 +1,37 @@ +/**************************************************************************** +* +* Copyright (c) 2019-2025 PX4 Development Team. All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* 1. Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* 2. Redistributions in binary form must reproduce the above copyright +* notice, this list of conditions and the following disclaimer in +* the documentation and/or other materials provided with the +* distribution. +* 3. Neither the name PX4 nor the names of its contributors may be +* used to endorse or promote products derived from this software +* without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +****************************************************************************/ +#pragma once #include #include @@ -21,7 +54,7 @@ static constexpr uint8_t GYRO_COUNT_MAX = 4; static constexpr uint8_t MAG_COUNT_MAX = 4; static constexpr uint8_t BARO_COUNT_MAX = 4; - +// drift by 10% static constexpr float RELATIVE_GPS_DRIFT = 0.1f; class FailureInjection @@ -29,8 +62,6 @@ class FailureInjection public: FailureInjection() {}; - void test(); - void check_failure_injections(); bool handle_gps_failure(sensor_gps_s &gps); diff --git a/src/modules/simulation/sensor_sim_manager/SensorSimManager.hpp b/src/modules/simulation/sensor_sim_manager/SensorSimManager.hpp index b6b364696a..7459d95e45 100644 --- a/src/modules/simulation/sensor_sim_manager/SensorSimManager.hpp +++ b/src/modules/simulation/sensor_sim_manager/SensorSimManager.hpp @@ -144,7 +144,6 @@ private: float _last_baro_pressure{0.0f}; float _last_baro_temperature{0.0f}; - // 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")}; @@ -179,7 +178,7 @@ private: hrt_abstime _last_baro_update_time{0}; float _baro_drift_pa{0.0f}; - float _baro_drift_pa_per_sec{0.1f}; // TODO, was 0 + float _baro_drift_pa_per_sec{0.1f}; bool _baro_rnd_use_last{false}; double _baro_rnd_y2{0.0};