Changeset 68 in flair-src for trunk/lib/FlairSensorActuator/src


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

gps vtg

Location:
trunk/lib/FlairSensorActuator/src
Files:
3 edited

Legend:

Unmodified
Added
Removed

  • trunk/lib/FlairSensorActuator/src/NmeaGps.cpp

    r67 r68  
    5353  }
    5454
    55  /*
    56   if ((NMEAFlags & GST) != 0) {
    57     desc->SetElementName(index, 0, "dev_lat");
    58     desc->SetElementName(index + 1, 0, "dev_lon");
    59     desc->SetElementName(index + 2, 0, "dev_elv");
    60     index += 3;
    61   }
    62   */
    63 
    6455  // station sol
    6556  mainTab = new Tab(parent->GetTabWidget(), name);

  • 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

  • trunk/lib/FlairSensorActuator/src/SimuGps.h

    r55 r68  
    8787
    8888  typedef struct {
    89     float x;
    90     float y;
    91     float z;
     89    float e;
     90    float n;
     91    float u;
     92    float ve;
     93    float vn;
    9294  } gps_states_t;
    9395
Note: See TracChangeset for help on using the changeset viewer.