Ejemplo n.º 1
0
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;
}
Ejemplo n.º 2
0
void nps_sensor_mag_run_step(struct NpsSensorMag* mag, double time, struct DoubleRMat* body_to_imu) {

  if (time < mag->next_update)
    return;

  /* transform magnetic field to body frame */
  struct DoubleVect3 h_body;
  double_quat_vmult(&h_body, &fdm.ltp_to_body_quat, &fdm.ltp_h);

  /* transform to imu frame */
  struct DoubleVect3 h_imu;
  MAT33_VECT3_MUL(h_imu, *body_to_imu, h_body );

  /* transform to sensor frame */
  struct DoubleVect3 h_sensor;
  MAT33_VECT3_MUL(h_sensor, mag->imu_to_sensor_rmat, h_imu );

  /* compute magnetometer reading */
  MAT33_VECT3_MUL(mag->value, mag->sensitivity, h_sensor);
  VECT3_ADD(mag->value, mag->neutral);
  /* FIXME: ADD error reading */

  /* round signal to account for adc discretisation */
  DOUBLE_VECT3_ROUND(mag->value);
  /* saturate                                       */
  VECT3_BOUND_CUBE(mag->value, mag->min, mag->max);

  mag->next_update += NPS_MAG_DT;
  mag->data_available = TRUE;
}
Ejemplo n.º 3
0
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;
}