Пример #1
0
struct DoubleEulers sigma_euler_from_sigma_q(struct DoubleQuat q, struct DoubleQuat sigma_q){
  struct DoubleVect3 v_q, v_sigma, temporary_result;
  struct DoubleEulers sigma_eu;
  
  QUAT_IMAGINARY_PART(q, v_q);
  QUAT_IMAGINARY_PART(sigma_q, v_sigma);
  if(DOUBLE_VECT3_NORM(v_sigma)>0.5){
    EULERS_ASSIGN(sigma_eu, M_PI_2, M_PI_2, M_PI_2);
    return sigma_eu;
  }
  
  VECT3_CROSS_PRODUCT(temporary_result, v_q, v_sigma);
  
  VECT3_SMUL(v_q, v_q, sigma_q.qi);
  VECT3_SMUL(v_sigma, v_sigma, q.qi);
  
  VECT3_ADD(temporary_result, v_sigma);
  VECT3_SUB(temporary_result, v_q);
  
  VECT3_TO_EULERS(temporary_result, sigma_eu);
  
  return sigma_eu;
}
Пример #2
0
static void traj_sineX_quad_update(void) {

  const float om = RadOfDeg(10);

  if ( aos.time > (M_PI/om) ) {
    const float a = 20;

    struct FloatVect3 jerk;
    VECT3_ASSIGN(jerk           , -a*om*om*om*cos(om*aos.time), 0, 0);
    VECT3_ASSIGN(aos.ltp_accel  , -a*om*om   *sin(om*aos.time), 0, 0);
    VECT3_ASSIGN(aos.ltp_vel    ,  a*om      *cos(om*aos.time), 0, 0);
    VECT3_ASSIGN(aos.ltp_pos    ,  a         *sin(om*aos.time), 0, 0);
    
    // this is based on differential flatness of the quad
    EULERS_ASSIGN(aos.ltp_to_imu_euler,    0., atan2(aos.ltp_accel.x, 9.81), 0.);
    FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler);
    const struct FloatEulers e_dot = {
      0., 
      9.81*jerk.x / ( (9.81*9.81) + (aos.ltp_accel.x*aos.ltp_accel.x)),
      0. };
    FLOAT_RATES_OF_EULER_DOT(aos.imu_rates, aos.ltp_to_imu_euler, e_dot);
  }
}
Пример #3
0
static void test_4_float(void)
{

  printf("euler to quat to euler - float\n");
  /* initial euler angles */
  struct FloatEulers e;
  EULERS_ASSIGN(e, RadOfDeg(-10.66), RadOfDeg(-0.7), RadOfDeg(0.));
  DISPLAY_FLOAT_EULERS_DEG("e", e);

  /* transform to quaternion */
  struct FloatQuat q;
  float_quat_of_eulers(&q, &e);
  //  DISPLAY_FLOAT_QUAT("q", q);
  float_quat_normalize(&q);
  DISPLAY_FLOAT_QUAT("q_n", q);
  DISPLAY_FLOAT_QUAT_AS_INT("q_n as int", q);
  /* back to eulers */
  struct FloatEulers e2;
  float_eulers_of_quat(&e2, &q);
  DISPLAY_FLOAT_EULERS_DEG("e2", e2);


}
Пример #4
0
static void test_4_int(void)
{

  printf("euler to quat to euler - int\n");
  /* initial euler angles */
  struct Int32Eulers _e;
  EULERS_ASSIGN(_e, ANGLE_BFP_OF_REAL(RadOfDeg(-10.66)), ANGLE_BFP_OF_REAL(RadOfDeg(-0.7)),
                ANGLE_BFP_OF_REAL(RadOfDeg(0.)));
  DISPLAY_INT32_EULERS_AS_FLOAT_DEG("euler orig ", _e);

  /* transform to quaternion */
  struct Int32Quat _q;
  int32_quat_of_eulers(&_q, &_e);
  DISPLAY_INT32_QUAT_AS_EULERS_DEG("quat1 ", _q);
  //  int32_quat_normalize(&_q);
  //  DISPLAY_INT32_QUAT_2("_q_n", _q);

  /* back to eulers */
  struct Int32Eulers _e2;
  int32_eulers_of_quat(&_e2, &_q);
  DISPLAY_INT32_EULERS_AS_FLOAT_DEG("back to euler ", _e2);


}
Пример #5
0
static void  traj_stop_stop_x_update(void){

  const float t0 = 5.;
  const float dt_jerk = 0.75;
  const float dt_nojerk = 10.;
  const float val_jerk = 5.;

  FLOAT_VECT3_INTEGRATE_FI(aos.ltp_pos, aos.ltp_vel, aos.dt);
  FLOAT_VECT3_INTEGRATE_FI(aos.ltp_vel, aos.ltp_accel, aos.dt);
  FLOAT_VECT3_INTEGRATE_FI(aos.ltp_accel, aos.ltp_jerk, aos.dt);

  if (aos.time < t0) return;
  else if (aos.time < t0+dt_jerk) {
    VECT3_ASSIGN(aos.ltp_jerk           ,  val_jerk, 0., 0.); }
  else if (aos.time < t0 + 2.*dt_jerk) {
    VECT3_ASSIGN(aos.ltp_jerk           , -val_jerk, 0., 0.); }
  else if (aos.time < t0 + 2.*dt_jerk + dt_nojerk) {
    VECT3_ASSIGN(aos.ltp_jerk           ,       0. , 0., 0.); }
  else if (aos.time < t0 + 3.*dt_jerk + dt_nojerk) {
    VECT3_ASSIGN(aos.ltp_jerk           , -val_jerk, 0., 0.); }
  else if (aos.time < t0 + 4.*dt_jerk + dt_nojerk) {
    VECT3_ASSIGN(aos.ltp_jerk           ,  val_jerk, 0., 0.); }
  else {
    VECT3_ASSIGN(aos.ltp_jerk           ,       0. , 0., 0.); }
  
    
  // this is based on differential flatness of the quad
  EULERS_ASSIGN(aos.ltp_to_imu_euler,    0., atan2(aos.ltp_accel.x, 9.81), 0.);
  FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler);
  const struct FloatEulers e_dot = {
    0., 
    9.81*aos.ltp_jerk.x / ( (9.81*9.81) + (aos.ltp_accel.x*aos.ltp_accel.x)),
    0. };
  FLOAT_RATES_OF_EULER_DOT(aos.imu_rates, aos.ltp_to_imu_euler, e_dot);

}
Пример #6
0
static void test_3(void) {

    /* Compute BODY to IMU eulers */
    struct Int32Eulers b2i_e;
    EULERS_ASSIGN(b2i_e, ANGLE_BFP_OF_REAL(RadOfDeg(10.66)), ANGLE_BFP_OF_REAL(RadOfDeg(-0.7)), ANGLE_BFP_OF_REAL(RadOfDeg(0.)));
    DISPLAY_INT32_EULERS_AS_FLOAT_DEG("b2i_e", b2i_e);

    /* Compute BODY to IMU quaternion */
    struct Int32Quat b2i_q;
    INT32_QUAT_OF_EULERS(b2i_q, b2i_e);
    DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q", b2i_q);
    //  INT32_QUAT_NORMALIZE(b2i_q);
    //  DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q_n", b2i_q);

    /* Compute BODY to IMU rotation matrix */
    struct Int32RMat b2i_r;
    INT32_RMAT_OF_EULERS(b2i_r, b2i_e);
    //  DISPLAY_INT32_RMAT("b2i_r", b2i_r);
    DISPLAY_INT32_RMAT_AS_EULERS_DEG("b2i_r", b2i_r);

    /* Compute LTP to IMU eulers */
    struct Int32Eulers l2i_e;
    EULERS_ASSIGN(l2i_e, ANGLE_BFP_OF_REAL(RadOfDeg(0.)), ANGLE_BFP_OF_REAL(RadOfDeg(20.)), ANGLE_BFP_OF_REAL(RadOfDeg(0.)));
    DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2i_e", l2i_e);

    /* Compute LTP to IMU quaternion */
    struct Int32Quat l2i_q;
    INT32_QUAT_OF_EULERS(l2i_q, l2i_e);
    DISPLAY_INT32_QUAT_AS_EULERS_DEG("l2i_q", l2i_q);

    /* Compute LTP to IMU rotation matrix */
    struct Int32RMat l2i_r;
    INT32_RMAT_OF_EULERS(l2i_r, l2i_e);
    //  DISPLAY_INT32_RMAT("l2i_r", l2i_r);
    DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2i_r", l2i_r);


    /* again but from quaternion */
    struct Int32RMat l2i_r2;
    INT32_RMAT_OF_QUAT(l2i_r2, l2i_q);
    //  DISPLAY_INT32_RMAT("l2i_r2", l2i_r2);
    DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2i_r2", l2i_r2);

    /* Compute LTP to BODY quaternion */
    struct Int32Quat l2b_q;
    INT32_QUAT_COMP_INV(l2b_q, b2i_q, l2i_q);
    DISPLAY_INT32_QUAT_AS_EULERS_DEG("l2b_q", l2b_q);

    /* Compute LTP to BODY rotation matrix */
    struct Int32RMat l2b_r;
    INT32_RMAT_COMP_INV(l2b_r, l2i_r, b2i_r);
    //  DISPLAY_INT32_RMAT("l2b_r", l2b_r);
    DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2b_r2", l2b_r);

    /* again but from quaternion */
    struct Int32RMat l2b_r2;
    INT32_RMAT_OF_QUAT(l2b_r2, l2b_q);
    //  DISPLAY_INT32_RMAT("l2b_r2", l2b_r2);
    DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2b_r2", l2b_r2);


    /* compute LTP to BODY eulers */
    struct Int32Eulers l2b_e;
    INT32_EULERS_OF_RMAT(l2b_e, l2b_r);
    DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2b_e", l2b_e);

    /* again but from quaternion */
    struct Int32Eulers l2b_e2;
    INT32_EULERS_OF_QUAT(l2b_e2, l2b_q);
    DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2b_e2", l2b_e2);

}
Пример #7
0
static void test_3(void)
{

  /* Compute BODY to IMU eulers */
  struct Int32Eulers b2i_e;
  EULERS_ASSIGN(b2i_e, ANGLE_BFP_OF_REAL(RadOfDeg(10.66)), ANGLE_BFP_OF_REAL(RadOfDeg(-0.7)),
                ANGLE_BFP_OF_REAL(RadOfDeg(0.)));
  DISPLAY_INT32_EULERS_AS_FLOAT_DEG("b2i_e", b2i_e);

  /* Compute BODY to IMU quaternion */
  struct Int32Quat b2i_q;
  int32_quat_of_eulers(&b2i_q, &b2i_e);
  DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q", b2i_q);
  //  int32_quat_normalize(&b2i_q);
  //  DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q_n", b2i_q);

  /* Compute BODY to IMU rotation matrix */
  struct Int32RMat b2i_r;
  int32_rmat_of_eulers(&b2i_r, &b2i_e);
  //  DISPLAY_INT32_RMAT("b2i_r", b2i_r);
  DISPLAY_INT32_RMAT_AS_EULERS_DEG("b2i_r", b2i_r);

  /* Compute LTP to IMU eulers */
  struct Int32Eulers l2i_e;
  EULERS_ASSIGN(l2i_e, ANGLE_BFP_OF_REAL(RadOfDeg(0.)), ANGLE_BFP_OF_REAL(RadOfDeg(20.)),
                ANGLE_BFP_OF_REAL(RadOfDeg(0.)));
  DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2i_e", l2i_e);

  /* Compute LTP to IMU quaternion */
  struct Int32Quat l2i_q;
  int32_quat_of_eulers(&l2i_q, &l2i_e);
  DISPLAY_INT32_QUAT_AS_EULERS_DEG("l2i_q", l2i_q);

  /* Compute LTP to IMU rotation matrix */
  struct Int32RMat l2i_r;
  int32_rmat_of_eulers(&l2i_r, &l2i_e);
  //  DISPLAY_INT32_RMAT("l2i_r", l2i_r);
  DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2i_r", l2i_r);


  /* again but from quaternion */
  struct Int32RMat l2i_r2;
  int32_rmat_of_quat(&l2i_r2, &l2i_q);
  //  DISPLAY_INT32_RMAT("l2i_r2", l2i_r2);
  DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2i_r2", l2i_r2);

  /* Compute LTP to BODY quaternion */
  struct Int32Quat l2b_q;
  int32_quat_comp_inv(&l2b_q, &b2i_q, &l2i_q);
  DISPLAY_INT32_QUAT_AS_EULERS_DEG("l2b_q", l2b_q);

  /* Compute LTP to BODY rotation matrix */
  struct Int32RMat l2b_r;
  int32_rmat_comp_inv(&l2b_r, &l2i_r, &b2i_r);
  //  DISPLAY_INT32_RMAT("l2b_r", l2b_r);
  DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2b_r2", l2b_r);

  /* again but from quaternion */
  struct Int32RMat l2b_r2;
  int32_rmat_of_quat(&l2b_r2, &l2b_q);
  //  DISPLAY_INT32_RMAT("l2b_r2", l2b_r2);
  DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2b_r2", l2b_r2);


  /* compute LTP to BODY eulers */
  struct Int32Eulers l2b_e;
  int32_eulers_of_rmat(&l2b_e, &l2b_r);
  DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2b_e", l2b_e);

  /* again but from quaternion */
  struct Int32Eulers l2b_e2;
  int32_eulers_of_quat(&l2b_e2, &l2b_q);
  DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2b_e2", l2b_e2);

}
Пример #8
0
void aos_init(int traj_nb)
{

  aos.traj = &traj[traj_nb];

  aos.time = 0;
  aos.dt = 1. / AHRS_PROPAGATE_FREQUENCY;
  aos.traj->ts = 0;
  aos.traj->ts = 1.; // default to one second

  /* default state */
  EULERS_ASSIGN(aos.ltp_to_imu_euler, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.));
  float_quat_of_eulers(&aos.ltp_to_imu_quat, &aos.ltp_to_imu_euler);
  RATES_ASSIGN(aos.imu_rates, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.));
  FLOAT_VECT3_ZERO(aos.ltp_pos);
  FLOAT_VECT3_ZERO(aos.ltp_vel);
  FLOAT_VECT3_ZERO(aos.ltp_accel);
  FLOAT_VECT3_ZERO(aos.ltp_jerk);
  aos.traj->init_fun();

  imu_init();
  ahrs_init();

