void nps_sensors_init(double time) {
struct DoubleEulers body_to_imu_eulers =
{ NPS_BODY_TO_IMU_PHI, NPS_BODY_TO_IMU_THETA, NPS_BODY_TO_IMU_PSI };
DOUBLE_RMAT_OF_EULERS(sensors.body_to_imu_rmat, body_to_imu_eulers);
nps_sensor_gyro_init(&sensors.gyro, time);
nps_sensor_accel_init(&sensors.accel, time);
nps_sensor_mag_init(&sensors.mag, time);
nps_sensor_baro_init(&sensors.baro, time);
nps_sensor_gps_init(&sensors.gps, time);
}
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;
}