void nps_sensor_accel_init(struct NpsSensorAccel* accel, double time) {
FLOAT_VECT3_ZERO(accel->value);
accel->resolution = NPS_ACCEL_RESOLUTION;
FLOAT_MAT33_DIAG(accel->sensitivity,
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);
VECT3_ASSIGN(accel->noise_std_dev,
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);
accel->next_update = time;
accel->data_available = FALSE;
}
void nps_sensor_mag_init(struct NpsSensorMag* mag, double time) {
VECT3_ASSIGN(mag->value, 0., 0., 0.);
// mag->resolution = NPS_MAG_RESOLUTION;
FLOAT_MAT33_DIAG(mag->sensitivity,
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);
VECT3_ASSIGN(mag->noise_std_dev,
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 };
DOUBLE_RMAT_OF_EULERS(mag->imu_to_sensor_rmat, imu_to_sensor_eulers);
mag->next_update = time;
mag->data_available = FALSE;
}
void nps_sensor_gyro_init(struct NpsSensorGyro* gyro, double time) {
FLOAT_VECT3_ZERO(gyro->value);
gyro->resolution = NPS_GYRO_RESOLUTION;
FLOAT_MAT33_DIAG(gyro->sensitivity,
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);
VECT3_ASSIGN(gyro->noise_std_dev,
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);
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);
FLOAT_VECT3_ZERO(gyro->bias_random_walk_value);
gyro->next_update = time;
gyro->data_available = FALSE;
}
