mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-15 19:13:34 +08:00
lots of renaming of constant values
This commit is contained in:
Marco Hauswirth
@@ -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;
|
||||
|
||||
@@ -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};
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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};
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
|
||||
@@ -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};
|
||||
|
||||
Reference in New Issue
Block a user