From 8d91861ea8d44351c5196f41ba1434927ab8ae14 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 26 Mar 2015 16:57:49 +0100 Subject: [PATCH 1/2] [nps] sensors: only use defines to init vars so it can be changed at runtime --- sw/simulator/nps/nps_sensor_accel.c | 17 +++++++------- sw/simulator/nps/nps_sensor_accel.h | 4 ++-- sw/simulator/nps/nps_sensor_baro.c | 17 +++++++++----- sw/simulator/nps/nps_sensor_baro.h | 5 ++-- sw/simulator/nps/nps_sensor_gps.c | 23 ++++++++++-------- sw/simulator/nps/nps_sensor_gps.h | 12 +++++----- sw/simulator/nps/nps_sensor_gyro.c | 34 +++++++++++++-------------- sw/simulator/nps/nps_sensor_gyro.h | 4 ++-- sw/simulator/nps/nps_sensor_mag.c | 21 ++++++++++------- sw/simulator/nps/nps_sensor_mag.h | 4 ++-- sw/simulator/nps/nps_sensor_sonar.c | 15 ++++++++---- sw/simulator/nps/nps_sensor_sonar.h | 12 ++++++---- sw/simulator/nps/nps_sensors.c | 23 +++++++++++------- sw/simulator/nps/nps_sensors_utils.c | 35 +++++++++++++++------------- sw/simulator/nps/nps_sensors_utils.h | 8 +++---- 15 files changed, 132 insertions(+), 102 deletions(-) diff --git a/sw/simulator/nps/nps_sensor_accel.c b/sw/simulator/nps/nps_sensor_accel.c index 7a4a2256d5..d75427962d 100644 --- a/sw/simulator/nps/nps_sensor_accel.c +++ b/sw/simulator/nps/nps_sensor_accel.c @@ -7,27 +7,28 @@ #include NPS_SENSORS_PARAMS #include "math/pprz_algebra_int.h" -void nps_sensor_accel_init(struct NpsSensorAccel* accel, double time) +void nps_sensor_accel_init(struct NpsSensorAccel *accel, double time) { FLOAT_VECT3_ZERO(accel->value); accel->min = NPS_ACCEL_MIN; accel->max = NPS_ACCEL_MAX; FLOAT_MAT33_DIAG(accel->sensitivity, - NPS_ACCEL_SENSITIVITY_XX, NPS_ACCEL_SENSITIVITY_YY, NPS_ACCEL_SENSITIVITY_ZZ); + NPS_ACCEL_SENSITIVITY_XX, NPS_ACCEL_SENSITIVITY_YY, NPS_ACCEL_SENSITIVITY_ZZ); VECT3_ASSIGN(accel->neutral, - NPS_ACCEL_NEUTRAL_X, NPS_ACCEL_NEUTRAL_Y, NPS_ACCEL_NEUTRAL_Z); + NPS_ACCEL_NEUTRAL_X, NPS_ACCEL_NEUTRAL_Y, NPS_ACCEL_NEUTRAL_Z); VECT3_ASSIGN(accel->noise_std_dev, - NPS_ACCEL_NOISE_STD_DEV_X, NPS_ACCEL_NOISE_STD_DEV_Y, NPS_ACCEL_NOISE_STD_DEV_Z); + NPS_ACCEL_NOISE_STD_DEV_X, NPS_ACCEL_NOISE_STD_DEV_Y, NPS_ACCEL_NOISE_STD_DEV_Z); VECT3_ASSIGN(accel->bias, - NPS_ACCEL_BIAS_X, NPS_ACCEL_BIAS_Y, NPS_ACCEL_BIAS_Z); + NPS_ACCEL_BIAS_X, NPS_ACCEL_BIAS_Y, NPS_ACCEL_BIAS_Z); accel->next_update = time; accel->data_available = FALSE; } -void nps_sensor_accel_run_step(struct NpsSensorAccel* accel, double time, struct DoubleRMat* body_to_imu) +void nps_sensor_accel_run_step(struct NpsSensorAccel *accel, double time, struct DoubleRMat *body_to_imu) { - if (time < accel->next_update) + if (time < accel->next_update) { return; + } /* transform to imu frame */ struct DoubleVect3 accelero_imu; @@ -44,7 +45,7 @@ void nps_sensor_accel_run_step(struct NpsSensorAccel* accel, double time, struct /* white noise */ double_vect3_add_gaussian_noise(&accelero_error, &accel->noise_std_dev); /* scale */ - struct DoubleVect3 gain = {NPS_ACCEL_SENSITIVITY_XX, NPS_ACCEL_SENSITIVITY_YY, NPS_ACCEL_SENSITIVITY_ZZ}; + struct DoubleVect3 gain = {accel->sensitivity.m[0], accel->sensitivity.m[4], accel->sensitivity.m[8]}; VECT3_EW_MUL(accelero_error, accelero_error, gain); /* add error */ VECT3_ADD(accel->value, accelero_error); diff --git a/sw/simulator/nps/nps_sensor_accel.h b/sw/simulator/nps/nps_sensor_accel.h index 4262e01f31..4f555b1d6d 100644 --- a/sw/simulator/nps/nps_sensor_accel.h +++ b/sw/simulator/nps/nps_sensor_accel.h @@ -19,7 +19,7 @@ struct NpsSensorAccel { }; -extern void nps_sensor_accel_init(struct NpsSensorAccel* accel, double time); -extern void nps_sensor_accel_run_step(struct NpsSensorAccel* accel, double time, struct DoubleRMat* body_to_imu); +extern void nps_sensor_accel_init(struct NpsSensorAccel *accel, double time); +extern void nps_sensor_accel_run_step(struct NpsSensorAccel *accel, double time, struct DoubleRMat *body_to_imu); #endif /* NPS_SENSOR_ACCEL_H */ diff --git a/sw/simulator/nps/nps_sensor_baro.c b/sw/simulator/nps/nps_sensor_baro.c index 22ca9f9a4c..5a10267614 100644 --- a/sw/simulator/nps/nps_sensor_baro.c +++ b/sw/simulator/nps/nps_sensor_baro.c @@ -8,24 +8,29 @@ #include "nps_random.h" #include NPS_SENSORS_PARAMS +#ifndef NPS_BARO_NOISE_STD_DEV +#define NPS_BARO_NOISE_STD_DEV 0. +#endif - -void nps_sensor_baro_init(struct NpsSensorBaro* baro, double time) { +void nps_sensor_baro_init(struct NpsSensorBaro *baro, double time) +{ baro->value = 0.; + baro->noise_std_dev = NPS_BARO_NOISE_STD_DEV; baro->next_update = time; baro->data_available = FALSE; } -void nps_sensor_baro_run_step(struct NpsSensorBaro* baro, double time) { - - if (time < baro->next_update) +void nps_sensor_baro_run_step(struct NpsSensorBaro *baro, double time) +{ + if (time < baro->next_update) { return; + } /* pressure in Pascal */ baro->value = pprz_isa_pressure_of_altitude(fdm.hmsl); /* add noise with std dev Pascal */ - baro->value += get_gaussian_noise() * NPS_BARO_NOISE_STD_DEV; + baro->value += get_gaussian_noise() * baro->noise_std_dev; baro->next_update += NPS_BARO_DT; baro->data_available = TRUE; diff --git a/sw/simulator/nps/nps_sensor_baro.h b/sw/simulator/nps/nps_sensor_baro.h index 0a07c4a04c..a781adb20f 100644 --- a/sw/simulator/nps/nps_sensor_baro.h +++ b/sw/simulator/nps/nps_sensor_baro.h @@ -8,12 +8,13 @@ struct NpsSensorBaro { double value; ///< pressure in Pascal + double noise_std_dev; ///< noise standard deviation double next_update; bool_t data_available; }; -extern void nps_sensor_baro_init(struct NpsSensorBaro* baro, double time); -extern void nps_sensor_baro_run_step(struct NpsSensorBaro* baro, double time); +extern void nps_sensor_baro_init(struct NpsSensorBaro *baro, double time); +extern void nps_sensor_baro_run_step(struct NpsSensorBaro *baro, double time); #endif /* NPS_SENSOR_BARO_H */ diff --git a/sw/simulator/nps/nps_sensor_gps.c b/sw/simulator/nps/nps_sensor_gps.c index 9a63423a32..cd10957c65 100644 --- a/sw/simulator/nps/nps_sensor_gps.c +++ b/sw/simulator/nps/nps_sensor_gps.c @@ -7,22 +7,23 @@ #include "nps_sensors_utils.h" #include NPS_SENSORS_PARAMS -void nps_sensor_gps_init(struct NpsSensorGps* gps, double time) { +void nps_sensor_gps_init(struct NpsSensorGps *gps, double time) +{ FLOAT_VECT3_ZERO(gps->ecef_pos); FLOAT_VECT3_ZERO(gps->ecef_vel); gps->hmsl = 0.0; gps->pos_latency = NPS_GPS_POS_LATENCY; gps->speed_latency = NPS_GPS_SPEED_LATENCY; VECT3_ASSIGN(gps->pos_noise_std_dev, - NPS_GPS_POS_NOISE_STD_DEV, NPS_GPS_POS_NOISE_STD_DEV, NPS_GPS_POS_NOISE_STD_DEV); + NPS_GPS_POS_NOISE_STD_DEV, NPS_GPS_POS_NOISE_STD_DEV, NPS_GPS_POS_NOISE_STD_DEV); VECT3_ASSIGN(gps->speed_noise_std_dev, - NPS_GPS_SPEED_NOISE_STD_DEV, NPS_GPS_SPEED_NOISE_STD_DEV, NPS_GPS_SPEED_NOISE_STD_DEV); + NPS_GPS_SPEED_NOISE_STD_DEV, NPS_GPS_SPEED_NOISE_STD_DEV, NPS_GPS_SPEED_NOISE_STD_DEV); VECT3_ASSIGN(gps->pos_bias_initial, - NPS_GPS_POS_BIAS_INITIAL_X, NPS_GPS_POS_BIAS_INITIAL_Y, NPS_GPS_POS_BIAS_INITIAL_Z); + NPS_GPS_POS_BIAS_INITIAL_X, NPS_GPS_POS_BIAS_INITIAL_Y, NPS_GPS_POS_BIAS_INITIAL_Z); VECT3_ASSIGN(gps->pos_bias_random_walk_std_dev, - NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X, - NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y, - NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z); + NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_X, + NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Y, + NPS_GPS_POS_BIAS_RANDOM_WALK_STD_DEV_Z); FLOAT_VECT3_ZERO(gps->pos_bias_random_walk_value); gps->next_update = time; gps->data_available = FALSE; @@ -35,10 +36,12 @@ void nps_sensor_gps_init(struct NpsSensorGps* gps, double time) { * */ -void nps_sensor_gps_run_step(struct NpsSensorGps* gps, double time) { +void nps_sensor_gps_run_step(struct NpsSensorGps *gps, double time) +{ - if (time < gps->next_update) + if (time < gps->next_update) { return; + } /* @@ -79,7 +82,7 @@ void nps_sensor_gps_run_step(struct NpsSensorGps* gps, double time) { */ /* convert current ecef reading to lla */ struct LlaCoor_d cur_lla_reading; - lla_of_ecef_d(&cur_lla_reading, (struct EcefCoor_d*) &cur_pos_reading); + lla_of_ecef_d(&cur_lla_reading, (struct EcefCoor_d *) &cur_pos_reading); /* store that for later and retrieve a previously stored data */ UpdateSensorLatency(time, &cur_lla_reading, &gps->lla_history, gps->pos_latency, &gps->lla_pos); diff --git a/sw/simulator/nps/nps_sensor_gps.h b/sw/simulator/nps/nps_sensor_gps.h index f632ea9abd..598bd30830 100644 --- a/sw/simulator/nps/nps_sensor_gps.h +++ b/sw/simulator/nps/nps_sensor_gps.h @@ -22,16 +22,16 @@ struct NpsSensorGps { struct DoubleVect3 pos_bias_random_walk_value; double pos_latency; double speed_latency; - GSList* hmsl_history; - GSList* pos_history; - GSList* lla_history; - GSList* speed_history; + GSList *hmsl_history; + GSList *pos_history; + GSList *lla_history; + GSList *speed_history; double next_update; bool_t data_available; }; -extern void nps_sensor_gps_init(struct NpsSensorGps* gps, double time); -extern void nps_sensor_gps_run_step(struct NpsSensorGps* gps, double time); +extern void nps_sensor_gps_init(struct NpsSensorGps *gps, double time); +extern void nps_sensor_gps_run_step(struct NpsSensorGps *gps, double time); #endif /* NPS_SENSOR_GPS_H */ diff --git a/sw/simulator/nps/nps_sensor_gyro.c b/sw/simulator/nps/nps_sensor_gyro.c index 623234fbb0..1bf66296f7 100644 --- a/sw/simulator/nps/nps_sensor_gyro.c +++ b/sw/simulator/nps/nps_sensor_gyro.c @@ -6,36 +6,39 @@ #include "math/pprz_algebra_int.h" #include "nps_random.h" -void nps_sensor_gyro_init(struct NpsSensorGyro* gyro, double time) { +void nps_sensor_gyro_init(struct NpsSensorGyro *gyro, double time) +{ FLOAT_VECT3_ZERO(gyro->value); gyro->min = NPS_GYRO_MIN; gyro->max = NPS_GYRO_MAX; FLOAT_MAT33_DIAG(gyro->sensitivity, - NPS_GYRO_SENSITIVITY_PP, NPS_GYRO_SENSITIVITY_QQ, NPS_GYRO_SENSITIVITY_RR); + NPS_GYRO_SENSITIVITY_PP, NPS_GYRO_SENSITIVITY_QQ, NPS_GYRO_SENSITIVITY_RR); VECT3_ASSIGN(gyro->neutral, - NPS_GYRO_NEUTRAL_P, NPS_GYRO_NEUTRAL_Q, NPS_GYRO_NEUTRAL_R); + NPS_GYRO_NEUTRAL_P, NPS_GYRO_NEUTRAL_Q, NPS_GYRO_NEUTRAL_R); VECT3_ASSIGN(gyro->noise_std_dev, - NPS_GYRO_NOISE_STD_DEV_P, NPS_GYRO_NOISE_STD_DEV_Q, NPS_GYRO_NOISE_STD_DEV_R); + NPS_GYRO_NOISE_STD_DEV_P, NPS_GYRO_NOISE_STD_DEV_Q, NPS_GYRO_NOISE_STD_DEV_R); VECT3_ASSIGN(gyro->bias_initial, - NPS_GYRO_BIAS_INITIAL_P, NPS_GYRO_BIAS_INITIAL_Q, NPS_GYRO_BIAS_INITIAL_R); + NPS_GYRO_BIAS_INITIAL_P, NPS_GYRO_BIAS_INITIAL_Q, NPS_GYRO_BIAS_INITIAL_R); VECT3_ASSIGN(gyro->bias_random_walk_std_dev, - NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_P, - NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q, - NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_R); + NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_P, + NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q, + NPS_GYRO_BIAS_RANDOM_WALK_STD_DEV_R); FLOAT_VECT3_ZERO(gyro->bias_random_walk_value); gyro->next_update = time; gyro->data_available = FALSE; } -void nps_sensor_gyro_run_step(struct NpsSensorGyro* gyro, double time, struct DoubleRMat* body_to_imu) { +void nps_sensor_gyro_run_step(struct NpsSensorGyro *gyro, double time, struct DoubleRMat *body_to_imu) +{ - if (time < gyro->next_update) + if (time < gyro->next_update) { return; + } /* transform body rates to IMU frame */ - struct DoubleVect3* rate_body = (struct DoubleVect3*)(&fdm.body_inertial_rotvel); + struct DoubleVect3 *rate_body = (struct DoubleVect3 *)(&fdm.body_inertial_rotvel); struct DoubleVect3 rate_imu; - MAT33_VECT3_MUL(rate_imu, *body_to_imu, *rate_body ); + MAT33_VECT3_MUL(rate_imu, *body_to_imu, *rate_body); /* compute gyros readings */ MAT33_VECT3_MUL(gyro->value, gyro->sensitivity, rate_imu); VECT3_ADD(gyro->value, gyro->neutral); @@ -44,10 +47,10 @@ void nps_sensor_gyro_run_step(struct NpsSensorGyro* gyro, double time, struct Do VECT3_COPY(gyro_error, gyro->bias_initial); double_vect3_add_gaussian_noise(&gyro_error, &gyro->noise_std_dev); double_vect3_update_random_walk(&gyro->bias_random_walk_value, &gyro->bias_random_walk_std_dev, - NPS_GYRO_DT, 5.); + NPS_GYRO_DT, 5.); VECT3_ADD(gyro_error, gyro->bias_random_walk_value); - struct DoubleVect3 gain = {NPS_GYRO_SENSITIVITY_PP, NPS_GYRO_SENSITIVITY_QQ, NPS_GYRO_SENSITIVITY_RR}; + struct DoubleVect3 gain = {gyro->sensitivity.m[0], gyro->sensitivity.m[4], gyro->sensitivity.m[8]}; VECT3_EW_MUL(gyro_error, gyro_error, gain); VECT3_ADD(gyro->value, gyro_error); @@ -60,6 +63,3 @@ void nps_sensor_gyro_run_step(struct NpsSensorGyro* gyro, double time, struct Do gyro->next_update += NPS_GYRO_DT; gyro->data_available = TRUE; } - - - diff --git a/sw/simulator/nps/nps_sensor_gyro.h b/sw/simulator/nps/nps_sensor_gyro.h index 55490e9447..ddb33703da 100644 --- a/sw/simulator/nps/nps_sensor_gyro.h +++ b/sw/simulator/nps/nps_sensor_gyro.h @@ -21,8 +21,8 @@ struct NpsSensorGyro { }; -extern void nps_sensor_gyro_init(struct NpsSensorGyro* gyro, double time); -extern void nps_sensor_gyro_run_step(struct NpsSensorGyro* gyro, double time, struct DoubleRMat* body_to_imu); +extern void nps_sensor_gyro_init(struct NpsSensorGyro *gyro, double time); +extern void nps_sensor_gyro_run_step(struct NpsSensorGyro *gyro, double time, struct DoubleRMat *body_to_imu); #endif /* NPS_SENSOR_GYRO_H */ diff --git a/sw/simulator/nps/nps_sensor_mag.c b/sw/simulator/nps/nps_sensor_mag.c index 08e3c40026..a34589c2ea 100644 --- a/sw/simulator/nps/nps_sensor_mag.c +++ b/sw/simulator/nps/nps_sensor_mag.c @@ -5,27 +5,30 @@ #include NPS_SENSORS_PARAMS #include "math/pprz_algebra_int.h" -void nps_sensor_mag_init(struct NpsSensorMag* mag, double time) { +void nps_sensor_mag_init(struct NpsSensorMag *mag, double time) +{ VECT3_ASSIGN(mag->value, 0., 0., 0.); mag->min = NPS_MAG_MIN; mag->max = NPS_MAG_MAX; FLOAT_MAT33_DIAG(mag->sensitivity, - NPS_MAG_SENSITIVITY_XX, NPS_MAG_SENSITIVITY_YY, NPS_MAG_SENSITIVITY_ZZ); + NPS_MAG_SENSITIVITY_XX, NPS_MAG_SENSITIVITY_YY, NPS_MAG_SENSITIVITY_ZZ); VECT3_ASSIGN(mag->neutral, - NPS_MAG_NEUTRAL_X, NPS_MAG_NEUTRAL_Y, NPS_MAG_NEUTRAL_Z); + NPS_MAG_NEUTRAL_X, NPS_MAG_NEUTRAL_Y, NPS_MAG_NEUTRAL_Z); VECT3_ASSIGN(mag->noise_std_dev, - NPS_MAG_NOISE_STD_DEV_X, NPS_MAG_NOISE_STD_DEV_Y, NPS_MAG_NOISE_STD_DEV_Z); + NPS_MAG_NOISE_STD_DEV_X, NPS_MAG_NOISE_STD_DEV_Y, NPS_MAG_NOISE_STD_DEV_Z); struct DoubleEulers imu_to_sensor_eulers = - { NPS_MAG_IMU_TO_SENSOR_PHI, NPS_MAG_IMU_TO_SENSOR_THETA, NPS_MAG_IMU_TO_SENSOR_PSI }; + { NPS_MAG_IMU_TO_SENSOR_PHI, NPS_MAG_IMU_TO_SENSOR_THETA, NPS_MAG_IMU_TO_SENSOR_PSI }; DOUBLE_RMAT_OF_EULERS(mag->imu_to_sensor_rmat, imu_to_sensor_eulers); mag->next_update = time; mag->data_available = FALSE; } -void nps_sensor_mag_run_step(struct NpsSensorMag* mag, double time, struct DoubleRMat* body_to_imu) { +void nps_sensor_mag_run_step(struct NpsSensorMag *mag, double time, struct DoubleRMat *body_to_imu) +{ - if (time < mag->next_update) + if (time < mag->next_update) { return; + } /* transform magnetic field to body frame */ struct DoubleVect3 h_body; @@ -33,11 +36,11 @@ void nps_sensor_mag_run_step(struct NpsSensorMag* mag, double time, struct Doubl /* transform to imu frame */ struct DoubleVect3 h_imu; - MAT33_VECT3_MUL(h_imu, *body_to_imu, h_body ); + MAT33_VECT3_MUL(h_imu, *body_to_imu, h_body); /* transform to sensor frame */ struct DoubleVect3 h_sensor; - MAT33_VECT3_MUL(h_sensor, mag->imu_to_sensor_rmat, h_imu ); + MAT33_VECT3_MUL(h_sensor, mag->imu_to_sensor_rmat, h_imu); /* compute magnetometer reading */ MAT33_VECT3_MUL(mag->value, mag->sensitivity, h_sensor); diff --git a/sw/simulator/nps/nps_sensor_mag.h b/sw/simulator/nps/nps_sensor_mag.h index ab0fdeb849..d1cf6f1356 100644 --- a/sw/simulator/nps/nps_sensor_mag.h +++ b/sw/simulator/nps/nps_sensor_mag.h @@ -19,7 +19,7 @@ struct NpsSensorMag { }; -extern void nps_sensor_mag_init(struct NpsSensorMag* mag, double time); -extern void nps_sensor_mag_run_step(struct NpsSensorMag* mag, double time, struct DoubleRMat* body_to_imu); +extern void nps_sensor_mag_init(struct NpsSensorMag *mag, double time); +extern void nps_sensor_mag_run_step(struct NpsSensorMag *mag, double time, struct DoubleRMat *body_to_imu); #endif /* NPS_SENSOR_MAG_H */ diff --git a/sw/simulator/nps/nps_sensor_sonar.c b/sw/simulator/nps/nps_sensor_sonar.c index 3d17a3f41c..ea33050888 100644 --- a/sw/simulator/nps/nps_sensor_sonar.c +++ b/sw/simulator/nps/nps_sensor_sonar.c @@ -50,22 +50,27 @@ #endif -void nps_sensor_sonar_init(struct NpsSensorSonar* sonar, double time) { +void nps_sensor_sonar_init(struct NpsSensorSonar *sonar, double time) +{ sonar->value = 0.; + sonar->offset = NPS_SONAR_OFFSET; + sonar->noise_std_dev = NPS_SONAR_NOISE_STD_DEV; sonar->next_update = time; sonar->data_available = FALSE; } -void nps_sensor_sonar_run_step(struct NpsSensorSonar* sonar, double time) { +void nps_sensor_sonar_run_step(struct NpsSensorSonar *sonar, double time) +{ - if (time < sonar->next_update) + if (time < sonar->next_update) { return; + } /* agl in meters */ - sonar->value = fdm.agl + NPS_SONAR_OFFSET; + sonar->value = fdm.agl + sonar->offset; /* add noise with std dev meters */ - sonar->value += get_gaussian_noise() * NPS_SONAR_NOISE_STD_DEV; + sonar->value += get_gaussian_noise() * sonar->noise_std_dev; sonar->next_update += NPS_SONAR_DT; sonar->data_available = TRUE; diff --git a/sw/simulator/nps/nps_sensor_sonar.h b/sw/simulator/nps/nps_sensor_sonar.h index 30df64e90e..cdae3ae3e5 100644 --- a/sw/simulator/nps/nps_sensor_sonar.h +++ b/sw/simulator/nps/nps_sensor_sonar.h @@ -35,13 +35,15 @@ #include "std.h" struct NpsSensorSonar { - double value; ///< sonar reading in meters - double next_update; - bool_t data_available; + double value; ///< sonar reading in meters + double offset; ///< offset in meters + double noise_std_dev; ///< noise standard deviation + double next_update; + bool_t data_available; }; -extern void nps_sensor_sonar_init(struct NpsSensorSonar* sonar, double time); -extern void nps_sensor_sonar_run_step(struct NpsSensorSonar* sonar, double time); +extern void nps_sensor_sonar_init(struct NpsSensorSonar *sonar, double time); +extern void nps_sensor_sonar_run_step(struct NpsSensorSonar *sonar, double time); #endif /* NPS_SENSOR_SONAR_H */ diff --git a/sw/simulator/nps/nps_sensors.c b/sw/simulator/nps/nps_sensors.c index 497a6237a3..1c9209a071 100644 --- a/sw/simulator/nps/nps_sensors.c +++ b/sw/simulator/nps/nps_sensors.c @@ -5,10 +5,11 @@ struct NpsSensors sensors; -void nps_sensors_init(double time) { +void nps_sensors_init(double time) +{ struct DoubleEulers body_to_imu_eulers = - { NPS_BODY_TO_IMU_PHI, NPS_BODY_TO_IMU_THETA, NPS_BODY_TO_IMU_PSI }; + { NPS_BODY_TO_IMU_PHI, NPS_BODY_TO_IMU_THETA, NPS_BODY_TO_IMU_PSI }; DOUBLE_RMAT_OF_EULERS(sensors.body_to_imu_rmat, body_to_imu_eulers); nps_sensor_gyro_init(&sensors.gyro, time); @@ -20,7 +21,8 @@ void nps_sensors_init(double time) { } -void nps_sensors_run_step(double time) { +void nps_sensors_run_step(double time) +{ nps_sensor_gyro_run_step(&sensors.gyro, time, &sensors.body_to_imu_rmat); nps_sensor_accel_run_step(&sensors.accel, time, &sensors.body_to_imu_rmat); nps_sensor_mag_run_step(&sensors.mag, time, &sensors.body_to_imu_rmat); @@ -30,7 +32,8 @@ void nps_sensors_run_step(double time) { } -bool_t nps_sensors_gyro_available(void) { +bool_t nps_sensors_gyro_available(void) +{ if (sensors.gyro.data_available) { sensors.gyro.data_available = FALSE; return TRUE; @@ -38,7 +41,8 @@ bool_t nps_sensors_gyro_available(void) { return FALSE; } -bool_t nps_sensors_mag_available(void) { +bool_t nps_sensors_mag_available(void) +{ if (sensors.mag.data_available) { sensors.mag.data_available = FALSE; return TRUE; @@ -46,7 +50,8 @@ bool_t nps_sensors_mag_available(void) { return FALSE; } -bool_t nps_sensors_baro_available(void) { +bool_t nps_sensors_baro_available(void) +{ if (sensors.baro.data_available) { sensors.baro.data_available = FALSE; return TRUE; @@ -54,7 +59,8 @@ bool_t nps_sensors_baro_available(void) { return FALSE; } -bool_t nps_sensors_gps_available(void) { +bool_t nps_sensors_gps_available(void) +{ if (sensors.gps.data_available) { sensors.gps.data_available = FALSE; return TRUE; @@ -62,7 +68,8 @@ bool_t nps_sensors_gps_available(void) { return FALSE; } -bool_t nps_sensors_sonar_available(void) { +bool_t nps_sensors_sonar_available(void) +{ if (sensors.sonar.data_available) { sensors.sonar.data_available = FALSE; return TRUE; diff --git a/sw/simulator/nps/nps_sensors_utils.c b/sw/simulator/nps/nps_sensors_utils.c index 0911fed2eb..684ffc12d9 100644 --- a/sw/simulator/nps/nps_sensors_utils.c +++ b/sw/simulator/nps/nps_sensors_utils.c @@ -3,44 +3,47 @@ //#include #include "math/pprz_algebra.h" -void UpdateSensorLatency(double time, gpointer cur_reading, GSList **history, double latency, gpointer sensor_reading) { +void UpdateSensorLatency(double time, gpointer cur_reading, GSList **history, double latency, gpointer sensor_reading) +{ /* add new reading */ - struct BoozDatedSensor* cur_read = g_new(struct BoozDatedSensor, 1); + struct BoozDatedSensor *cur_read = g_new(struct BoozDatedSensor, 1); cur_read->time = time; - cur_read->value = (struct DoubleVect3*) g_memdup(cur_reading, sizeof(struct DoubleVect3)); + cur_read->value = (struct DoubleVect3 *) g_memdup(cur_reading, sizeof(struct DoubleVect3)); *history = g_slist_prepend(*history, cur_read); /* remove old readings */ - GSList* last = g_slist_last(*history); - while (last && ((struct BoozDatedSensor*)last->data)->time < time - latency) { + GSList *last = g_slist_last(*history); + while (last && ((struct BoozDatedSensor *)last->data)->time < time - latency) { *history = g_slist_remove_link(*history, last); - g_free(((struct BoozDatedSensor*)last->data)->value); - g_free((struct BoozDatedSensor*)last->data); + g_free(((struct BoozDatedSensor *)last->data)->value); + g_free((struct BoozDatedSensor *)last->data); g_slist_free_1(last); last = g_slist_last(*history); } /* update sensor */ //g_memmove((gpointer)sensor_reading, (gpointer)((struct BoozDatedSensor*)last->data)->value, sizeof(struct DoubleVect3)); - VECT3_COPY(*((struct DoubleVect3*)sensor_reading), *((struct BoozDatedSensor*)last->data)->value); + VECT3_COPY(*((struct DoubleVect3 *)sensor_reading), *((struct BoozDatedSensor *)last->data)->value); } -void UpdateSensorLatency_Single(double time, gpointer cur_reading, GSList **history, double latency, gpointer sensor_reading) { +void UpdateSensorLatency_Single(double time, gpointer cur_reading, GSList **history, double latency, + gpointer sensor_reading) +{ /* add new reading */ - struct BoozDatedSensor_Single* cur_read = g_new(struct BoozDatedSensor_Single, 1); + struct BoozDatedSensor_Single *cur_read = g_new(struct BoozDatedSensor_Single, 1); cur_read->time = time; - cur_read->value = (double*) g_memdup(cur_reading, sizeof(double)); + cur_read->value = (double *) g_memdup(cur_reading, sizeof(double)); *history = g_slist_prepend(*history, cur_read); /* remove old readings */ - GSList* last = g_slist_last(*history); - while (last && ((struct BoozDatedSensor_Single*)last->data)->time < time - latency) { + GSList *last = g_slist_last(*history); + while (last && ((struct BoozDatedSensor_Single *)last->data)->time < time - latency) { *history = g_slist_remove_link(*history, last); - g_free(((struct BoozDatedSensor_Single*)last->data)->value); - g_free((struct BoozDatedSensor_Single*)last->data); + g_free(((struct BoozDatedSensor_Single *)last->data)->value); + g_free((struct BoozDatedSensor_Single *)last->data); g_slist_free_1(last); last = g_slist_last(*history); } /* update sensor */ //g_memmove((gpointer)sensor_reading, (gpointer)((struct BoozDatedSensor*)last->data)->value, sizeof(struct DoubleVect3)); - *((double*)sensor_reading) = *(((struct BoozDatedSensor_Single*)last->data)->value); + *((double *)sensor_reading) = *(((struct BoozDatedSensor_Single *)last->data)->value); } diff --git a/sw/simulator/nps/nps_sensors_utils.h b/sw/simulator/nps/nps_sensors_utils.h index 74b8545b3a..3193cf7316 100644 --- a/sw/simulator/nps/nps_sensors_utils.h +++ b/sw/simulator/nps/nps_sensors_utils.h @@ -5,21 +5,21 @@ #include "math/pprz_algebra_double.h" struct BoozDatedSensor { - struct DoubleVect3* value; + struct DoubleVect3 *value; double time; }; struct BoozDatedSensor_Single { - double* value; + double *value; double time; }; /* cur_reading and sensor_reading must be of a type that can be cast to DoubleVect3* */ extern void UpdateSensorLatency(double time, gpointer cur_reading, GSList **history, - double latency, gpointer sensor_reading); + double latency, gpointer sensor_reading); /* ...and the same for single double values */ extern void UpdateSensorLatency_Single(double time, gpointer cur_reading, GSList **history, - double latency, gpointer sensor_reading); + double latency, gpointer sensor_reading); #endif /* NPS_SENSORS_UTILS_H */ From bdd7ceb43ee8f4f4d0765aafa83768c4c52edec7 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 26 Mar 2015 19:00:06 +0100 Subject: [PATCH 2/2] [nps] add possibility to simulate loss of datalink/downlink It is a bit of a hack, especially we can't disable changing settings if datalink is disabled, since we need to parse that to enable the datalink via settings again. Nevertheless it should suffice for most cases and close #934 --- conf/settings/nps.xml | 2 ++ sw/simulator/nps/nps_autopilot.h | 1 + sw/simulator/nps/nps_autopilot_fixedwing.c | 8 ++++++ sw/simulator/nps/nps_autopilot_rotorcraft.c | 8 ++++++ sw/simulator/nps/nps_ivy_common.c | 19 +++++++++++++ sw/simulator/nps/nps_ivy_fixedwing.c | 3 +++ sw/simulator/nps/nps_ivy_mission_commands.c | 30 +++++++++++++++++++++ sw/simulator/nps/nps_ivy_rotorcraft.c | 3 +++ 8 files changed, 74 insertions(+) diff --git a/conf/settings/nps.xml b/conf/settings/nps.xml index fd58df0a73..fbd42226ea 100644 --- a/conf/settings/nps.xml +++ b/conf/settings/nps.xml @@ -7,6 +7,8 @@ + + diff --git a/sw/simulator/nps/nps_autopilot.h b/sw/simulator/nps/nps_autopilot.h index ce18596952..c25b66b2ae 100644 --- a/sw/simulator/nps/nps_autopilot.h +++ b/sw/simulator/nps/nps_autopilot.h @@ -27,6 +27,7 @@ struct NpsAutopilot { double commands[NPS_COMMANDS_NB]; bool_t launch; + bool_t datalink_enabled; }; extern struct NpsAutopilot autopilot; diff --git a/sw/simulator/nps/nps_autopilot_fixedwing.c b/sw/simulator/nps/nps_autopilot_fixedwing.c index 0e1bc09855..9271269d0d 100644 --- a/sw/simulator/nps/nps_autopilot_fixedwing.c +++ b/sw/simulator/nps/nps_autopilot_fixedwing.c @@ -52,6 +52,9 @@ // for launch #include "firmwares/fixedwing/autopilot.h" +// for datalink_time hack +#include "subsystems/datalink/datalink.h" + struct NpsAutopilot autopilot; bool_t nps_bypass_ahrs; bool_t nps_bypass_ins; @@ -72,6 +75,7 @@ bool_t nps_bypass_ins; void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, char* rc_dev) { autopilot.launch = FALSE; + autopilot.datalink_enabled = TRUE; nps_radio_control_init(type_rc, num_rc_script, rc_dev); nps_electrical_init(); @@ -169,6 +173,10 @@ PRINT_CONFIG_VAR(COMMAND_YAW) if (!launch) autopilot.commands[COMMAND_THROTTLE] = 0; + // hack to reset datalink_time, since we don't use actual dl_parse_msg + if (autopilot.datalink_enabled) { + datalink_time = 0; + } } void sim_overwrite_ahrs(void) { diff --git a/sw/simulator/nps/nps_autopilot_rotorcraft.c b/sw/simulator/nps/nps_autopilot_rotorcraft.c index d93dd90b88..6eb5a71f00 100644 --- a/sw/simulator/nps/nps_autopilot_rotorcraft.c +++ b/sw/simulator/nps/nps_autopilot_rotorcraft.c @@ -42,6 +42,9 @@ #include "messages.h" #include "subsystems/datalink/downlink.h" +// for datalink_time hack +#include "subsystems/datalink/datalink.h" + struct NpsAutopilot autopilot; bool_t nps_bypass_ahrs; bool_t nps_bypass_ins; @@ -60,6 +63,7 @@ bool_t nps_bypass_ins; void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, char* rc_dev) { autopilot.launch = TRUE; + autopilot.datalink_enabled = TRUE; nps_radio_control_init(type_rc, num_rc_script, rc_dev); nps_electrical_init(); @@ -134,6 +138,10 @@ void nps_autopilot_run_step(double time) { for (uint8_t i=0; i < NPS_COMMANDS_NB; i++) autopilot.commands[i] = (double)motor_mixing.commands[i]/MAX_PPRZ; + // hack to reset datalink_time, since we don't use actual dl_parse_msg + if (autopilot.datalink_enabled) { + datalink_time = 0; + } } diff --git a/sw/simulator/nps/nps_ivy_common.c b/sw/simulator/nps/nps_ivy_common.c index 5ef6f19d7b..9c5fdf76a4 100644 --- a/sw/simulator/nps/nps_ivy_common.c +++ b/sw/simulator/nps/nps_ivy_common.c @@ -87,6 +87,13 @@ static void on_DL_SETTING(IvyClientPtr app __attribute__ ((unused)), if (atoi(argv[1]) != AC_ID) return; + /* HACK: + * we actually don't want to allow changing settings if datalink is disabled, + * but since we currently change this variable via settings we have to allow it + */ + //if (!autopilot.datalink_enabled) + // return; + uint8_t index = atoi(argv[2]); float value = atof(argv[3]); DlSetting(index, value); @@ -99,6 +106,8 @@ static void on_DL_GET_SETTING(IvyClientPtr app __attribute__ ((unused)), int argc __attribute__ ((unused)), char *argv[]) { if (atoi(argv[1]) != AC_ID) return; + if (!autopilot.datalink_enabled) + return; uint8_t index = atoi(argv[2]); float value = settings_get_value(index); @@ -109,6 +118,9 @@ static void on_DL_GET_SETTING(IvyClientPtr app __attribute__ ((unused)), static void on_DL_PING(IvyClientPtr app __attribute__ ((unused)), void *user_data __attribute__ ((unused)), int argc __attribute__ ((unused)), char *argv[] __attribute__ ((unused))) { + if (!autopilot.datalink_enabled) + return; + DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice); } @@ -117,6 +129,8 @@ static void on_DL_BLOCK(IvyClientPtr app __attribute__ ((unused)), int argc __attribute__ ((unused)), char *argv[]){ if (atoi(argv[2]) != AC_ID) return; + if (!autopilot.datalink_enabled) + return; int block = atoi(argv[1]); nav_goto_block(block); @@ -127,6 +141,9 @@ static void on_DL_BLOCK(IvyClientPtr app __attribute__ ((unused)), static void on_DL_RC_3CH(IvyClientPtr app __attribute__ ((unused)), void *user_data __attribute__ ((unused)), int argc __attribute__ ((unused)), char *argv[]){ + if (!autopilot.datalink_enabled) + return; + uint8_t throttle_mode = atoi(argv[2]); int8_t roll = atoi(argv[3]); int8_t pitch = atoi(argv[4]); @@ -139,6 +156,8 @@ static void on_DL_RC_4CH(IvyClientPtr app __attribute__ ((unused)), int argc __attribute__ ((unused)), char *argv[]){ if (atoi(argv[1]) != AC_ID) return; + if (!autopilot.datalink_enabled) + return; uint8_t mode = atoi(argv[2]); uint8_t throttle = atoi(argv[3]); diff --git a/sw/simulator/nps/nps_ivy_fixedwing.c b/sw/simulator/nps/nps_ivy_fixedwing.c index f94e099a05..cbd617bc07 100644 --- a/sw/simulator/nps/nps_ivy_fixedwing.c +++ b/sw/simulator/nps/nps_ivy_fixedwing.c @@ -8,6 +8,7 @@ #include "math/pprz_algebra_double.h" #include "subsystems/ins.h" #include "subsystems/navigation/common_nav.h" +#include "nps_autopilot.h" /* fixedwing specific Datalink Ivy functions */ void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)), @@ -33,6 +34,8 @@ void nps_ivy_init(char* ivy_bus) { void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)), void *user_data __attribute__ ((unused)), int argc __attribute__ ((unused)), char *argv[]) { + if (!autopilot.datalink_enabled) + return; if (atoi(argv[2]) == AC_ID) { uint8_t wp_id = atoi(argv[1]); diff --git a/sw/simulator/nps/nps_ivy_mission_commands.c b/sw/simulator/nps/nps_ivy_mission_commands.c index 1e6e64416d..968202fbb6 100644 --- a/sw/simulator/nps/nps_ivy_mission_commands.c +++ b/sw/simulator/nps/nps_ivy_mission_commands.c @@ -93,6 +93,9 @@ void nps_ivy_mission_commands_init(void) { static void on_DL_MISSION_GOTO_WP(IvyClientPtr app __attribute__ ((unused)), void *user_data __attribute__ ((unused)), int argc __attribute__ ((unused)), char *argv[]) { + if (!autopilot.datalink_enabled) + return; + uint8_t i = 0; float dummy; dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id @@ -109,6 +112,9 @@ static void on_DL_MISSION_GOTO_WP(IvyClientPtr app __attribute__ ((unused)), static void on_DL_MISSION_GOTO_WP_LLA(IvyClientPtr app __attribute__ ((unused)), void *user_data __attribute__ ((unused)), int argc __attribute__ ((unused)), char *argv[]) { + if (!autopilot.datalink_enabled) + return; + uint8_t i = 0; float dummy; dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id @@ -125,6 +131,9 @@ static void on_DL_MISSION_GOTO_WP_LLA(IvyClientPtr app __attribute__ ((unused)), static void on_DL_MISSION_CIRCLE(IvyClientPtr app __attribute__ ((unused)), void *user_data __attribute__ ((unused)), int argc __attribute__ ((unused)), char *argv[]) { + if (!autopilot.datalink_enabled) + return; + uint8_t i = 0; float dummy; dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id @@ -141,6 +150,9 @@ static void on_DL_MISSION_CIRCLE(IvyClientPtr app __attribute__ ((unused)), static void on_DL_MISSION_CIRCLE_LLA(IvyClientPtr app __attribute__ ((unused)), void *user_data __attribute__ ((unused)), int argc __attribute__ ((unused)), char *argv[]) { + if (!autopilot.datalink_enabled) + return; + uint8_t i = 0; float dummy; dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id @@ -157,6 +169,9 @@ static void on_DL_MISSION_CIRCLE_LLA(IvyClientPtr app __attribute__ ((unused)), static void on_DL_MISSION_SEGMENT(IvyClientPtr app __attribute__ ((unused)), void *user_data __attribute__ ((unused)), int argc __attribute__ ((unused)), char *argv[]) { + if (!autopilot.datalink_enabled) + return; + uint8_t i = 0; float dummy; dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id @@ -173,6 +188,9 @@ static void on_DL_MISSION_SEGMENT(IvyClientPtr app __attribute__ ((unused)), static void on_DL_MISSION_SEGMENT_LLA(IvyClientPtr app __attribute__ ((unused)), void *user_data __attribute__ ((unused)), int argc __attribute__ ((unused)), char *argv[]) { + if (!autopilot.datalink_enabled) + return; + uint8_t i = 0; float dummy; dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id @@ -189,6 +207,9 @@ static void on_DL_MISSION_SEGMENT_LLA(IvyClientPtr app __attribute__ ((unused)), static void on_DL_MISSION_PATH(IvyClientPtr app __attribute__ ((unused)), void *user_data __attribute__ ((unused)), int argc __attribute__ ((unused)), char *argv[]) { + if (!autopilot.datalink_enabled) + return; + uint8_t i = 0; float dummy; dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id @@ -206,6 +227,9 @@ static void on_DL_MISSION_PATH(IvyClientPtr app __attribute__ ((unused)), static void on_DL_MISSION_PATH_LLA(IvyClientPtr app __attribute__ ((unused)), void *user_data __attribute__ ((unused)), int argc __attribute__ ((unused)), char *argv[]) { + if (!autopilot.datalink_enabled) + return; + uint8_t i = 0; float dummy; dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id @@ -223,6 +247,8 @@ static void on_DL_MISSION_PATH_LLA(IvyClientPtr app __attribute__ ((unused)), static void on_DL_GOTO_MISSION(IvyClientPtr app __attribute__ ((unused)), void *user_data __attribute__ ((unused)), int argc __attribute__ ((unused)), char *argv[]) { + if (!autopilot.datalink_enabled) + return; dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id dl_buffer[3] = (uint8_t)(atoi(argv[2])); //mission_id @@ -233,6 +259,8 @@ static void on_DL_GOTO_MISSION(IvyClientPtr app __attribute__ ((unused)), static void on_DL_NEXT_MISSION(IvyClientPtr app __attribute__ ((unused)), void *user_data __attribute__ ((unused)), int argc __attribute__ ((unused)), char *argv[]) { + if (!autopilot.datalink_enabled) + return; dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id @@ -242,6 +270,8 @@ static void on_DL_NEXT_MISSION(IvyClientPtr app __attribute__ ((unused)), static void on_DL_END_MISSION(IvyClientPtr app __attribute__ ((unused)), void *user_data __attribute__ ((unused)), int argc __attribute__ ((unused)), char *argv[]) { + if (!autopilot.datalink_enabled) + return; dl_buffer[2] = (uint8_t)(atoi(argv[1])); //ac_id diff --git a/sw/simulator/nps/nps_ivy_rotorcraft.c b/sw/simulator/nps/nps_ivy_rotorcraft.c index 9ab8dfc3b0..c3b7935309 100644 --- a/sw/simulator/nps/nps_ivy_rotorcraft.c +++ b/sw/simulator/nps/nps_ivy_rotorcraft.c @@ -41,6 +41,9 @@ void nps_ivy_init(char* ivy_bus) { static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)), void *user_data __attribute__ ((unused)), int argc __attribute__ ((unused)), char *argv[]) { + if (!autopilot.datalink_enabled) + return; + if (atoi(argv[2]) == AC_ID) { uint8_t wp_id = atoi(argv[1]);