#ifdef PERFECT_SENSORS
  RATES_ASSIGN(aos.gyro_bias,  RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.));
  RATES_ASSIGN(aos.gyro_noise, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.));
  VECT3_ASSIGN(aos.accel_noise, 0., 0., 0.);
  aos.heading_noise = 0.;
#else
  RATES_ASSIGN(aos.gyro_bias,  RadOfDeg(1.), RadOfDeg(2.), RadOfDeg(3.));
  RATES_ASSIGN(aos.gyro_noise, RadOfDeg(1.), RadOfDeg(1.), RadOfDeg(1.));
  VECT3_ASSIGN(aos.accel_noise, .5, .5, .5);
  aos.heading_noise = RadOfDeg(3.);
#endif


#ifdef FORCE_ALIGNEMENT
  //  DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("# oas quat", aos.ltp_to_imu_quat);
  aos_compute_sensors();
  //  DISPLAY_FLOAT_RATES_DEG("# oas gyro_bias", aos.gyro_bias);
  //  DISPLAY_FLOAT_RATES_DEG("# oas imu_rates", aos.imu_rates);
  VECT3_COPY(ahrs_aligner.lp_accel, imu.accel);
  VECT3_COPY(ahrs_aligner.lp_mag, imu.mag);
  RATES_COPY(ahrs_aligner.lp_gyro, imu.gyro);
  //  DISPLAY_INT32_RATES_AS_FLOAT_DEG("# ahrs_aligner.lp_gyro", ahrs_aligner.lp_gyro);
  ahrs_align();
  //  DISPLAY_FLOAT_RATES_DEG("# ahrs_impl.gyro_bias", ahrs_impl.gyro_bias);

