From 47fc3643ae10e892fb9750bc408b64f37969daac Mon Sep 17 00:00:00 2001 From: Marco Hauswirth Date: Thu, 18 Sep 2025 14:29:12 +0200 Subject: [PATCH] lots of renaming of constant values --- .../failure_injection/failureInjection.cpp | 95 ++++++++++++------- .../failure_injection/failureInjection.hpp | 31 +++--- .../sensor_sim_manager/SensorSimManager.cpp | 41 ++++---- .../sensor_sim_manager/SensorSimManager.hpp | 14 +-- .../simulator_mavlink/SimulatorMavlink.cpp | 12 +-- .../simulator_mavlink/SimulatorMavlink.hpp | 21 ++-- 6 files changed, 116 insertions(+), 98 deletions(-) diff --git a/src/modules/simulation/failure_injection/failureInjection.cpp b/src/modules/simulation/failure_injection/failureInjection.cpp index 6c59ce9ffd..fc46d70368 100644 --- a/src/modules/simulation/failure_injection/failureInjection.cpp +++ b/src/modules/simulation/failure_injection/failureInjection.cpp @@ -1,4 +1,37 @@ +/**************************************************************************** + * + * 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 bool FailureInjection::handle_gps_failure(sensor_gps_s &gps) @@ -21,12 +54,10 @@ bool FailureInjection::handle_gps_failure(sensor_gps_s &gps) gps.vel_d_m_s += 2.f; } - if (_gps_drift) { if (!_has_drift_ref) { _mp.initReference(gps.latitude_deg, gps.longitude_deg); - // Project current LL to local meters to anchor the reference point _mp.project(gps.latitude_deg, gps.longitude_deg, _gps_drift_pos(0), _gps_drift_pos(1)); _has_drift_ref = true; } @@ -34,9 +65,9 @@ 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 RELATIVE_GPS_DRIFT + // directional drift by kRelativeGpsDrift if (vel_norm > 1.f) { - vel_gps *= 1.f + RELATIVE_GPS_DRIFT; + vel_gps *= 1.f + kRelativeGpsDrift; } else if (vel_norm > 0.01f) { vel_gps *= 0.5f / vel_norm; @@ -47,8 +78,8 @@ bool FailureInjection::handle_gps_failure(sensor_gps_s &gps) // 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); + vel_gps(0) += vel_norm * kRelativeGpsDrift * vel_normal(1); + vel_gps(1) -= vel_norm * kRelativeGpsDrift * vel_normal(0); gps.vel_n_m_s = vel_gps(0); gps.vel_e_m_s = vel_gps(1); @@ -61,7 +92,6 @@ bool FailureInjection::handle_gps_failure(sensor_gps_s &gps) } - } else { gps = _gps_prev; } @@ -69,9 +99,7 @@ bool FailureInjection::handle_gps_failure(sensor_gps_s &gps) _last_gps_timestamp = gps.timestamp; _gps_prev = gps; - return true; - } bool FailureInjection::handle_gps_alt_failure(sensor_gps_s &gps) @@ -89,13 +117,15 @@ bool FailureInjection::handle_gps_alt_failure(sensor_gps_s &gps) } if (_gps_alt_drift) { - if (_t0 == 0) { - _t0 = gps.timestamp; + if (_alt_drift_t0 == 0) { + _alt_drift_t0 = gps.timestamp; } - if (_gps_alt_offset < 0.01) { _gps_alt_offset = gps.altitude_msl_m; } + if (_gps_alt_offset < DBL_EPSILON) { _gps_alt_offset = gps.altitude_msl_m; } - gps.vel_d_m_s = 5. * sin(2 * M_PI / 20. * 1e-6 * (gps.timestamp - _t0)); + static constexpr double kDriftPeriodT = 20.; + static constexpr double kDriftMagnitude = 5.; + gps.vel_d_m_s = kDriftMagnitude * sin(2 * M_PI / kDriftPeriodT * 1e-6 * (gps.timestamp - _alt_drift_t0)); if (_last_gps_timestamp > 0) { _gps_alt_offset += -(double)(gps.vel_d_m_s * (gps.timestamp - _last_gps_timestamp) / 1.e6f); @@ -157,7 +187,6 @@ void FailureInjection::check_failure_injections() } else if (failure_type == vehicle_command_s::FAILURE_TYPE_WRONG) { supported = true; _gps_wrong = true; - // } } else if (failure_type == vehicle_command_s::FAILURE_TYPE_DRIFT) { PX4_INFO("CMD_INJECT_FAILURE, GPS drift"); @@ -197,8 +226,6 @@ void FailureInjection::check_failure_injections() _gps_alt_drift = true; } - - } else if (failure_unit == vehicle_command_s::FAILURE_UNIT_SENSOR_ACCEL) { handled = true; @@ -207,13 +234,13 @@ void FailureInjection::check_failure_injections() // 0 to signal all if (instance == 0) { - for (int i = 0; i < ACCEL_COUNT_MAX; i++) { + for (int i = 0; i < kAccelCountMax; i++) { PX4_WARN("CMD_INJECT_FAILURE, accel %d off", i); _accel_blocked[i] = true; _accel_stuck[i] = false; } - } else if (instance >= 1 && instance <= ACCEL_COUNT_MAX) { + } else if (instance >= 1 && instance <= kAccelCountMax) { PX4_WARN("CMD_INJECT_FAILURE, accel %d off", instance - 1); _accel_blocked[instance - 1] = true; _accel_stuck[instance - 1] = false; @@ -224,13 +251,13 @@ void FailureInjection::check_failure_injections() // 0 to signal all if (instance == 0) { - for (int i = 0; i < ACCEL_COUNT_MAX; i++) { + for (int i = 0; i < kAccelCountMax; i++) { PX4_WARN("CMD_INJECT_FAILURE, accel %d stuck", i); _accel_blocked[i] = false; _accel_stuck[i] = true; } - } else if (instance >= 1 && instance <= ACCEL_COUNT_MAX) { + } else if (instance >= 1 && instance <= kAccelCountMax) { PX4_WARN("CMD_INJECT_FAILURE, accel %d stuck", instance - 1); _accel_blocked[instance - 1] = false; _accel_stuck[instance - 1] = true; @@ -241,13 +268,13 @@ void FailureInjection::check_failure_injections() // 0 to signal all if (instance == 0) { - for (int i = 0; i < ACCEL_COUNT_MAX; i++) { + for (int i = 0; i < kAccelCountMax; i++) { PX4_INFO("CMD_INJECT_FAILURE, accel %d ok", i); _accel_blocked[i] = false; _accel_stuck[i] = false; } - } else if (instance >= 1 && instance <= ACCEL_COUNT_MAX) { + } else if (instance >= 1 && instance <= kAccelCountMax) { PX4_INFO("CMD_INJECT_FAILURE, accel %d ok", instance - 1); _accel_blocked[instance - 1] = false; _accel_stuck[instance - 1] = false; @@ -262,13 +289,13 @@ void FailureInjection::check_failure_injections() // 0 to signal all if (instance == 0) { - for (int i = 0; i < GYRO_COUNT_MAX; i++) { + for (int i = 0; i < kGyroCountMax; i++) { PX4_WARN("CMD_INJECT_FAILURE, gyro %d off", i); _gyro_blocked[i] = true; _gyro_stuck[i] = false; } - } else if (instance >= 1 && instance <= GYRO_COUNT_MAX) { + } else if (instance >= 1 && instance <= kGyroCountMax) { PX4_WARN("CMD_INJECT_FAILURE, gyro %d off", instance - 1); _gyro_blocked[instance - 1] = true; _gyro_stuck[instance - 1] = false; @@ -279,13 +306,13 @@ void FailureInjection::check_failure_injections() // 0 to signal all if (instance == 0) { - for (int i = 0; i < GYRO_COUNT_MAX; i++) { + for (int i = 0; i < kGyroCountMax; i++) { PX4_WARN("CMD_INJECT_FAILURE, gyro %d stuck", i); _gyro_blocked[i] = false; _gyro_stuck[i] = true; } - } else if (instance >= 1 && instance <= GYRO_COUNT_MAX) { + } else if (instance >= 1 && instance <= kGyroCountMax) { PX4_INFO("CMD_INJECT_FAILURE, gyro %d stuck", instance - 1); _gyro_blocked[instance - 1] = false; _gyro_stuck[instance - 1] = true; @@ -296,13 +323,13 @@ void FailureInjection::check_failure_injections() // 0 to signal all if (instance == 0) { - for (int i = 0; i < GYRO_COUNT_MAX; i++) { + for (int i = 0; i < kGyroCountMax; i++) { PX4_INFO("CMD_INJECT_FAILURE, gyro %d ok", i); _gyro_blocked[i] = false; _gyro_stuck[i] = false; } - } else if (instance >= 1 && instance <= GYRO_COUNT_MAX) { + } else if (instance >= 1 && instance <= kGyroCountMax) { PX4_INFO("CMD_INJECT_FAILURE, gyro %d ok", instance - 1); _gyro_blocked[instance - 1] = false; _gyro_stuck[instance - 1] = false; @@ -317,13 +344,13 @@ void FailureInjection::check_failure_injections() // 0 to signal all if (instance == 0) { - for (int i = 0; i < MAG_COUNT_MAX; i++) { + for (int i = 0; i < kMagCountMax; i++) { PX4_WARN("CMD_INJECT_FAILURE, mag %d off", i); _mag_blocked[i] = true; _mag_stuck[i] = false; } - } else if (instance >= 1 && instance <= MAG_COUNT_MAX) { + } else if (instance >= 1 && instance <= kMagCountMax) { PX4_WARN("CMD_INJECT_FAILURE, mag %d off", instance - 1); _mag_blocked[instance - 1] = true; _mag_stuck[instance - 1] = false; @@ -334,13 +361,13 @@ void FailureInjection::check_failure_injections() // 0 to signal all if (instance == 0) { - for (int i = 0; i < MAG_COUNT_MAX; i++) { + for (int i = 0; i < kMagCountMax; i++) { PX4_WARN("CMD_INJECT_FAILURE, mag %d stuck", i); _mag_blocked[i] = false; _mag_stuck[i] = true; } - } else if (instance >= 1 && instance <= MAG_COUNT_MAX) { + } else if (instance >= 1 && instance <= kMagCountMax) { PX4_WARN("CMD_INJECT_FAILURE, mag %d stuck", instance - 1); _mag_blocked[instance - 1] = false; _mag_stuck[instance - 1] = true; @@ -351,13 +378,13 @@ void FailureInjection::check_failure_injections() // 0 to signal all if (instance == 0) { - for (int i = 0; i < MAG_COUNT_MAX; i++) { + for (int i = 0; i < kMagCountMax; i++) { PX4_INFO("CMD_INJECT_FAILURE, mag %d ok", i); _mag_blocked[i] = false; _mag_stuck[i] = false; } - } else if (instance >= 1 && instance <= MAG_COUNT_MAX) { + } else if (instance >= 1 && instance <= kMagCountMax) { PX4_INFO("CMD_INJECT_FAILURE, mag %d ok", instance - 1); _mag_blocked[instance - 1] = false; _mag_stuck[instance - 1] = false; diff --git a/src/modules/simulation/failure_injection/failureInjection.hpp b/src/modules/simulation/failure_injection/failureInjection.hpp index 34421886b1..c2b9c30b82 100644 --- a/src/modules/simulation/failure_injection/failureInjection.hpp +++ b/src/modules/simulation/failure_injection/failureInjection.hpp @@ -48,20 +48,17 @@ #include #include +static constexpr uint8_t kAccelCountMax = 4; +static constexpr uint8_t kGyroCountMax = 4; +static constexpr uint8_t kMagCountMax = 4; -static constexpr uint8_t ACCEL_COUNT_MAX = 4; -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; +static constexpr float kRelativeGpsDrift = 0.1f; class FailureInjection { public: - FailureInjection() {}; + FailureInjection() = default; void check_failure_injections(); bool handle_gps_failure(sensor_gps_s &gps); @@ -97,9 +94,6 @@ private: uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; - - - // GPS bool _gps_drift{false}; bool _gps_alt_drift{false}; MapProjection _mp; @@ -107,7 +101,7 @@ private: matrix::Vector2f _gps_drift_pos; double _gps_alt_offset{0}; hrt_abstime _last_gps_timestamp{0}; - hrt_abstime _t0{0}; + hrt_abstime _alt_drift_t0{0}; bool _gps_blocked{false}; bool _gps_alt_blocked{false}; @@ -117,12 +111,13 @@ private: bool _gps_alt_wrong{false}; sensor_gps_s _gps_prev{}; - bool _accel_blocked[ACCEL_COUNT_MAX] {}; - bool _accel_stuck[ACCEL_COUNT_MAX] {}; - bool _gyro_blocked[GYRO_COUNT_MAX] {}; - bool _gyro_stuck[GYRO_COUNT_MAX] {}; - bool _mag_blocked[MAG_COUNT_MAX] {}; - bool _mag_stuck[MAG_COUNT_MAX] {}; + bool _accel_blocked[kAccelCountMax] {}; + bool _accel_stuck[kAccelCountMax] {}; + bool _gyro_blocked[kGyroCountMax] {}; + bool _gyro_stuck[kGyroCountMax] {}; + + bool _mag_blocked[kMagCountMax] {}; + bool _mag_stuck[kMagCountMax] {}; bool _baro_blocked{false}; bool _baro_stuck{false}; bool _airspeed_disconnected{false}; diff --git a/src/modules/simulation/sensor_sim_manager/SensorSimManager.cpp b/src/modules/simulation/sensor_sim_manager/SensorSimManager.cpp index 5f087008a8..2938cafcbb 100644 --- a/src/modules/simulation/sensor_sim_manager/SensorSimManager.cpp +++ b/src/modules/simulation/sensor_sim_manager/SensorSimManager.cpp @@ -49,8 +49,8 @@ SensorSimManager::SensorSimManager() : _gen(_rd()), _uniform_dist(0.0f, 1.0f) { - _px4_accel.set_temperature(T1_C); - _px4_gyro.set_temperature(T1_C); + _px4_accel.set_temperature(kT1C); + _px4_gyro.set_temperature(kT1C); srand(1234); @@ -269,7 +269,7 @@ void SensorSimManager::updateGPS() const hrt_abstime now = hrt_absolute_time(); if (lpos.timestamp > 0 && gpos.timestamp > 0 && - (now - lpos.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US && (now - gpos.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US) { + (now - lpos.timestamp) < kGroundtruthDataMaxAgeUs && (now - gpos.timestamp) < kGroundtruthDataMaxAgeUs) { 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); @@ -316,7 +316,7 @@ void SensorSimManager::updateGPS() 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_m_s = gps_vel.xy().norm(); // 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); @@ -337,7 +337,6 @@ void SensorSimManager::updateGPS() // 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); - } } @@ -353,14 +352,14 @@ void SensorSimManager::updateBarometer() if (_vehicle_global_position_sub.copy(&gpos)) { const hrt_abstime now = hrt_absolute_time(); - if (gpos.timestamp > 0 && (now - gpos.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US) { + if (gpos.timestamp > 0 && (now - gpos.timestamp) < kGroundtruthDataMaxAgeUs) { 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; + const float temperature_local = kTemperatureMsl - kLapseRate * alt_msl; + const float pressure_ratio = powf(kTemperatureMsl / temperature_local, 5.256f); + const float absolute_pressure = kPressureMsl / pressure_ratio; // generate Gaussian noise sequence using polar form of Box-Muller transformation double y1; @@ -425,7 +424,7 @@ void SensorSimManager::updateMagnetometer() if (_vehicle_global_position_sub.copy(&gpos)) { const hrt_abstime now = hrt_absolute_time(); - if (gpos.timestamp > 0 && (now - gpos.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US && gpos.eph < 1000) { + if (gpos.timestamp > 0 && (now - gpos.timestamp) < kGroundtruthDataMaxAgeUs && 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)); @@ -486,8 +485,8 @@ void SensorSimManager::updateAirspeed() const hrt_abstime now = hrt_absolute_time(); if (lpos.timestamp > 0 && gpos.timestamp > 0 && attitude.timestamp > 0 && - (now - lpos.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US && (now - gpos.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US - && (now - attitude.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US) { + (now - lpos.timestamp) < kGroundtruthDataMaxAgeUs && (now - gpos.timestamp) < kGroundtruthDataMaxAgeUs + && (now - attitude.timestamp) < kGroundtruthDataMaxAgeUs) { Vector3f velocity_E{lpos.vx, lpos.vy, lpos.vz}; // Velocity in NED frame (similar to Earth frame) @@ -509,11 +508,11 @@ void SensorSimManager::updateAirspeed() // Calculate indicated airspeed using air density ratio // For now, use standard air density ratio calculation 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; + const float temperature_local = kTemperatureMsl - kLapseRate * alt_amsl; + const float density_ratio = powf(kTemperatureMsl / temperature_local, 4.256f); + const float air_density = kAirDensityMsl / density_ratio; - airspeed.indicated_airspeed_m_s = true_airspeed * sqrtf(air_density / RHO); + airspeed.indicated_airspeed_m_s = true_airspeed * sqrtf(air_density / kRho); airspeed.confidence = 0.7f; airspeed.timestamp = hrt_absolute_time(); _airspeed_pub.publish(airspeed); @@ -558,7 +557,7 @@ void SensorSimManager::updateAGP() const hrt_abstime now = hrt_absolute_time(); - if (gpos.timestamp > 0 && (now - gpos.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US) { + if (gpos.timestamp > 0 && (now - gpos.timestamp) < kGroundtruthDataMaxAgeUs) { if (_failure_injection.is_agp_blocked()) { perf_end(_agp_perf); @@ -625,9 +624,9 @@ void SensorSimManager::updateIMU() const hrt_abstime now = hrt_absolute_time(); if (gpos.timestamp > 0 && lpos.timestamp > 0 && attitude.timestamp > 0 && angular_velocity.timestamp > 0 && - (now - gpos.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US && (now - lpos.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US && - (now - attitude.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US - && (now - angular_velocity.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US) { + (now - gpos.timestamp) < kGroundtruthDataMaxAgeUs && (now - lpos.timestamp) < kGroundtruthDataMaxAgeUs && + (now - attitude.timestamp) < kGroundtruthDataMaxAgeUs + && (now - angular_velocity.timestamp) < kGroundtruthDataMaxAgeUs) { // The sensor signals reconstruction and noise levels are from [1] // [1] Bulka, Eitan, and Meyer Nahon. "Autonomous fixed-wing aerobatics: from theory to flight." @@ -699,7 +698,7 @@ void SensorSimManager::updateDistanceSensor() const hrt_abstime now = hrt_absolute_time(); if (lpos.timestamp > 0 && attitude.timestamp > 0 && - (now - lpos.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US && (now - attitude.timestamp) < GROUNDTRUTH_DATA_MAX_AGE_US) { + (now - lpos.timestamp) < kGroundtruthDataMaxAgeUs && (now - attitude.timestamp) < kGroundtruthDataMaxAgeUs) { if (_failure_injection.is_distance_sensor_blocked()) { perf_end(_distance_sensor_perf); diff --git a/src/modules/simulation/sensor_sim_manager/SensorSimManager.hpp b/src/modules/simulation/sensor_sim_manager/SensorSimManager.hpp index 7459d95e45..5b25397b8d 100644 --- a/src/modules/simulation/sensor_sim_manager/SensorSimManager.hpp +++ b/src/modules/simulation/sensor_sim_manager/SensorSimManager.hpp @@ -196,16 +196,16 @@ private: float _last_distance_sensor_value{0.0f}; // 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] - static constexpr float RHO = 1.225f; // Air density at sea level [kg/m^3] + static constexpr float kTemperatureMsl = 288.0f; // [K] + static constexpr float kPressureMsl = 101325.0f; // [Pa] + static constexpr float kLapseRate = 0.0065f; // [K/m] + static constexpr float kAirDensityMsl = 1.225f; // [kg/m^3] + static constexpr float kRho = 1.225f; // Air density at sea level [kg/m^3] // IMU constants - static constexpr float T1_C = 15.0f; // Temperature constant + static constexpr float kT1C = 15.0f; // Temperature constant - static constexpr hrt_abstime GROUNDTRUTH_DATA_MAX_AGE_US = 12000; + static constexpr hrt_abstime kGroundtruthDataMaxAgeUs = 12000; // Parameter handles for cross-module access param_t _param_sim_gz_en_handle{PARAM_INVALID}; diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index da0e0ec9b0..ac955237eb 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp @@ -197,8 +197,8 @@ void SimulatorMavlink::update_sensors(const hrt_abstime &time, const mavlink_hil // accel if ((sensors.fields_updated & SensorSource::ACCEL) == SensorSource::ACCEL) { - if (sensors.id >= ACCEL_COUNT_MAX) { - PX4_ERR("Number of simulated accelerometer %d out of range. Max: %d", sensors.id, ACCEL_COUNT_MAX); + if (sensors.id >= kAccelCountMax) { + PX4_ERR("Number of simulated accelerometer %d out of range. Max: %d", sensors.id, kAccelCountMax); return; } @@ -241,8 +241,8 @@ void SimulatorMavlink::update_sensors(const hrt_abstime &time, const mavlink_hil // gyro if ((sensors.fields_updated & SensorSource::GYRO) == SensorSource::GYRO) { - if (sensors.id >= GYRO_COUNT_MAX) { - PX4_ERR("Number of simulated gyroscope %d out of range. Max: %d", sensors.id, GYRO_COUNT_MAX); + if (sensors.id >= kGyroCountMax) { + PX4_ERR("Number of simulated gyroscope %d out of range. Max: %d", sensors.id, kGyroCountMax); return; } @@ -284,8 +284,8 @@ void SimulatorMavlink::update_sensors(const hrt_abstime &time, const mavlink_hil // magnetometer if ((sensors.fields_updated & SensorSource::MAG) == SensorSource::MAG) { - if (sensors.id >= MAG_COUNT_MAX) { - PX4_ERR("Number of simulated magnetometer %d out of range. Max: %d", sensors.id, MAG_COUNT_MAX); + if (sensors.id >= kMagCountMax) { + PX4_ERR("Number of simulated magnetometer %d out of range. Max: %d", sensors.id, kMagCountMax); return; } diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp index eea0fabbdf..897a5b3f51 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp @@ -164,22 +164,22 @@ private: static SimulatorMavlink *_instance; // simulated sensor instances - static constexpr uint8_t ACCEL_COUNT_MAX = 3; - PX4Accelerometer _px4_accel[ACCEL_COUNT_MAX] { + static constexpr uint8_t kAccelCountMax = 3; + PX4Accelerometer _px4_accel[kAccelCountMax] { {1310988, ROTATION_NONE}, // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION {1310996, ROTATION_NONE}, // 1310996: DRV_IMU_DEVTYPE_SIM, BUS: 2, ADDR: 1, TYPE: SIMULATION {1311004, ROTATION_NONE}, // 1311004: DRV_IMU_DEVTYPE_SIM, BUS: 3, ADDR: 1, TYPE: SIMULATION }; - static constexpr uint8_t GYRO_COUNT_MAX = 3; - PX4Gyroscope _px4_gyro[GYRO_COUNT_MAX] { + static constexpr uint8_t kGyroCountMax = 3; + PX4Gyroscope _px4_gyro[kGyroCountMax] { {1310988, ROTATION_NONE}, // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION {1310996, ROTATION_NONE}, // 1310996: DRV_IMU_DEVTYPE_SIM, BUS: 2, ADDR: 1, TYPE: SIMULATION {1311004, ROTATION_NONE}, // 1311004: DRV_IMU_DEVTYPE_SIM, BUS: 3, ADDR: 1, TYPE: SIMULATION }; - static constexpr uint8_t MAG_COUNT_MAX = 2; - PX4Magnetometer _px4_mag[MAG_COUNT_MAX] { + static constexpr uint8_t kMagCountMax = 2; + PX4Magnetometer _px4_mag[kMagCountMax] { {197388, ROTATION_NONE}, {197644, ROTATION_NONE}, }; @@ -279,15 +279,12 @@ private: battery_status_s _battery_status{}; sensor_accel_fifo_s _last_accel_fifo{}; - matrix::Vector3f _last_accel[GYRO_COUNT_MAX] {}; + matrix::Vector3f _last_accel[kGyroCountMax] {}; sensor_gyro_fifo_s _last_gyro_fifo{}; - matrix::Vector3f _last_gyro[GYRO_COUNT_MAX] {}; + matrix::Vector3f _last_gyro[kGyroCountMax] {}; - - - - matrix::Vector3f _last_mag[MAG_COUNT_MAX] {}; + matrix::Vector3f _last_mag[kMagCountMax] {}; float _last_baro_pressure{0.0f}; float _last_baro_temperature{0.0f};