void aos_compute_sensors(void)
{
struct FloatRates gyro;
RATES_SUM(gyro, aos.imu_rates, aos.gyro_bias);
// printf("#aos.gyro_bias %f\n",DegOfRad( aos.gyro_bias.r));
float_rates_add_gaussian_noise(&gyro, &aos.gyro_noise);
RATES_BFP_OF_REAL(imu.gyro, gyro);
RATES_BFP_OF_REAL(imu.gyro_prev, gyro);
struct FloatVect3 g_ltp = {0., 0., 9.81};
struct FloatVect3 accelero_ltp;
VECT3_DIFF(accelero_ltp, aos.ltp_accel, g_ltp);
struct FloatVect3 accelero_imu;
float_quat_vmult(&accelero_imu, &aos.ltp_to_imu_quat, &accelero_ltp);
float_vect3_add_gaussian_noise(&accelero_imu, &aos.accel_noise);
ACCELS_BFP_OF_REAL(imu.accel, accelero_imu);
#ifndef DISABLE_MAG_UPDATE
struct FloatVect3 h_earth = {AHRS_H_X, AHRS_H_Y, AHRS_H_Z};
struct FloatVect3 h_imu;
float_quat_vmult(&h_imu, &aos.ltp_to_imu_quat, &h_earth);
MAGS_BFP_OF_REAL(imu.mag, h_imu);
#endif
aos.heading_meas = aos.ltp_to_imu_euler.psi + get_gaussian_noise() * aos.heading_noise;
#ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN
#if AHRS_TYPE == AHRS_TYPE_FCQ || AHRS_TYPE == AHRS_TYPE_FLQ
ahrs_impl.ltp_vel_norm = float_vect3_norm(&aos.ltp_vel);
ahrs_impl.ltp_vel_norm_valid = true;
#endif
#if AHRS_TYPE == AHRS_TYPE_FCR2
ahrs_impl.ltp_vel_norm = float_vect3_norm(&aos.ltp_vel);
ahrs_impl.ltp_vel_norm_valid = true;
#endif
#if AHRS_TYPE == AHRS_TYPE_FCR
ahrs_impl.gps_speed = float_vect3_norm(&aos.ltp_vel);
ahrs_impl.gps_age = 0;
ahrs_update_gps();
//RunOnceEvery(100,printf("# gps accel: %f\n", ahrs_impl.gps_acceleration));
#endif
#if AHRS_TYPE == AHRS_TYPE_ICQ
ahrs_impl.ltp_vel_norm = SPEED_BFP_OF_REAL(float_vect3_norm(&aos.ltp_vel));
ahrs_impl.ltp_vel_norm_valid = true;
#endif
#endif
}
void nps_sensor_sonar_run_step(struct NpsSensorSonar* sonar, double time) {
if (time < sonar->next_update)
return;
/* agl in meters */
sonar->value = fdm.agl + NPS_SONAR_OFFSET;
/* add noise with std dev meters */
sonar->value += get_gaussian_noise() * NPS_SONAR_NOISE_STD_DEV;
sonar->next_update += NPS_SONAR_DT;
sonar->data_available = TRUE;
}
void nps_sensor_temperature_run_step(struct NpsSensorTemperature *temperature, double time)
{
if (time < temperature->next_update) {
return;
}
/* termperature in degrees Celcius */
temperature->value = fdm.temperature;
/* add noise with std dev */
temperature->value += get_gaussian_noise() * temperature->noise_std_dev;
temperature->next_update += NPS_TEMPERATURE_DT;
temperature->data_available = TRUE;
}
void nps_sensor_sonar_run_step(struct NpsSensorSonar *sonar, double time)
{
if (time < sonar->next_update) {
return;
}
/* agl in meters */
sonar->value = fdm.agl + sonar->offset;
/* add noise with std dev meters */
sonar->value += get_gaussian_noise() * sonar->noise_std_dev;
sonar->next_update += NPS_SONAR_DT;
sonar->data_available = TRUE;
}
void nps_sensor_baro_run_step(struct NpsSensorBaro* baro, double time) {
if (time < baro->next_update)
return;
/*if (time < 10.)
baro->value = rint(time*90);
else {*/
double z = fdm.ltpprz_pos.z + get_gaussian_noise()*NPS_BARO_NOISE_STD_DEV;
double baro_reading = NPS_BARO_QNH + z * NPS_BARO_SENSITIVITY;
baro_reading = rint(baro_reading);
baro->value = baro_reading;
Bound(baro->value, 0, 1024);
//}
baro->next_update += NPS_BARO_DT;
baro->data_available = TRUE;
}
void nps_sensor_airspeed_run_step(struct NpsSensorAirspeed *airspeed, double time)
{
if (time < airspeed->next_update) {
return;
}
/* equivalent airspeed + sensor offset */
airspeed->value = fdm.airspeed + airspeed->offset;
/* add noise with std dev meters/second */
airspeed->value += get_gaussian_noise() * airspeed->noise_std_dev;
/* can't be negative, min is zero */
if (airspeed->value < 0) {
airspeed->value = 0.0;
}
airspeed->next_update += NPS_AIRSPEED_DT;
airspeed->data_available = TRUE;
}
void double_vect3_get_gaussian_noise(struct DoubleVect3* vect, struct DoubleVect3* std_dev) {
vect->x = get_gaussian_noise() * std_dev->x;
vect->y = get_gaussian_noise() * std_dev->y;
vect->z = get_gaussian_noise() * std_dev->z;
}
void float_rates_add_gaussian_noise(struct FloatRates* vect, struct FloatRates* std_dev) {
vect->p += get_gaussian_noise() * std_dev->p;
vect->q += get_gaussian_noise() * std_dev->q;
vect->r += get_gaussian_noise() * std_dev->r;
}
void float_vect3_add_gaussian_noise(struct FloatVect3* vect, struct FloatVect3* std_dev) {
vect->x += get_gaussian_noise() * std_dev->x;
vect->y += get_gaussian_noise() * std_dev->y;
vect->z += get_gaussian_noise() * std_dev->z;
}
void double_vect3_add_gaussian_noise(struct DoubleVect3* vect, struct DoubleVect3* std_dev) {
vect->x += get_gaussian_noise() * std_dev->x;
vect->y += get_gaussian_noise() * std_dev->y;
vect->z += get_gaussian_noise() * std_dev->z;
}
