コード例 #1
0
ファイル: test_algebra.c プロジェクト: EwoudSmeur/paparazzi
static void test_7(void)
{

  printf("\n");
  struct FloatEulers ea2c;
  EULERS_ASSIGN(ea2c, RadOfDeg(29.742755), RadOfDeg(-40.966522), RadOfDeg(69.467265));
  DISPLAY_FLOAT_EULERS_DEG("ea2c", ea2c);

  struct FloatEulers eb2c;
  EULERS_ASSIGN(eb2c, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(90.));
  DISPLAY_FLOAT_EULERS_DEG("eb2c", eb2c);
  struct FloatRMat fa2c;
  float_rmat_of_eulers(&fa2c, &ea2c);
  struct FloatRMat fb2c;
  float_rmat_of_eulers(&fb2c, &eb2c);

  printf("\n");
  test_rmat_comp_inv(fa2c, fb2c, 1);

  struct FloatQuat qa2c;
  float_quat_of_eulers(&qa2c, &ea2c);
  struct FloatQuat qb2c;
  float_quat_of_eulers(&qb2c, &eb2c);

  printf("\n");
  test_quat_comp_inv(qa2c, qb2c, 1);
  printf("\n");

}
コード例 #2
0
ファイル: test_algebra.c プロジェクト: EwoudSmeur/paparazzi
static void test_6(void)
{

  printf("\n");
  struct FloatEulers ea2b;
  EULERS_ASSIGN(ea2b, RadOfDeg(45.), RadOfDeg(22.), RadOfDeg(0.));
  DISPLAY_FLOAT_EULERS_DEG("ea2b", ea2b);
  struct FloatEulers eb2c;
  EULERS_ASSIGN(eb2c, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(90.));
  DISPLAY_FLOAT_EULERS_DEG("eb2c", eb2c);
  struct FloatRMat fa2b;
  float_rmat_of_eulers(&fa2b, &ea2b);
  struct FloatRMat fb2c;
  float_rmat_of_eulers(&fb2c, &eb2c);

  printf("\n");
  test_rmat_comp(fa2b, fb2c, 1);

  struct FloatQuat qa2b;
  float_quat_of_eulers(&qa2b, &ea2b);
  struct FloatQuat qb2c;
  float_quat_of_eulers(&qb2c, &eb2c);

  printf("\n");
  test_quat_comp(qa2b, qb2c, 1);
  printf("\n");

}
コード例 #3
0
ファイル: test_algebra.c プロジェクト: EwoudSmeur/paparazzi
float test_quat2(void)
{

  struct FloatEulers eula2b;
  EULERS_ASSIGN(eula2b, RadOfDeg(70.), RadOfDeg(0.), RadOfDeg(0.));
  //  DISPLAY_FLOAT_EULERS_DEG("eula2b", eula2b);

  struct FloatQuat qa2b;
  float_quat_of_eulers(&qa2b, &eula2b);
  DISPLAY_FLOAT_QUAT("qa2b", qa2b);

  struct DoubleEulers eula2b_d;
  EULERS_ASSIGN(eula2b_d, RadOfDeg(70.), RadOfDeg(0.), RadOfDeg(0.));
  struct DoubleQuat qa2b_d;
  double_quat_of_eulers(&qa2b_d, &eula2b_d);
  DISPLAY_FLOAT_QUAT("qa2b_d", qa2b_d);

  struct FloatVect3 u = { 1., 0., 0.};
  float angle = RadOfDeg(70.);

  struct FloatQuat q;
  FLOAT_QUAT_OF_AXIS_ANGLE(q, u, angle);
  DISPLAY_FLOAT_QUAT("q ", q);


  struct FloatEulers eula2c;
  EULERS_ASSIGN(eula2c, RadOfDeg(80.), RadOfDeg(0.), RadOfDeg(0.));
  //  DISPLAY_FLOAT_EULERS_DEG("eula2c", eula2c);

  struct FloatQuat qa2c;
  float_quat_of_eulers(&qa2c, &eula2c);
  DISPLAY_FLOAT_QUAT("qa2c", qa2c);


  struct FloatQuat qb2a;
  FLOAT_QUAT_INVERT(qb2a, qa2b);
  DISPLAY_FLOAT_QUAT("qb2a", qb2a);


  struct FloatQuat qb2c1;
  float_quat_comp(&qb2c1, &qb2a, &qa2c);
  DISPLAY_FLOAT_QUAT("qb2c1", qb2c1);

  struct FloatQuat qb2c2;
  float_quat_inv_comp(&qb2c2, &qa2b, &qa2c);
  DISPLAY_FLOAT_QUAT("qb2c2", qb2c2);

  return 0.;

}
コード例 #4
0
void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
{
    // copy euler setpoint for debugging
    EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *rpy);

    float_quat_of_eulers(&stab_att_sp_quat, &stab_att_sp_euler);
}
コード例 #5
0
ファイル: test_algebra.c プロジェクト: EwoudSmeur/paparazzi
static void test_10(void)
{

  struct FloatEulers euler;
  EULERS_ASSIGN(euler , RadOfDeg(0.), RadOfDeg(10.), RadOfDeg(0.));
  DISPLAY_FLOAT_EULERS_DEG("euler", euler);
  struct FloatQuat quat;
  float_quat_of_eulers(&quat, &euler);
  DISPLAY_FLOAT_QUAT("####quat", quat);

  struct Int32Eulers euleri;
  EULERS_BFP_OF_REAL(euleri, euler);
  DISPLAY_INT32_EULERS("euleri", euleri);
  struct Int32Quat quati;
  int32_quat_of_eulers(&quati, &euleri);
  DISPLAY_INT32_QUAT("####quat", quati);
  struct Int32RMat rmati;
  int32_rmat_of_eulers(&rmati, &euleri);
  DISPLAY_INT32_RMAT("####rmat", rmati);

  struct Int32Quat quat_ltp_to_body;
  struct Int32Quat body_to_imu_quat;
  int32_quat_identity(&body_to_imu_quat);


  int32_quat_comp_inv(&quat_ltp_to_body, &body_to_imu_quat, &quati);
  DISPLAY_INT32_QUAT("####quat_ltp_to_body", quat_ltp_to_body);

}
コード例 #6
0
ファイル: ahrs_on_synth.c プロジェクト: EwoudSmeur/paparazzi
static void traj_coordinated_circle_update(void)
{
  const float speed = 15;  // m/s
  const float R = 100;     // radius in m
  float omega = speed / R;
  // tan phi = v^2/Rg
  float phi = atan2(speed * speed, R * 9.81);
  if (aos.time > 2.*M_PI / omega) {
    VECT3_ASSIGN(aos.ltp_pos,                R * cos(omega * aos.time),              R * sin(omega * aos.time), 0.);
    VECT3_ASSIGN(aos.ltp_vel,         -omega * R * sin(omega * aos.time),        omega * R * cos(omega * aos.time), 0.);
    VECT3_ASSIGN(aos.ltp_accel, -omega * omega * R * cos(omega * aos.time), -omega * omega * R * sin(omega * aos.time), 0.);


    //  float psi = atan2(aos.ltp_pos.y, aos.ltp_pos.x);
    float psi = M_PI_2 + omega * aos.time;
    while (psi > M_PI) { psi -= 2.*M_PI; }
    EULERS_ASSIGN(aos.ltp_to_imu_euler,   phi, 0, psi);
    float_quat_of_eulers(&aos.ltp_to_imu_quat, &aos.ltp_to_imu_euler);

    struct FloatEulers e_dot;
    EULERS_ASSIGN(e_dot, 0., 0., omega);
    float_rates_of_euler_dot(&aos.imu_rates, &aos.ltp_to_imu_euler, &e_dot);
  }

}
コード例 #7
0
ファイル: ahrs_on_synth.c プロジェクト: EwoudSmeur/paparazzi
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);
  }
}
コード例 #8
0
ファイル: ahrs_on_synth.c プロジェクト: EwoudSmeur/paparazzi
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);
  }
}
コード例 #9
0
ファイル: ahrs_on_synth.c プロジェクト: EwoudSmeur/paparazzi
static void traj_bungee_takeoff_init(void)
{

  aos.traj->te = 40.;
  EULERS_ASSIGN(aos.ltp_to_imu_euler, 0, RadOfDeg(10), 0);
  float_quat_of_eulers(&aos.ltp_to_imu_quat, &aos.ltp_to_imu_euler);

}
コード例 #10
0
ファイル: ahrs_on_synth.c プロジェクト: EwoudSmeur/paparazzi
static void traj_coordinated_circle_init(void)
{

  aos.traj->te = 120.;

  const float speed = 15;  // m/s
  const float R = 100;     // radius in m
  // tan phi = v^2/Rg
  float phi = atan2(speed * speed, R * 9.81);
  EULERS_ASSIGN(aos.ltp_to_imu_euler,   phi, 0, M_PI_2);
  float_quat_of_eulers(&aos.ltp_to_imu_quat, &aos.ltp_to_imu_euler);
}
コード例 #11
0
ファイル: ahrs_vectornav.c プロジェクト: EwoudSmeur/paparazzi
/**
 * Read received data
 */
