diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 3ad0610afb17..efea8588f220 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -237,7 +237,7 @@ PARAM_DEFINE_INT32(SENS_INT_BARO_EN, 1); /** * ************************************************************************************************** * CUSTOM FAULT PARAMETERS - * ************************************************************************************************** + * ************************************************************************************************** */ /** * Trigger to expose fault injection modules @@ -297,7 +297,7 @@ PARAM_DEFINE_FLOAT(SENS_ACCEL_SCAL, 0); PARAM_DEFINE_FLOAT(SENS_ACCEL_DRIFT, 0); /** - * Set to zero + * Set to zero * * Target - Sensor: Accelerometer * @@ -315,7 +315,7 @@ PARAM_DEFINE_INT32(SENS_ACCEL_ZERO, 0); */ /** * Trigger to expose fault injection modules - * + * * Target - Sensor: Gyroscope * * @value 0 Functional @@ -371,7 +371,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_SCAL, 0); PARAM_DEFINE_FLOAT(SENS_GYRO_DRIFT, 0); /** - * Set to zero + * Set to zero * * Target - Sensor: Gyroscope * @@ -389,7 +389,7 @@ PARAM_DEFINE_INT32(SENS_GYRO_ZERO, 0); */ /** * Trigger to expose fault injection modules - * + * * Target - Sensor: Magnetometer * * @value 0 Functional @@ -445,7 +445,7 @@ PARAM_DEFINE_FLOAT(SENS_MAG_SCAL, 0); PARAM_DEFINE_FLOAT(SENS_MAG_DRIFT, 0); /** - * Set to zero + * Set to zero * * Target - Sensor: Magnetometer * @@ -462,7 +462,7 @@ PARAM_DEFINE_INT32(SENS_MAG_ZERO, 0); */ /** * Trigger to expose fault injection modules - * + * * Target - Sensor: Barometer * * @value 0 Functional @@ -518,7 +518,7 @@ PARAM_DEFINE_FLOAT(SENS_BARO_SCAL, 0); PARAM_DEFINE_FLOAT(SENS_BARO_DRIFT, 0); /** - * Set to zero + * Set to zero * * Target - Sensor: Barometer * @@ -535,7 +535,7 @@ PARAM_DEFINE_INT32(SENS_BARO_ZERO, 0); */ /** * Trigger to expose fault injection modules - * + * * Target - Sensor: GPS * * @value 0 Functional @@ -591,7 +591,7 @@ PARAM_DEFINE_FLOAT(SENS_GPS_SCAL, 0); PARAM_DEFINE_FLOAT(SENS_GPS_DRIFT, 0); /** - * Set to zero + * Set to zero * * Target - Sensor: GPS * diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index fbb21486d4c1..e84dec5110c5 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -679,42 +679,38 @@ bool VehicleIMU::Publish() imu.gyro_device_id = _gyro_calibration.device_id(); delta_angle_corrected.copyTo(imu.delta_angle); delta_velocity_corrected.copyTo(imu.delta_velocity); - + // Adding faults to the accelerometer param_t accel_fault = param_find("SENS_ACCEL_FAULT"); int32_t accel_fault_flag; param_get(accel_fault, &accel_fault_flag); - if (accel_fault_flag == 1) - { + if (accel_fault_flag == 1) { param_t accel_noise = param_find("SENS_ACCEL_NOISE"); float_t accel_noise_flag; param_get(accel_noise, &accel_noise_flag); - if (abs(accel_noise_flag) > 0) - { - imu.delta_velocity[0] += imu.delta_velocity[0]*generate_wgn()*accel_noise_flag; - imu.delta_velocity[1] += imu.delta_velocity[1]*generate_wgn()*accel_noise_flag; - imu.delta_velocity[2] += imu.delta_velocity[2]*generate_wgn()*accel_noise_flag; + if (abs(accel_noise_flag) > 0) { + imu.delta_velocity[0] += imu.delta_velocity[0] * generate_wgn() * accel_noise_flag; + imu.delta_velocity[1] += imu.delta_velocity[1] * generate_wgn() * accel_noise_flag; + imu.delta_velocity[2] += imu.delta_velocity[2] * generate_wgn() * accel_noise_flag; } param_t accel_bias_shift = param_find("SENS_ACCEL_SHIF"); float_t accel_bias_shift_flag; param_get(accel_bias_shift, &accel_bias_shift_flag); - if (abs(accel_bias_shift_flag) > 0) - { - imu.delta_velocity[0] += imu.delta_velocity[0]*accel_bias_shift_flag; - imu.delta_velocity[1] += imu.delta_velocity[1]*accel_bias_shift_flag; - imu.delta_velocity[2] += imu.delta_velocity[2]*accel_bias_shift_flag; + if (abs(accel_bias_shift_flag) > 0) { + imu.delta_velocity[0] += imu.delta_velocity[0] * accel_bias_shift_flag; + imu.delta_velocity[1] += imu.delta_velocity[1] * accel_bias_shift_flag; + imu.delta_velocity[2] += imu.delta_velocity[2] * accel_bias_shift_flag; } param_t accel_bias_scale = param_find("SENS_ACCEL_SCAL"); float_t accel_bias_scale_flag; param_get(accel_bias_scale, &accel_bias_scale_flag); - if (abs(accel_bias_scale_flag) > 0) - { + if (abs(accel_bias_scale_flag) > 0) { imu.delta_velocity[0] *= accel_bias_scale_flag; imu.delta_velocity[1] *= accel_bias_scale_flag; imu.delta_velocity[2] *= accel_bias_scale_flag; @@ -724,21 +720,19 @@ bool VehicleIMU::Publish() float_t accel_drift_flag; param_get(accel_drift, &accel_drift_flag); - if (abs(accel_drift_flag) > 0) - { - imu.delta_velocity[0] += 0.01f*accel_drift_flag*accel_drift_timestep/1000000; - imu.delta_velocity[1] += 0.01f*accel_drift_flag*accel_drift_timestep/1000000; - imu.delta_velocity[2] += 0.01f*accel_drift_flag*accel_drift_timestep/1000000; + if (abs(accel_drift_flag) > 0) { + imu.delta_velocity[0] += 0.01f * accel_drift_flag * accel_drift_timestep / 1000000; + imu.delta_velocity[1] += 0.01f * accel_drift_flag * accel_drift_timestep / 1000000; + imu.delta_velocity[2] += 0.01f * accel_drift_flag * accel_drift_timestep / 1000000; accel_drift_timestep += 1; } - param_t accel_zero = param_find("SENS_ACCEL_ZERO"); + param_t accel_zero = param_find("SENS_ACCEL_ZERO"); int32_t accel_zero_flag; param_get(accel_zero, &accel_zero_flag); - if (accel_zero_flag == 1) - { + if (accel_zero_flag == 1) { imu.delta_velocity[0] = 0; imu.delta_velocity[1] = 0; imu.delta_velocity[2] = 0; @@ -750,36 +744,32 @@ bool VehicleIMU::Publish() int32_t gyro_fault_flag; param_get(gyro_fault, &gyro_fault_flag); - if (gyro_fault_flag == 1) - { + if (gyro_fault_flag == 1) { param_t gyro_noise = param_find("SENS_GYRO_NOISE"); float_t gyro_noise_flag; param_get(gyro_noise, &gyro_noise_flag); - if (abs(gyro_noise_flag) > 0) - { - imu.delta_angle[0] += imu.delta_angle[0]*generate_wgn()*gyro_noise_flag; - imu.delta_angle[1] += imu.delta_angle[0]*generate_wgn()*gyro_noise_flag; - imu.delta_angle[2] += imu.delta_angle[0]*generate_wgn()*gyro_noise_flag; + if (abs(gyro_noise_flag) > 0) { + imu.delta_angle[0] += imu.delta_angle[0] * generate_wgn() * gyro_noise_flag; + imu.delta_angle[1] += imu.delta_angle[0] * generate_wgn() * gyro_noise_flag; + imu.delta_angle[2] += imu.delta_angle[0] * generate_wgn() * gyro_noise_flag; } param_t gyro_bias_shift = param_find("SENS_GYRO_SHIF"); float_t gyro_bias_shift_flag; param_get(gyro_bias_shift, &gyro_bias_shift_flag); - if (abs(gyro_bias_shift_flag) > 0) - { - imu.delta_angle[0] += imu.delta_angle[0]*gyro_bias_shift_flag; - imu.delta_angle[1] += imu.delta_angle[1]*gyro_bias_shift_flag; - imu.delta_angle[2] += imu.delta_angle[2]*gyro_bias_shift_flag; + if (abs(gyro_bias_shift_flag) > 0) { + imu.delta_angle[0] += imu.delta_angle[0] * gyro_bias_shift_flag; + imu.delta_angle[1] += imu.delta_angle[1] * gyro_bias_shift_flag; + imu.delta_angle[2] += imu.delta_angle[2] * gyro_bias_shift_flag; } param_t gyro_bias_scale = param_find("SENS_GYRO_SCAL"); float_t gyro_bias_scale_flag; param_get(gyro_bias_scale, &gyro_bias_scale_flag); - if (abs(gyro_bias_scale_flag) > 0) - { + if (abs(gyro_bias_scale_flag) > 0) { imu.delta_angle[0] *= gyro_bias_scale_flag; imu.delta_angle[1] *= gyro_bias_scale_flag; imu.delta_angle[2] *= gyro_bias_scale_flag; @@ -789,27 +779,25 @@ bool VehicleIMU::Publish() float_t gyro_drift_flag; param_get(gyro_drift, &gyro_drift_flag); - if (abs(gyro_drift_flag) > 0) - { - imu.delta_angle[0] += 0.01f*gyro_drift_flag*gyro_drift_timestep/1000000; - imu.delta_angle[1] += 0.01f*gyro_drift_flag*gyro_drift_timestep/1000000; - imu.delta_angle[2] += 0.01f*gyro_drift_flag*gyro_drift_timestep/1000000; + if (abs(gyro_drift_flag) > 0) { + imu.delta_angle[0] += 0.01f * gyro_drift_flag * gyro_drift_timestep / 1000000; + imu.delta_angle[1] += 0.01f * gyro_drift_flag * gyro_drift_timestep / 1000000; + imu.delta_angle[2] += 0.01f * gyro_drift_flag * gyro_drift_timestep / 1000000; gyro_drift_timestep += 1; } - param_t gyro_zero = param_find("SENS_GYRO_ZERO"); + param_t gyro_zero = param_find("SENS_GYRO_ZERO"); int32_t gyro_zero_flag; param_get(gyro_zero, &gyro_zero_flag); - if (gyro_zero_flag == 1) - { + if (gyro_zero_flag == 1) { imu.delta_angle[0] = 0; imu.delta_angle[1] = 0; imu.delta_angle[2] = 0; } } - + imu.delta_velocity_clipping = _delta_velocity_clipping; imu.accel_calibration_count = _accel_calibration.calibration_count(); imu.gyro_calibration_count = _gyro_calibration.calibration_count(); diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp index 9601721e6cdd..602392531ab4 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp @@ -197,7 +197,7 @@ class VehicleIMU : public ModuleParams, public px4::ScheduledWorkItem perf_counter_t _gyro_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro data gap")}; int accel_drift_timestep, gyro_drift_timestep; - + DEFINE_PARAMETERS( (ParamInt) _param_imu_integ_rate, (ParamBool) _param_sens_imu_autocal diff --git a/src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp b/src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp index 6cad61f9e959..5f17641bdef3 100644 --- a/src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp +++ b/src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp @@ -47,7 +47,7 @@ SensorBaroSim::SensorBaroSim() : SensorBaroSim::~SensorBaroSim() { perf_free(_loop_perf); - baro_drift_timestep = 0; + baro_drift_timestep = 0; } bool SensorBaroSim::init() @@ -169,56 +169,51 @@ void SensorBaroSim::Run() sensor_baro.pressure = pressure; sensor_baro.temperature = temperature; sensor_baro.timestamp = hrt_absolute_time(); - - // Adding faults to the barometer - param_t baro_fault = param_find("SENS_BARO_FAULT"); - int32_t baro_fault_flag; - param_get(baro_fault, &baro_fault_flag); - - if (baro_fault_flag == 1) - { - param_t baro_noise = param_find("SENS_BARO_NOISE"); - float_t baro_noise_flag; - param_get(baro_noise, &baro_noise_flag); - - if (abs(baro_noise_flag) > 0) - { - sensor_baro.pressure += sensor_baro.pressure*generate_wgn()*baro_noise_flag; - sensor_baro.temperature += sensor_baro.temperature*generate_wgn()*baro_noise_flag; - } - - param_t baro_bias_shift = param_find("SENS_BARO_SHIF"); - float_t baro_bias_shift_flag; - param_get(baro_bias_shift, &baro_bias_shift_flag); - - if (abs(baro_bias_shift_flag) > 0) - { - sensor_baro.pressure += sensor_baro.pressure*baro_bias_shift_flag; - sensor_baro.temperature += sensor_baro.temperature*baro_bias_shift_flag; - } - - param_t baro_bias_scale = param_find("SENS_BARO_SCAL"); - float_t baro_bias_scale_flag; - param_get(baro_bias_scale, &baro_bias_scale_flag); - - if (abs(baro_bias_scale_flag) > 0) - { - sensor_baro.pressure *= sensor_baro.pressure*baro_bias_scale_flag; - sensor_baro.temperature *= sensor_baro.temperature*baro_bias_scale_flag; - } - - param_t baro_drift = param_find("SENS_BARO_DRIFT"); - float_t baro_drift_flag; - param_get(baro_drift, &baro_drift_flag); - - if (abs(baro_drift_flag) > 0) - { - sensor_baro.pressure += 0.01f*baro_drift_flag*baro_drift_timestep/1000000; - sensor_baro.temperature += 0.01f*baro_drift_flag*baro_drift_timestep/1000000; - baro_drift_timestep += 1; - } - } - + + // Adding faults to the barometer + param_t baro_fault = param_find("SENS_BARO_FAULT"); + int32_t baro_fault_flag; + param_get(baro_fault, &baro_fault_flag); + + if (baro_fault_flag == 1) { + param_t baro_noise = param_find("SENS_BARO_NOISE"); + float_t baro_noise_flag; + param_get(baro_noise, &baro_noise_flag); + + if (abs(baro_noise_flag) > 0) { + sensor_baro.pressure += sensor_baro.pressure * generate_wgn() * baro_noise_flag; + sensor_baro.temperature += sensor_baro.temperature * generate_wgn() * baro_noise_flag; + } + + param_t baro_bias_shift = param_find("SENS_BARO_SHIF"); + float_t baro_bias_shift_flag; + param_get(baro_bias_shift, &baro_bias_shift_flag); + + if (abs(baro_bias_shift_flag) > 0) { + sensor_baro.pressure += sensor_baro.pressure * baro_bias_shift_flag; + sensor_baro.temperature += sensor_baro.temperature * baro_bias_shift_flag; + } + + param_t baro_bias_scale = param_find("SENS_BARO_SCAL"); + float_t baro_bias_scale_flag; + param_get(baro_bias_scale, &baro_bias_scale_flag); + + if (abs(baro_bias_scale_flag) > 0) { + sensor_baro.pressure *= sensor_baro.pressure * baro_bias_scale_flag; + sensor_baro.temperature *= sensor_baro.temperature * baro_bias_scale_flag; + } + + param_t baro_drift = param_find("SENS_BARO_DRIFT"); + float_t baro_drift_flag; + param_get(baro_drift, &baro_drift_flag); + + if (abs(baro_drift_flag) > 0) { + sensor_baro.pressure += 0.01f * baro_drift_flag * baro_drift_timestep / 1000000; + sensor_baro.temperature += 0.01f * baro_drift_flag * baro_drift_timestep / 1000000; + baro_drift_timestep += 1; + } + } + _sensor_baro_pub.publish(sensor_baro); diff --git a/src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp b/src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp index 4dd8b7984130..7b8a10f0a410 100644 --- a/src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp +++ b/src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp @@ -70,8 +70,8 @@ class SensorBaroSim : public ModuleBase, public ModuleParams, pub // generate white Gaussian noise sample with std=1 static float generate_wgn(); - int baro_drift_timestep; - + int baro_drift_timestep; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)}; diff --git a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp index 071e8f9769f4..158428102731 100644 --- a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp +++ b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp @@ -181,38 +181,34 @@ void SensorGpsSim::Run() int32_t gps_fault_flag; param_get(gps_fault, &gps_fault_flag); - if (gps_fault_flag == 1) - { + if (gps_fault_flag == 1) { param_t gps_noise = param_find("SENS_GPS_NOISE"); float_t gps_noise_flag; param_get(gps_noise, &gps_noise_flag); - if (abs(gps_noise_flag) > 0) - { - sensor_gps.lat += sensor_gps.lat*(double)(generate_wgn()*gps_noise_flag); - sensor_gps.lon += sensor_gps.lon*(double)(generate_wgn()*gps_noise_flag); - sensor_gps.alt += sensor_gps.alt*(double)(generate_wgn()*gps_noise_flag); - sensor_gps.alt_ellipsoid += sensor_gps.alt_ellipsoid*(double)(generate_wgn()*gps_noise_flag); + if (abs(gps_noise_flag) > 0) { + sensor_gps.lat += sensor_gps.lat * (double)(generate_wgn() * gps_noise_flag); + sensor_gps.lon += sensor_gps.lon * (double)(generate_wgn() * gps_noise_flag); + sensor_gps.alt += sensor_gps.alt * (double)(generate_wgn() * gps_noise_flag); + sensor_gps.alt_ellipsoid += sensor_gps.alt_ellipsoid * (double)(generate_wgn() * gps_noise_flag); } param_t gps_bias_shift = param_find("SENS_GPS_SHIF"); float_t gps_bias_shift_flag; param_get(gps_bias_shift, &gps_bias_shift_flag); - if (abs(gps_bias_shift_flag) > 0) - { - sensor_gps.lat += sensor_gps.lat*static_cast(gps_bias_shift_flag); - sensor_gps.lon += sensor_gps.lon*static_cast(gps_bias_shift_flag); - sensor_gps.alt += sensor_gps.alt*static_cast(gps_bias_shift_flag); - sensor_gps.alt_ellipsoid += sensor_gps.alt_ellipsoid*static_cast(gps_bias_shift_flag); + if (abs(gps_bias_shift_flag) > 0) { + sensor_gps.lat += sensor_gps.lat * static_cast(gps_bias_shift_flag); + sensor_gps.lon += sensor_gps.lon * static_cast(gps_bias_shift_flag); + sensor_gps.alt += sensor_gps.alt * static_cast(gps_bias_shift_flag); + sensor_gps.alt_ellipsoid += sensor_gps.alt_ellipsoid * static_cast(gps_bias_shift_flag); } param_t gps_bias_scale = param_find("SENS_GPS_SCAL"); float_t gps_bias_scale_flag; param_get(gps_bias_scale, &gps_bias_scale_flag); - if (abs(gps_bias_scale_flag) > 0) - { + if (abs(gps_bias_scale_flag) > 0) { sensor_gps.lat *= static_cast(gps_bias_scale_flag); sensor_gps.lon *= static_cast(gps_bias_scale_flag); sensor_gps.alt *= static_cast(gps_bias_scale_flag); @@ -223,12 +219,11 @@ void SensorGpsSim::Run() float_t gps_drift_flag; param_get(gps_drift, &gps_drift_flag); - if (abs(gps_drift_flag) > 0) - { - sensor_gps.lat += 0.01*static_cast(gps_drift_flag)*gps_drift_timestep/1000000; - sensor_gps.lon += 0.01*static_cast(gps_drift_flag)*gps_drift_timestep/1000000; - sensor_gps.alt += 0.01*static_cast(gps_drift_flag)*gps_drift_timestep/1000000; - sensor_gps.alt_ellipsoid += 0.01*static_cast(gps_drift_flag)*gps_drift_timestep/1000000; + if (abs(gps_drift_flag) > 0) { + sensor_gps.lat += 0.01 * static_cast(gps_drift_flag) * gps_drift_timestep / 1000000; + sensor_gps.lon += 0.01 * static_cast(gps_drift_flag) * gps_drift_timestep / 1000000; + sensor_gps.alt += 0.01 * static_cast(gps_drift_flag) * gps_drift_timestep / 1000000; + sensor_gps.alt_ellipsoid += 0.01 * static_cast(gps_drift_flag) * gps_drift_timestep / 1000000; gps_drift_timestep += 1; } @@ -237,8 +232,7 @@ void SensorGpsSim::Run() int32_t gps_zero_flag; param_get(gps_zero, &gps_zero_flag); - if (gps_zero_flag == 1) - { + if (gps_zero_flag == 1) { sensor_gps.lat = 0; sensor_gps.lon = 0; sensor_gps.alt = 0; diff --git a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp index d1c84dc701e5..d8275f1715d1 100644 --- a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp +++ b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp @@ -70,8 +70,8 @@ class SensorGpsSim : public ModuleBase, public ModuleParams, publi // generate white Gaussian noise sample with std=1 static float generate_wgn(); - int gps_drift_timestep; - + int gps_drift_timestep; + // 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); } diff --git a/src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp b/src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp index 5345f184ffad..b68c4706eeff 100644 --- a/src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp +++ b/src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp @@ -42,7 +42,7 @@ SensorMagSim::SensorMagSim() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) { - mag_drift_timestep = 0; + mag_drift_timestep = 0; _px4_mag.set_device_type(DRV_MAG_DEVTYPE_MAGSIM); } @@ -137,63 +137,58 @@ void SensorMagSim::Run() expected_field(0) + _sim_mag_offset_x.get(), expected_field(1) + _sim_mag_offset_y.get(), expected_field(2) + _sim_mag_offset_z.get()); - + // Adding faults to the magnetometer - param_t mag_fault = param_find("SENS_MAG_FAULT"); - int32_t mag_fault_flag; - param_get(mag_fault, &mag_fault_flag); - - if (mag_fault_flag == 1) - { - param_t mag_noise = param_find("SENS_MAG_NOISE"); - float_t mag_noise_flag; - param_get(mag_noise, &mag_noise_flag); - - if (abs(mag_noise_flag) > 0) - { - _px4_mag.update(attitude.timestamp, - (1 + generate_wgn()*mag_noise_flag) * expected_field(0), - (1 + generate_wgn()*mag_noise_flag) * expected_field(1), - (1 + generate_wgn()*mag_noise_flag) * expected_field(2)); - } - - param_t mag_bias_shift = param_find("SENS_MAG_SHIF"); - float_t mag_bias_shift_flag; - param_get(mag_bias_shift, &mag_bias_shift_flag); - - if (abs(mag_bias_shift_flag) > 0) - { - _px4_mag.update(attitude.timestamp, - (1 + mag_bias_shift_flag) * expected_field(0), - (1 + mag_bias_shift_flag) * expected_field(1), - (1 + mag_bias_shift_flag) * expected_field(2)); - } - - param_t mag_bias_scale = param_find("SENS_MAG_SCAL"); - float_t mag_bias_scale_flag; - param_get(mag_bias_scale, &mag_bias_scale_flag); - - if (abs(mag_bias_scale_flag) > 0) - { - _px4_mag.update(attitude.timestamp, - mag_bias_scale_flag * expected_field(0), - mag_bias_scale_flag * expected_field(1), - mag_bias_scale_flag * expected_field(2)); - } - - param_t mag_drift = param_find("SENS_MAG_DRIFT"); - float_t mag_drift_flag; - param_get(mag_drift, &mag_drift_flag); - - if (abs(mag_drift_flag) > 0) - { - _px4_mag.update(attitude.timestamp, - 0.01f*mag_drift_flag*mag_drift_timestep/1000000 + expected_field(0), - 0.01f*mag_drift_flag*mag_drift_timestep/1000000 + expected_field(1), - 0.01f*mag_drift_flag*mag_drift_timestep/1000000 + expected_field(2)); - mag_drift_timestep += 1; - } - } + param_t mag_fault = param_find("SENS_MAG_FAULT"); + int32_t mag_fault_flag; + param_get(mag_fault, &mag_fault_flag); + + if (mag_fault_flag == 1) { + param_t mag_noise = param_find("SENS_MAG_NOISE"); + float_t mag_noise_flag; + param_get(mag_noise, &mag_noise_flag); + + if (abs(mag_noise_flag) > 0) { + _px4_mag.update(attitude.timestamp, + (1 + generate_wgn()*mag_noise_flag) * expected_field(0), + (1 + generate_wgn()*mag_noise_flag) * expected_field(1), + (1 + generate_wgn()*mag_noise_flag) * expected_field(2)); + } + + param_t mag_bias_shift = param_find("SENS_MAG_SHIF"); + float_t mag_bias_shift_flag; + param_get(mag_bias_shift, &mag_bias_shift_flag); + + if (abs(mag_bias_shift_flag) > 0) { + _px4_mag.update(attitude.timestamp, + (1 + mag_bias_shift_flag) * expected_field(0), + (1 + mag_bias_shift_flag) * expected_field(1), + (1 + mag_bias_shift_flag) * expected_field(2)); + } + + param_t mag_bias_scale = param_find("SENS_MAG_SCAL"); + float_t mag_bias_scale_flag; + param_get(mag_bias_scale, &mag_bias_scale_flag); + + if (abs(mag_bias_scale_flag) > 0) { + _px4_mag.update(attitude.timestamp, + mag_bias_scale_flag * expected_field(0), + mag_bias_scale_flag * expected_field(1), + mag_bias_scale_flag * expected_field(2)); + } + + param_t mag_drift = param_find("SENS_MAG_DRIFT"); + float_t mag_drift_flag; + param_get(mag_drift, &mag_drift_flag); + + if (abs(mag_drift_flag) > 0) { + _px4_mag.update(attitude.timestamp, + 0.01f * mag_drift_flag * mag_drift_timestep / 1000000 + expected_field(0), + 0.01f * mag_drift_flag * mag_drift_timestep / 1000000 + expected_field(1), + 0.01f * mag_drift_flag * mag_drift_timestep / 1000000 + expected_field(2)); + mag_drift_timestep += 1; + } + } } } diff --git a/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp b/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp index 18c21d24f1f4..8e1cc699a4b9 100644 --- a/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp +++ b/src/modules/simulation/sensor_mag_sim/SensorMagSim.hpp @@ -70,8 +70,8 @@ class SensorMagSim : public ModuleBase, public ModuleParams, publi // generate white Gaussian noise sample with std=1 static float generate_wgn(); - int mag_drift_timestep; - + int mag_drift_timestep; + // 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); }