Пример #1
0
/***************************************************************************************
 *decode the gps data packet
 ***************************************************************************************/
static void decode_gpspacket(struct NpsFdm *fdm, byte *buffer)
{
  /* gps velocity (1e2 m/s to  m/s */
  struct NedCoor_d vel;
  vel.x = (double)LongOfBuf(buffer, 3) * 1.0e-2;
  vel.y = (double)LongOfBuf(buffer, 7) * 1.0e-2;
  vel.z = (double)LongOfBuf(buffer, 11) * 1.0e-2;
  fdm->ltp_ecef_vel = vel;
  ecef_of_ned_vect_d(&fdm->ecef_ecef_vel, &ltpdef, &vel);

  /* No airspeed from CRRCSIM?
   * use ground speed for now, since we also don't know wind
   */
  struct DoubleVect3 ltp_airspeed;
  VECT3_COPY(ltp_airspeed, vel);
  fdm.airspeed = double_vect3_norm(&ltp_airspeed);

  /* gps position (1e7 deg to rad and 1e3 m to m) */
  struct LlaCoor_d pos;
  pos.lon = (double)LongOfBuf(buffer, 15) * 1.74533e-9;
  pos.lat = (double)LongOfBuf(buffer, 19) * 1.74533e-9;
  pos.alt = (double)LongOfBuf(buffer, 23) * 1.0e-3;

  pos.lat += ltpdef.lla.lat;
  pos.lon += ltpdef.lla.lon;
  pos.alt += ltpdef.lla.alt;

  fdm->lla_pos = pos;
  ecef_of_lla_d(&fdm->ecef_pos, &pos);
  fdm->hmsl = pos.alt - NAV_MSL0 / 1000.;

  fdm->pressure = pprz_isa_pressure_of_altitude(fdm->hmsl);

  /* gps time */
  fdm->time = (double)UShortOfBuf(buffer, 27);

  /* in LTP pprz */
  ned_of_ecef_point_d(&fdm->ltpprz_pos, &ltpdef, &fdm->ecef_pos);
  fdm->lla_pos_pprz = pos;
  ned_of_ecef_vect_d(&fdm->ltpprz_ecef_vel, &ltpdef, &fdm->ecef_ecef_vel);

#if NPS_CRRCSIM_DEBUG
  printf("decode gps | pos %f %f %f | vel %f %f %f | time %f\n",
         57.3 * fdm->lla_pos.lat,
         57.3 * fdm->lla_pos.lon,
         fdm->lla_pos.alt,
         fdm->ltp_ecef_vel.x,
         fdm->ltp_ecef_vel.y,
         fdm->ltp_ecef_vel.z,
         fdm->time);
#endif
}
Пример #2
0
void baro_sim_periodic(void) {
  float pressure = pprz_isa_pressure_of_altitude(gps.hmsl / 1000.0);
  AbiSendMsgBARO_ABS(BARO_SIM_SENDER_ID, &pressure);
}