void ahrs_vectornav_propagate(void)
{
  // Rates [rad/s]
  static struct FloatRates body_rate;
  float_rmat_ratemult(&body_rate, orientationGetRMat_f(&ahrs_vn.body_to_imu), &ahrs_vn.vn_data.gyro); // compute body rates
  stateSetBodyRates_f(&body_rate);   // Set state [rad/s]

  // Attitude [deg]
  static struct FloatQuat imu_quat; // convert from euler to quat
  float_quat_of_eulers(&imu_quat, &ahrs_vn.vn_data.attitude);
  stateSetNedToBodyQuat_f(&imu_quat);

}
コード例 #12
0
ファイル: test_algebra.c プロジェクト: EwoudSmeur/paparazzi
static void test_2(void)
{

  struct Int32Vect3 v1 = { 5000, 5000, 5000 };
  DISPLAY_INT32_VECT3("v1", v1);

  struct FloatEulers euler_f = { RadOfDeg(45.), RadOfDeg(0.), RadOfDeg(0.)};
  DISPLAY_FLOAT_EULERS("euler_f", euler_f);

  struct Int32Eulers euler_i;
  EULERS_BFP_OF_REAL(euler_i, euler_f);
  DISPLAY_INT32_EULERS("euler_i", euler_i);

  struct Int32Quat quat_i;
  int32_quat_of_eulers(&quat_i, &euler_i);
  DISPLAY_INT32_QUAT("quat_i", quat_i);
  int32_quat_normalize(&quat_i);
  DISPLAY_INT32_QUAT("quat_i_n", quat_i);

  struct Int32Vect3 v2;
  int32_quat_vmult(&v2, &quat_i, &v1);
  DISPLAY_INT32_VECT3("v2", v2);

  struct Int32RMat rmat_i;
  int32_rmat_of_quat(&rmat_i, &quat_i);
  DISPLAY_INT32_RMAT("rmat_i", rmat_i);

  struct Int32Vect3 v3;
  INT32_RMAT_VMULT(v3, rmat_i, v1);
  DISPLAY_INT32_VECT3("v3", v3);

  struct Int32RMat rmat_i2;
  int32_rmat_of_eulers(&rmat_i2, &euler_i);
  DISPLAY_INT32_RMAT("rmat_i2", rmat_i2);

  struct Int32Vect3 v4;
  INT32_RMAT_VMULT(v4, rmat_i2, v1);
  DISPLAY_INT32_VECT3("v4", v4);

  struct FloatQuat quat_f;
  float_quat_of_eulers(&quat_f, &euler_f);
  DISPLAY_FLOAT_QUAT("quat_f", quat_f);

  struct FloatVect3 v5;
  VECT3_COPY(v5, v1);
  DISPLAY_FLOAT_VECT3("v5", v5);
  struct FloatVect3 v6;
  float_quat_vmult(&v6, &quat_f, &v5);
  DISPLAY_FLOAT_VECT3("v6", v6);

}
コード例 #13
0
void orientationCalcQuat_f(struct OrientationReps* orientation)
{
  if (bit_is_set(orientation->status, ORREP_QUAT_F)) {
    return;
  }

  if (bit_is_set(orientation->status, ORREP_QUAT_I)) {
    QUAT_FLOAT_OF_BFP(orientation->quat_f, orientation->quat_i);
  } else if (bit_is_set(orientation->status, ORREP_RMAT_F)) {
    float_quat_of_rmat(&(orientation->quat_f), &(orientation->rmat_f));
  } else if (bit_is_set(orientation->status, ORREP_EULER_F)) {
    float_quat_of_eulers(&(orientation->quat_f), &(orientation->eulers_f));
  } else if (bit_is_set(orientation->status, ORREP_RMAT_I)) {
    RMAT_FLOAT_OF_BFP(orientation->rmat_f, orientation->rmat_i);
    SetBit(orientation->status, ORREP_RMAT_F);
    float_quat_of_rmat(&(orientation->quat_f), &(orientation->rmat_f));
  } else if (bit_is_set(orientation->status, ORREP_EULER_I)) {
    EULERS_FLOAT_OF_BFP(orientation->eulers_f, orientation->eulers_i);
    SetBit(orientation->status, ORREP_EULER_F);
    float_quat_of_eulers(&(orientation->quat_f), &(orientation->eulers_f));
  }
  /* set bit to indicate this representation is computed */
  SetBit(orientation->status, ORREP_QUAT_F);
}
コード例 #14
0
ファイル: test_algebra.c プロジェクト: EwoudSmeur/paparazzi
static void test_5(void)
{

  struct FloatEulers fe;
  EULERS_ASSIGN(fe, RadOfDeg(-10.66), RadOfDeg(-0.7), RadOfDeg(0.));
  DISPLAY_FLOAT_EULERS_DEG("fe", fe);
  printf("\n");
  struct FloatQuat fq;
  float_quat_of_eulers(&fq, &fe);
  test_eulers_of_quat(fq, 1);
  printf("\n");
  struct FloatRMat frm;
  float_rmat_of_eulers(&frm, &fe);
  DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("frm", frm);
  test_eulers_of_rmat(frm, 1);
  printf("\n");

}
コード例 #15
0
ファイル: test_algebra.c プロジェクト: EwoudSmeur/paparazzi
float test_quat_of_rmat(void)
{

  //  struct FloatEulers eul = {-0.280849, 0.613423, -1.850440};
  struct FloatEulers eul = {RadOfDeg(0.131579),  RadOfDeg(-62.397659), RadOfDeg(-110.470299)};
  //  struct FloatEulers eul = {RadOfDeg(0.13), RadOfDeg(180.), RadOfDeg(-61.)};

  struct FloatRMat rm;
  float_rmat_of_eulers(&rm, &eul);
  DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", rm);

  struct FloatQuat q;
  float_quat_of_rmat(&q, &rm);
  DISPLAY_FLOAT_QUAT("q_of_rm   ", q);
  DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("q_of_rm   ", q);

  struct FloatQuat qref;
  float_quat_of_eulers(&qref, &eul);
  DISPLAY_FLOAT_QUAT("q_of_euler", qref);
  DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("q_of_euler", qref);

  printf("\n\n\n");

  struct FloatRMat r_att;
  struct FloatEulers e312 = { eul.phi, eul.theta, eul.psi };
  float_rmat_of_eulers_312(&r_att, &e312);
  DISPLAY_FLOAT_RMAT("r_att  ", r_att);
  DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("r_att  ", r_att);

  struct FloatQuat q_att;
  float_quat_of_rmat(&q_att, &r_att);

  struct FloatEulers e_att;
  float_eulers_of_rmat(&e_att, &r_att);
  DISPLAY_FLOAT_EULERS_DEG("of rmat", e_att);

  float_eulers_of_quat(&e_att, &q_att);
  DISPLAY_FLOAT_EULERS_DEG("of quat", e_att);

  return 0.;
}
コード例 #16
0
ファイル: ahrs_on_synth.c プロジェクト: EwoudSmeur/paparazzi
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);

}
コード例 #17
0
ファイル: test_algebra.c プロジェクト: EwoudSmeur/paparazzi
static void test_1(void)
{

  struct FloatEulers euler_f = { RadOfDeg(45.), RadOfDeg(0.), RadOfDeg(0.)};
  DISPLAY_FLOAT_EULERS("euler_f", euler_f);

  struct Int32Eulers euler_i;
  EULERS_BFP_OF_REAL(euler_i, euler_f);
  DISPLAY_INT32_EULERS("euler_i", euler_i);

  struct FloatQuat quat_f;
  float_quat_of_eulers(&quat_f, &euler_f);
  DISPLAY_FLOAT_QUAT("quat_f", quat_f);

  struct Int32Quat quat_i;
  int32_quat_of_eulers(&quat_i, &euler_i);
  DISPLAY_INT32_QUAT("quat_i", quat_i);

  struct Int32RMat rmat_i;
  int32_rmat_of_quat(&rmat_i, &quat_i);
  DISPLAY_INT32_RMAT("rmat_i", rmat_i);

}
コード例 #18
0
ファイル: test_algebra.c プロジェクト: EwoudSmeur/paparazzi
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);


}
コード例 #19
0
ファイル: guidance_indi.c プロジェクト: 2seasuav/paparuzzi
void stabilization_attitude_set_setpoint_rp_quat_f(bool in_flight, int32_t heading)
{
  struct FloatQuat q_rp_cmd;
  float_quat_of_eulers(&q_rp_cmd, &guidance_euler_cmd); //TODO this is a quaternion without yaw! add the desired yaw before you use it!

  /* get current heading */
  const struct FloatVect3 zaxis = {0., 0., 1.};
  struct FloatQuat q_yaw;

  float_quat_of_axis_angle(&q_yaw, &zaxis, stateGetNedToBodyEulers_f()->psi);

  /* roll/pitch commands applied to to current heading */
  struct FloatQuat q_rp_sp;
  float_quat_comp(&q_rp_sp, &q_yaw, &q_rp_cmd);
  float_quat_normalize(&q_rp_sp);

  struct FloatQuat q_sp;

  if (in_flight) {
    /* get current heading setpoint */
    struct FloatQuat q_yaw_sp;
    float_quat_of_axis_angle(&q_yaw_sp, &zaxis, ANGLE_FLOAT_OF_BFP(heading));


    /* rotation between current yaw and yaw setpoint */
    struct FloatQuat q_yaw_diff;
    float_quat_comp_inv(&q_yaw_diff, &q_yaw_sp, &q_yaw);

    /* compute final setpoint with yaw */
    float_quat_comp_norm_shortest(&q_sp, &q_rp_sp, &q_yaw_diff);
  } else {
    QUAT_COPY(q_sp, q_rp_sp);
  }

  QUAT_BFP_OF_REAL(stab_att_sp_quat,q_sp);
}
コード例 #20
0
/**
 *  Propagate the received states into the vehicle
 *  state machine
 */
