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; }
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_accel_run_step(struct NpsSensorAccel* accel, double time, struct DoubleRMat* body_to_imu) { if (time < accel->next_update) return; /* transform gravity to body frame */ struct DoubleVect3 g_body; FLOAT_QUAT_VMULT(g_body, fdm.ltp_to_body_quat, fdm.ltp_g); // printf(" g_body %f %f %f\n", g_body.x, g_body.y, g_body.z); // printf(" accel_fdm %f %f %f\n", fdm.body_ecef_accel.x, fdm.body_ecef_accel.y, fdm.body_ecef_accel.z); /* substract gravity to acceleration in body frame */ struct DoubleVect3 accelero_body; VECT3_DIFF(accelero_body, fdm.body_ecef_accel, g_body); // printf(" accelero body %f %f %f\n", accelero_body.x, accelero_body.y, accelero_body.z); /* transform to imu frame */ struct DoubleVect3 accelero_imu; MAT33_VECT3_MUL(accelero_imu, *body_to_imu, accelero_body ); /* compute accelero readings */ MAT33_VECT3_MUL(accel->value, accel->sensitivity, accelero_imu); VECT3_ADD(accel->value, accel->neutral); /* Compute sensor error */ struct DoubleVect3 accelero_error; /* constant bias */ VECT3_COPY(accelero_error, accel->bias); /* 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}; VECT3_EW_MUL(accelero_error, accelero_error, gain); /* add error */ VECT3_ADD(accel->value, accelero_error); /* round signal to account for adc discretisation */ DOUBLE_VECT3_ROUND(accel->value); /* saturate */ VECT3_BOUND_CUBE(accel->value, 0, accel->resolution); accel->next_update += NPS_ACCEL_DT; accel->data_available = TRUE; }