/*************************************************************************************** *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, <pdef, &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(<p_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, <pdef, &fdm->ecef_pos); fdm->lla_pos_pprz = pos; ned_of_ecef_vect_d(&fdm->ltpprz_ecef_vel, <pdef, &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 }
void baro_sim_periodic(void) { float pressure = pprz_isa_pressure_of_altitude(gps.hmsl / 1000.0); AbiSendMsgBARO_ABS(BARO_SIM_SENDER_ID, &pressure); }