void ins_vectornav_propagate()
{
  // Acceleration [m/s^2]
  // in fixed point for sending as ABI and telemetry msgs
  ACCELS_BFP_OF_REAL(ins_vn.accel_i, ins_vn.accel);

  // Rates [rad/s]
  static struct FloatRates body_rate;
  // in fixed point for sending as ABI and telemetry msgs
  RATES_BFP_OF_REAL(ins_vn.gyro_i, ins_vn.gyro);
  float_rmat_ratemult(&body_rate, orientationGetRMat_f(&ins_vn.body_to_imu), &ins_vn.gyro); // compute body rates
  stateSetBodyRates_f(&body_rate);   // Set state [rad/s]

  // Attitude [deg]
  ins_vectornav_yaw_pitch_roll_to_attitude(&ins_vn.attitude); // convert to correct units and axis [rad]
  static struct FloatQuat imu_quat; // convert from euler to quat
  float_quat_of_eulers(&imu_quat, &ins_vn.attitude);
  static struct FloatRMat imu_rmat; // convert from quat to rmat
  float_rmat_of_quat(&imu_rmat, &imu_quat);
  static struct FloatRMat ltp_to_body_rmat; // rotate to body frame
  float_rmat_comp(&ltp_to_body_rmat, &imu_rmat, orientationGetRMat_f(&ins_vn.body_to_imu));
  stateSetNedToBodyRMat_f(&ltp_to_body_rmat); // set body states [rad]

  // NED (LTP) velocity [m/s]
  // North east down (NED), also known as local tangent plane (LTP),
  // is a geographical coordinate system for representing state vectors that is commonly used in aviation.
  // It consists of three numbers: one represents the position along the northern axis,
  // one along the eastern axis, and one represents vertical position. Down is chosen as opposed to
  // up in order to comply with the right-hand rule.
  // The origin of this coordinate system is usually chosen to be the aircraft's center of gravity.
  // x = North
  // y = East
  // z = Down
  stateSetSpeedNed_f(&ins_vn.vel_ned); // set state

  // NED (LTP) acceleration [m/s^2]
  static struct FloatVect3 accel_meas_ltp;// first we need to rotate linear acceleration from imu-frame to body-frame
  float_rmat_transp_vmult(&accel_meas_ltp, orientationGetRMat_f(&ins_vn.body_to_imu), &(ins_vn.lin_accel));
  static struct NedCoor_f ltp_accel; // assign to NedCoord_f struct
  VECT3_ASSIGN(ltp_accel, accel_meas_ltp.x, accel_meas_ltp.y, accel_meas_ltp.z);
  stateSetAccelNed_f(&ltp_accel); // then set the states
  ins_vn.ltp_accel_f = ltp_accel;

  // LLA position [rad, rad, m]
  //static struct LlaCoor_f lla_pos; // convert from deg to rad, and from double to float
  ins_vn.lla_pos.lat = RadOfDeg((float)ins_vn.pos_lla[0]); // ins_impl.pos_lla[0] = lat
  ins_vn.lla_pos.lon = RadOfDeg((float)ins_vn.pos_lla[1]); // ins_impl.pos_lla[1] = lon
  ins_vn.lla_pos.alt = ((float)ins_vn.pos_lla[2]); // ins_impl.pos_lla[2] = alt
  LLA_BFP_OF_REAL(gps.lla_pos, ins_vn.lla_pos);
  stateSetPositionLla_i(&gps.lla_pos);

  // ECEF position
  struct LtpDef_f def;
  ltp_def_from_lla_f(&def, &ins_vn.lla_pos);
  struct EcefCoor_f ecef_vel;
  ecef_of_ned_point_f(&ecef_vel, &def, &ins_vn.vel_ned);
  ECEF_BFP_OF_REAL(gps.ecef_vel, ecef_vel);

  // ECEF velocity
  gps.ecef_pos.x = stateGetPositionEcef_i()->x;
  gps.ecef_pos.y = stateGetPositionEcef_i()->y;
  gps.ecef_pos.z = stateGetPositionEcef_i()->z;


#if GPS_USE_LATLONG
  // GPS UTM
  /* Computes from (lat, long) in the referenced UTM zone */
  struct UtmCoor_f utm_f;
  utm_f.zone = nav_utm_zone0;
  /* convert to utm */
  //utm_of_lla_f(&utm_f, &lla_f);
  utm_of_lla_f(&utm_f, &ins_vn.lla_pos);
  /* copy results of utm conversion */
  gps.utm_pos.east = (int32_t)(utm_f.east * 100);
  gps.utm_pos.north = (int32_t)(utm_f.north * 100);
  gps.utm_pos.alt = (int32_t)(utm_f.alt * 1000);
  gps.utm_pos.zone = (uint8_t)nav_utm_zone0;
#endif

  // GPS Ground speed
  float speed = sqrt(ins_vn.vel_ned.x * ins_vn.vel_ned.x + ins_vn.vel_ned.y * ins_vn.vel_ned.y);
  gps.gspeed = ((uint16_t)(speed * 100));

  // GPS course
  gps.course = (int32_t)(1e7 * (atan2(ins_vn.vel_ned.y, ins_vn.vel_ned.x)));

  // Because we have not HMSL data from Vectornav, we are using LLA-Altitude
  // as a workaround
  gps.hmsl = (uint32_t)(gps.lla_pos.alt);

  // set position uncertainty
  ins_vectornav_set_pacc();

  // set velocity uncertainty
  ins_vectornav_set_sacc();

  // check GPS status
  gps.last_msg_time = sys_time.nb_sec;
  gps.last_msg_ticks = sys_time.nb_sec_rem;
  if (gps.fix == GPS_FIX_3D) {
    gps.last_3dfix_time = sys_time.nb_sec;
    gps.last_3dfix_ticks = sys_time.nb_sec_rem;
  }

  // read INS status
  ins_vectornav_check_status();

  // update internal states for telemetry purposes
  // TODO: directly convert vectornav output instead of using state interface
  // to support multiple INS running at the same time
  ins_vn.ltp_pos_i = *stateGetPositionNed_i();
  ins_vn.ltp_speed_i = *stateGetSpeedNed_i();
  ins_vn.ltp_accel_i = *stateGetAccelNed_i();

  // send ABI messages
  uint32_t now_ts = get_sys_time_usec();
  AbiSendMsgGPS(GPS_UBX_ID, now_ts, &gps);
  AbiSendMsgIMU_GYRO_INT32(IMU_ASPIRIN_ID, now_ts, &ins_vn.gyro_i);
  AbiSendMsgIMU_ACCEL_INT32(IMU_ASPIRIN_ID, now_ts, &ins_vn.accel_i);
}
コード例 #21
0
ファイル: ahrs_on_synth.c プロジェクト: EwoudSmeur/paparazzi
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);

};