Exemple #1
0
void stateCalcPositionEcef_i(void)
{
  if (bit_is_set(state.pos_status, POS_ECEF_I)) {
    return;
  }

  if (bit_is_set(state.pos_status, POS_ECEF_F)) {
    ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f);
  } else if (bit_is_set(state.pos_status, POS_NED_I) && state.ned_initialized_i) {
    ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
  } else if (bit_is_set(state.pos_status, POS_NED_F) && state.ned_initialized_f) {
    /* transform ned_f to ecef_f, set status bit, then convert to int */
    ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
    SetBit(state.pos_status, POS_ECEF_F);
    ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f);
  } else if (bit_is_set(state.pos_status, POS_LLA_I)) {
    ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i);
  } else if (bit_is_set(state.pos_status, POS_LLA_F)) {
    /* transform lla_f to ecef_f, set status bit, then convert to int */
    ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
    SetBit(state.pos_status, POS_ECEF_F);
    ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f);
  } else {
    /* could not get this representation,  set errno */
    //struct EcefCoor_i _ecef_zero = {0};
    //return _ecef_zero;
  }
  /* set bit to indicate this representation is computed */
  SetBit(state.pos_status, POS_ECEF_I);
}
Exemple #2
0
void stateCalcPositionEcef_f(void) {
  if (bit_is_set(state.pos_status, POS_ECEF_F))
    return;

  if (bit_is_set(state.pos_status, POS_ECEF_I)) {
    ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i);
  }
  else if (bit_is_set(state.pos_status, POS_NED_F)) {
    ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
  }
  else if (bit_is_set(state.pos_status, POS_NED_I)) {
    /* transform ned_i -> ecef_i -> ecef_f, set status bits */
    /// @todo check if resolution is good enough
    ecef_of_ned_point_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
    SetBit(state.pos_status, POS_ECEF_F);
    ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i);
  }
  else if (bit_is_set(state.pos_status, POS_LLA_F)) {
    ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
  }
  else if (bit_is_set(state.pos_status, POS_LLA_I)) {
    LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i);
    SetBit(state.pos_status, POS_LLA_F);
    ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f);
  }
  else {
    /* could not get this representation,  set errno */
    //struct EcefCoor_f _ecef_zero = {0.0f};
    //return _ecef_zero;
  }
  /* set bit to indicate this representation is computed */
  SetBit(state.pos_status, POS_ECEF_F);
}
Exemple #3
0
void stateCalcPositionLla_f(void) {
  if (bit_is_set(state.pos_status, POS_LLA_F))
    return;

  if (bit_is_set(state.pos_status, POS_LLA_I)) {
    LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_f);
  }
  else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
    lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
  }
  else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
    /* transform ecef_i -> ecef_f -> lla_f, set status bits */
    ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i);
    SetBit(state.pos_status, POS_ECEF_F);
    lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
  }
  else if (bit_is_set(state.pos_status, POS_NED_F)) {
    /* transform ned_f -> ecef_f -> lla_f, set status bits */
    ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
    SetBit(state.pos_status, POS_ECEF_F);
    lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
  }
  else if (bit_is_set(state.pos_status, POS_NED_I)) {
    /* transform ned_i -> ned_f -> ecef_f -> lla_f, set status bits */
    NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i);
    SetBit(state.pos_status, POS_NED_F);
    ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
    SetBit(state.pos_status, POS_ECEF_F);
    lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
  }
  else if (bit_is_set(state.pos_status, POS_UTM_F)) {
    lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
  }
  else {
    /* could not get this representation,  set errno */
    //struct LlaCoor_f _lla_zero = {0.0};
    //return _lla_zero;
  }
  /* set bit to indicate this representation is computed */
  SetBit(state.pos_status, POS_LLA_F);
}
Exemple #4
0
void stateCalcPositionLla_i(void) {
  if (bit_is_set(state.pos_status, POS_LLA_I))
    return;

  if (bit_is_set(state.pos_status, POS_LLA_F)) {
    LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
  }
  else if (bit_is_set(state.pos_status, POS_ECEF_I)) {
    lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i);
  }
  else if (bit_is_set(state.pos_status, POS_ECEF_F)) {
    /* transform ecef_f -> lla_f, set status bit, then convert to int */
    lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
    SetBit(state.pos_status, POS_LLA_F);
    LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
  }
  else if (bit_is_set(state.pos_status, POS_NED_F)) {
    /* transform ned_f -> ecef_f -> lla_f -> lla_i, set status bits */
    ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f);
    SetBit(state.pos_status, POS_ECEF_F);
    lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f);
    SetBit(state.pos_status, POS_LLA_F);
    LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
  }
  else if (bit_is_set(state.pos_status, POS_NED_I)) {
    /* transform ned_i -> ecef_i -> lla_i, set status bits */
    /// @todo check if resolution is enough
    ecef_of_ned_point_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i);
    SetBit(state.pos_status, POS_ECEF_I);
    lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i); /* uses double version internally */
  }
  else if (bit_is_set(state.pos_status, POS_UTM_F)) {
    /* transform utm_f -> lla_f -> lla_i, set status bits */
    lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f);
    SetBit(state.pos_status, POS_LLA_F);
    LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f);
  }
  else {
    /* could not get this representation,  set errno */
    //struct LlaCoor_i _lla_zero = {0};
    //return _lla_zero;
  }
  /* set bit to indicate this representation is computed */
  SetBit(state.pos_status, POS_LLA_I);
}
/**
 *  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);
}