void nps_sensor_gyro_run_step(struct NpsSensorGyro* gyro, double time, struct DoubleRMat* body_to_imu) {
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_imu;
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);
/* compute gyro error readings */
struct DoubleVect3 gyro_error;
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.);
VECT3_ADD(gyro_error, gyro->bias_random_walk_value);
struct DoubleVect3 gain = {NPS_GYRO_SENSITIVITY_PP, NPS_GYRO_SENSITIVITY_QQ, NPS_GYRO_SENSITIVITY_RR};
VECT3_EW_MUL(gyro_error, gyro_error, gain);
VECT3_ADD(gyro->value, gyro_error);
/* round signal to account for adc discretisation */
DOUBLE_VECT3_ROUND(gyro->value);
/* saturate */
VECT3_BOUND_CUBE(gyro->value, gyro->min, gyro->max);
gyro->next_update += NPS_GYRO_DT;
gyro->data_available = TRUE;
}
void nps_sensor_gps_run_step(struct NpsSensorGps* gps, double time) {
if (time < gps->next_update)
return;
/*
* simulate speed sensor
*/
struct DoubleVect3 cur_speed_reading;
VECT3_COPY(cur_speed_reading, fdm.ecef_ecef_vel);
/* add a gaussian noise */
double_vect3_add_gaussian_noise(&cur_speed_reading, &gps->speed_noise_std_dev);
/* store that for later and retrieve a previously stored data */
UpdateSensorLatency(time, &cur_speed_reading, &gps->speed_history, gps->speed_latency, &gps->ecef_vel);
/*
* simulate position sensor
*/
/* compute gps error readings */
struct DoubleVect3 pos_error;
VECT3_COPY(pos_error, gps->pos_bias_initial);
/* add a gaussian noise */
double_vect3_add_gaussian_noise(&pos_error, &gps->pos_noise_std_dev);
/* update random walk bias and add it to error*/
double_vect3_update_random_walk(&gps->pos_bias_random_walk_value, &gps->pos_bias_random_walk_std_dev, NPS_GPS_DT, 5.);
VECT3_ADD(pos_error, gps->pos_bias_random_walk_value);
/* add error to current pos reading */
struct DoubleVect3 cur_pos_reading;
VECT3_COPY(cur_pos_reading, fdm.ecef_pos);
VECT3_ADD(cur_pos_reading, pos_error);
/* store that for later and retrieve a previously stored data */
UpdateSensorLatency(time, &cur_pos_reading, &gps->pos_history, gps->pos_latency, &gps->ecef_pos);
/*
* simulate lla pos
*/
/* convert current ecef reading to lla */
struct LlaCoor_d cur_lla_reading;
lla_of_ecef_d(&cur_lla_reading, (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);
double cur_hmsl_reading = fdm.hmsl;
UpdateSensorLatency_Single(time, &cur_hmsl_reading, &gps->hmsl_history, gps->pos_latency, &gps->hmsl);
gps->next_update += NPS_GPS_DT;
gps->data_available = TRUE;
}
