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; }