Ignore:
Timestamp:
Aug 30, 2016, 6:42:07 PM (4 years ago)
Author:
Sanahuja Guillaume
Message:

gps vtg

File:
1 edited

Legend:

Unmodified
Added
Removed
  • trunk/lib/FlairSensorActuator/src/SimuGps.cpp

    r55 r68  
    7979    //simulator is ned, convert it to enu
    8080    //TODO: put simulator in enu?
    81     state.x = input->ValueNoMutex(5, 0);
    82     state.y = input->ValueNoMutex(4, 0);
    83     state.z = -input->ValueNoMutex(6, 0);
     81    state.e = input->ValueNoMutex(5, 0);//y simulator
     82    state.n = input->ValueNoMutex(4, 0);//x simulator
     83    state.u = -input->ValueNoMutex(6, 0);//z simulator
     84    state.ve = input->ValueNoMutex(11, 0);//vy simulator
     85    state.vn = input->ValueNoMutex(10, 0);//vx simulator
    8486    input->ReleaseMutex();
    8587
     
    9092void SimuGps::Run(void) {
    9193  gps_states_t state;
    92   char buf[500];
     94  char buf[512];
    9395  nmeaGPGGA gga;
    9496  nmeaGPVTG vtg;
    9597  nmeaPOS pos;
    9698  nmeaINFO info;
     99
     100  vtg.dir_t='T';
     101  vtg.dec_m='M';
     102  vtg.spk_k='K';
     103  vtg.spn_n='N';
    97104
    98105  if (dataRate == NULL) {
     
    105112  WarnUponSwitches(true);
    106113
    107   vtg.spn=1;
    108   vtg.dir=0;
    109114
    110115  while (!ToBeStopped()) {
     
    118123
    119124    double x, y, z;
    120     ENU_2_ECEF(state.x, state.y, state.z, x,y,z, Euler::ToRadian(longitudeRef->Value()), Euler::ToRadian(latitudeRef->Value()), altitudeRef->Value());
     125    ENU_2_ECEF(state.e, state.n, state.u, x,y,z, Euler::ToRadian(longitudeRef->Value()), Euler::ToRadian(latitudeRef->Value()), altitudeRef->Value());
    121126    ECEF_2_Geographique( x, y, z,pos.lon, pos.lat, gga.elv);
    122127    nmea_pos2info(&pos,&info);
     
    142147    nmea_gen_GPGGA(buf,sizeof(buf),&gga);
    143148    parseFrame(buf,sizeof(buf));
     149
     150    vtg.dir=90-Euler::ToDegree(atan2f(state.vn,state.ve));
     151    vtg.spk=sqrtf(state.ve*state.ve+state.vn*state.vn)*3600./1000.;
     152    nmea_gen_GPVTG(buf,sizeof(buf),&vtg);
     153    parseFrame(buf,sizeof(buf));
    144154  }
    145155
Note: See TracChangeset for help on using the changeset viewer.