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

lots of renaming of constant values

Browse Source
This commit is contained in:
Marco Hauswirth
2025-10-03 10:41:12 +02:00
parent 2f71fc382c
commit 47fc3643ae
6 changed files with 116 additions and 98 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulation/failure_injection/failureInjection.cpp
+61 -34
View File
@@ -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 <simulation/failure_injection/failureInjection.hpp>
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;
src/modules/simulation/failure_injection/failureInjection.hpp
+13 -18
View File
@@ -48,20 +48,17 @@
#include <lib/geo/geo.h>
#include <matrix/math.hpp>
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<vehicle_command_ack_s> _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};
src/modules/simulation/sensor_sim_manager/SensorSimManager.cpp
+20 -21
View File
@@ -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);
src/modules/simulation/sensor_sim_manager/SensorSimManager.hpp
+7 -7
View File
@@ -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};
src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp
+6 -6
View File
@@ -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;
}
src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp
+9 -12
View File
@@ -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};