#endif


#ifdef DISABLE_ALIGNEMENT
  printf("# DISABLE_ALIGNEMENT\n");
#endif
#ifdef DISABLE_PROPAGATE
  printf("# DISABLE_PROPAGATE\n");
#endif
#ifdef DISABLE_ACCEL_UPDATE
  printf("# DISABLE_ACCEL_UPDATE\n");
#endif
#ifdef DISABLE_MAG_UPDATE
  printf("# DISABLE_MAG_UPDATE\n");
#endif
  printf("# AHRS_TYPE  %s\n", ahrs_type_str[AHRS_TYPE]);
  printf("# AHRS_PROPAGATE_FREQUENCY %d\n", AHRS_PROPAGATE_FREQUENCY);
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
  printf("# AHRS_PROPAGATE_LOW_PASS_RATES\n");
#endif
#if AHRS_MAG_UPDATE_YAW_ONLY
  printf("# AHRS_MAG_UPDATE_YAW_ONLY\n");
#endif
#if AHRS_GRAVITY_UPDATE_COORDINATED_TURN
  printf("# AHRS_GRAVITY_UPDATE_COORDINATED_TURN\n");
#endif
#if AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
  printf("# AHRS_GRAVITY_UPDATE_NORM_HEURISTIC\n");
#endif
#ifdef PERFECT_SENSORS
  printf("# PERFECT_SENSORS\n");
#endif
#if AHRS_USE_GPS_HEADING
  printf("# AHRS_USE_GPS_HEADING\n");
#endif
#if USE_AHRS_GPS_ACCELERATIONS
  printf("# USE_AHRS_GPS_ACCELERATIONS\n");
#endif

  printf("# tajectory : %s\n", aos.traj->name);

};
Пример #9
0
static void traj_step_phi_update(void) {
  if (aos.time > 5) {
    EULERS_ASSIGN(aos.ltp_to_imu_euler,   RadOfDeg(5), 0, 0);
    FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler);
  }
}