Skip to content

Commit

Permalink
Linting and formatting
Browse files Browse the repository at this point in the history
  • Loading branch information
@juniorsundar-tii
juniorsundar-tii committed Feb 12, 2024
1 parent fc316e6 commit 1b9f666
Show file tree
Hide file tree
Showing 9 changed files with 167 additions and 195 deletions.
20 changes: 10 additions & 10 deletions src/modules/sensors/sensor_params.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -237,7 +237,7 @@ PARAM_DEFINE_INT32(SENS_INT_BARO_EN, 1);
/**
* **************************************************************************************************
* CUSTOM FAULT PARAMETERS
* **************************************************************************************************
* **************************************************************************************************
*/
/**
* Trigger to expose fault injection modules
Expand Down Expand Up @@ -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
*
Expand All @@ -315,7 +315,7 @@ PARAM_DEFINE_INT32(SENS_ACCEL_ZERO, 0);
*/
/**
* Trigger to expose fault injection modules
*
*
* Target - Sensor: Gyroscope
*
* @value 0 Functional
Expand Down Expand Up @@ -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
*
Expand All @@ -389,7 +389,7 @@ PARAM_DEFINE_INT32(SENS_GYRO_ZERO, 0);
*/
/**
* Trigger to expose fault injection modules
*
*
* Target - Sensor: Magnetometer
*
* @value 0 Functional
Expand Down Expand Up @@ -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
*
Expand All @@ -462,7 +462,7 @@ PARAM_DEFINE_INT32(SENS_MAG_ZERO, 0);
*/
/**
* Trigger to expose fault injection modules
*
*
* Target - Sensor: Barometer
*
* @value 0 Functional
Expand Down Expand Up @@ -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
*
Expand All @@ -535,7 +535,7 @@ PARAM_DEFINE_INT32(SENS_BARO_ZERO, 0);
*/
/**
* Trigger to expose fault injection modules
*
*
* Target - Sensor: GPS
*
* @value 0 Functional
Expand Down Expand Up @@ -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
*
Expand Down
80 changes: 34 additions & 46 deletions src/modules/sensors/vehicle_imu/VehicleIMU.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
Expand All @@ -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;
Expand All @@ -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();
Expand Down
2 changes: 1 addition & 1 deletion src/modules/sensors/vehicle_imu/VehicleIMU.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<px4::params::IMU_INTEG_RATE>) _param_imu_integ_rate,
(ParamBool<px4::params::SENS_IMU_AUTOCAL>) _param_sens_imu_autocal
Expand Down
97 changes: 46 additions & 51 deletions src/modules/simulation/sensor_baro_sim/SensorBaroSim.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ SensorBaroSim::SensorBaroSim() :
SensorBaroSim::~SensorBaroSim()
{
perf_free(_loop_perf);
baro_drift_timestep = 0;
baro_drift_timestep = 0;
}

bool SensorBaroSim::init()
Expand Down Expand Up @@ -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);


Expand Down
4 changes: 2 additions & 2 deletions src/modules/simulation/sensor_baro_sim/SensorBaroSim.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,8 +70,8 @@ class SensorBaroSim : public ModuleBase<SensorBaroSim>, 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)};

Expand Down
Loading

0 comments on commit 1b9f666

Please sign in to comment.