Changeset 15 in flair-src for trunk/lib/FlairSensorActuator/src/Gps.cpp


Ignore:
Timestamp:
04/08/16 15:40:57 (7 years ago)
Author:
Bayard Gildas
Message:

sources reformatted with flair-format-dir script

File:
1 edited

Legend:

Unmodified
Added
Removed

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

    r3 r15  
    3838using namespace flair::gui;
    3939
    40 namespace flair
    41 {
    42 namespace sensor
    43 {
    44 
    45 Gps::Gps(const FrameworkManager* parent,string name,NMEAFlags_t NMEAFlags) : IODevice(parent,name)
    46 {
    47     this->NMEAFlags=NMEAFlags;
    48 
    49     nmea_zero_INFO(&info);
    50     nmea_parser_init(&parser);
    51     alt_ref=0;
    52 
    53 
    54     if((NMEAFlags&GGA)==0)
     40namespace flair {
     41namespace sensor {
     42
     43Gps::Gps(const FrameworkManager *parent, string name, NMEAFlags_t NMEAFlags)
     44    : IODevice(parent, name) {
     45  this->NMEAFlags = NMEAFlags;
     46
     47  nmea_zero_INFO(&info);
     48  nmea_parser_init(&parser);
     49  alt_ref = 0;
     50
     51  if ((NMEAFlags & GGA) == 0) {
     52    Err("Enable at least GGA sentence\n");
     53  }
     54
     55  int index = 0;
     56  if ((NMEAFlags & GGA) != 0) {
     57    index += 3;
     58  }
     59  if ((NMEAFlags & VTG) != 0) {
     60    index += 2;
     61  }
     62  if ((NMEAFlags & GST) != 0) {
     63    index += 3;
     64  }
     65
     66  cvmatrix_descriptor *desc = new cvmatrix_descriptor(index, 1);
     67  index = 0;
     68  if ((NMEAFlags & GGA) != 0) {
     69    desc->SetElementName(0, 0, "e");
     70    desc->SetElementName(1, 0, "n");
     71    desc->SetElementName(2, 0, "u");
     72    index += 3;
     73  }
     74  if ((NMEAFlags & VTG) != 0) {
     75    desc->SetElementName(index, 0, "ve");
     76    desc->SetElementName(index + 1, 0, "vn");
     77    index += 2;
     78  }
     79  if ((NMEAFlags & GST) != 0) {
     80    desc->SetElementName(index, 0, "dev_lat");
     81    desc->SetElementName(index + 1, 0, "dev_lon");
     82    desc->SetElementName(index + 2, 0, "dev_elv");
     83    index += 3;
     84  }
     85  output = new cvmatrix((IODevice *)this, desc, floatType);
     86  AddDataToLog(output);
     87
     88  // station sol
     89  main_tab = new Tab(parent->GetTabWidget(), name);
     90  tab = new TabWidget(main_tab->NewRow(), name);
     91  sensor_tab = new Tab(tab, "Reglages");
     92  GroupBox *reglages_groupbox = new GroupBox(sensor_tab->NewRow(), name);
     93  button_ref = new PushButton(reglages_groupbox->NewRow(), "set ref");
     94  nb_sat_label = new Label(reglages_groupbox->NewRow(), "nb_sat");
     95  fix_label = new Label(reglages_groupbox->LastRowLastCol(), "fix");
     96
     97  position = new GeoCoordinate((IODevice *)this, "position", 0, 0, 0);
     98
     99  fix = FixQuality_t::Invalid;
     100  nb_sat = 0;
     101  take_ref = false;
     102  nb_sat_label->SetText("nb_sat: %i", nb_sat);
     103  fix_label->SetText("fix: %i", fix);
     104}
     105
     106Gps::~Gps() {
     107  nmea_parser_destroy(&parser);
     108  delete main_tab;
     109}
     110
     111void Gps::UseDefaultPlot(void) {
     112  int index = 0;
     113  plot_tab = new Tab(tab, "Mesures");
     114
     115  if ((NMEAFlags & GGA) != 0) {
     116    e_plot = new DataPlot1D(plot_tab->NewRow(), "e", -10, 10);
     117    e_plot->AddCurve(output->Element(index));
     118    n_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "n", -10, 10);
     119    n_plot->AddCurve(output->Element(index + 1));
     120    u_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "u", -10, 10);
     121    u_plot->AddCurve(output->Element(index + 2));
     122    index += 3;
     123  }
     124  if ((NMEAFlags & VTG) != 0) {
     125    ve_plot = new DataPlot1D(plot_tab->NewRow(), "ve", -10, 10);
     126    ve_plot->AddCurve(output->Element(index));
     127    vn_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "vn", -10, 10);
     128    vn_plot->AddCurve(output->Element(index + 1));
     129    index += 2;
     130  }
     131
     132  Tab *map_tab = new Tab(tab, "carte");
     133  map = new Map(map_tab->NewRow(), "map");
     134  map->AddPoint(position, "drone");
     135}
     136
     137DataPlot1D *Gps::EPlot(void) const {
     138  if ((NMEAFlags & GGA) != 0) {
     139    return e_plot;
     140  } else {
     141    Err("GGA sentence not requested\n");
     142    return NULL;
     143  }
     144}
     145
     146DataPlot1D *Gps::NPlot(void) const {
     147  if ((NMEAFlags & GGA) != 0) {
     148    return n_plot;
     149  } else {
     150    Err("GGA sentence not requested\n");
     151    return NULL;
     152  }
     153}
     154
     155DataPlot1D *Gps::UPlot(void) const {
     156  if ((NMEAFlags & GGA) != 0) {
     157    return u_plot;
     158  } else {
     159    Err("GGA sentence not requested\n");
     160    return NULL;
     161  }
     162}
     163
     164DataPlot1D *Gps::VEPlot(void) const {
     165  if ((NMEAFlags & VTG) != 0) {
     166    return ve_plot;
     167  } else {
     168    Err("GGA sentence not requested\n");
     169    return NULL;
     170  }
     171}
     172
     173DataPlot1D *Gps::VNPlot(void) const {
     174  if ((NMEAFlags & VTG) != 0) {
     175    return vn_plot;
     176  } else {
     177    Err("GGA sentence not requested\n");
     178    return NULL;
     179  }
     180}
     181
     182Layout *Gps::GetLayout(void) const { return sensor_tab; }
     183
     184Tab *Gps::GetPlotTab(void) const { return plot_tab; }
     185
     186TabWidget *Gps::GetTab(void) const { return tab; }
     187
     188uint16_t Gps::NbSat(void) const {
     189  output->GetMutex();
     190  uint16_t result = nb_sat;
     191  output->ReleaseMutex();
     192  return result;
     193}
     194
     195Gps::FixQuality_t Gps::FixQuality(void) const {
     196  output->GetMutex();
     197  FixQuality_t result = fix;
     198  output->ReleaseMutex();
     199  return result;
     200}
     201
     202void Gps::SetRef(void) { take_ref = true; }
     203
     204void Gps::GetENUPosition(Vector3D *point) {
     205  output->GetMutex();
     206  point->x = output->ValueNoMutex(0, 0);
     207  point->y = output->ValueNoMutex(1, 0);
     208  point->z = output->ValueNoMutex(2, 0);
     209  output->ReleaseMutex();
     210}
     211
     212void Gps::parseFrame(const char *frame, int frame_size) {
     213
     214  int result;
     215
     216  result = nmea_parse(&parser, frame, frame_size, &info);
     217  if (result != 1) {
     218    Warn("unrecognized nmea sentence\n");
     219    Warn("%s\n", frame);
     220  }
     221
     222  result = nmea_parse_GPGGA(frame, frame_size, &pack);
     223
     224  if (result == 1) {
     225    // Printf("%s\n",frame);
     226    // Printf("nb:%i fix:%i lat:%f long:%f alt:%f vel:%f
     227    // angle:%f\n",pack.satinuse,pack.sig,info.lat,info.lon,info.elv,info.speed*1000./3600.,info.direction);
     228    // Printf("lat:%f long:%f\n",pos.lat,pos.lon);
     229    output->GetMutex(); // on utilise le mutex de l'output pour fix et nb_sat
     230    if ((int)fix != pack.sig) {
     231      fix = (FixQuality_t)pack.sig;
     232      fix_label->SetText("fix: %i", fix);
     233    }
     234    if (nb_sat != pack.satinuse) {
     235      nb_sat = pack.satinuse;
     236      nb_sat_label->SetText("nb_sat: %i", nb_sat);
     237    }
     238    output->ReleaseMutex();
     239
     240    nmea_info2pos(&info, &pos);
     241    position->SetCoordinates(Euler::ToDegree(pos.lat), Euler::ToDegree(pos.lon),
     242                             info.elv);
     243
     244    if ((info.sig == 2 && alt_ref == 0) || button_ref->Clicked() == true ||
     245        take_ref == true) {
     246      Printf("prise pos ref\n");
     247      lat_ref = pos.lat;
     248      long_ref = pos.lon;
     249      alt_ref = info.elv;
     250      take_ref = false;
     251    }
     252    // if(alt_ref!=0)
    55253    {
    56         Err("Enable at least GGA sentence\n");
    57     }
    58 
    59     int index=0;
    60     if((NMEAFlags&GGA)!=0)
    61     {
    62         index+=3;
    63     }
    64     if((NMEAFlags&VTG)!=0)
    65     {
    66         index+=2;
    67     }
    68     if((NMEAFlags&GST)!=0)
    69     {
    70         index+=3;
    71     }
    72 
    73     cvmatrix_descriptor* desc=new cvmatrix_descriptor(index,1);
    74     index=0;
    75     if((NMEAFlags&GGA)!=0)
    76     {
    77         desc->SetElementName(0,0,"e");
    78         desc->SetElementName(1,0,"n");
    79         desc->SetElementName(2,0,"u");
    80         index+=3;
    81     }
    82     if((NMEAFlags&VTG)!=0)
    83     {
    84         desc->SetElementName(index,0,"ve");
    85         desc->SetElementName(index+1,0,"vn");
    86         index+=2;
    87     }
    88     if((NMEAFlags&GST)!=0)
    89     {
    90         desc->SetElementName(index,0,"dev_lat");
    91         desc->SetElementName(index+1,0,"dev_lon");
    92         desc->SetElementName(index+2,0,"dev_elv");
    93         index+=3;
    94     }
    95     output=new cvmatrix((IODevice*)this,desc,floatType);
    96     AddDataToLog(output);
    97 
    98     //station sol
    99     main_tab=new Tab(parent->GetTabWidget(),name);
    100     tab=new TabWidget(main_tab->NewRow(),name);
    101         sensor_tab=new Tab(tab,"Reglages");
    102         GroupBox* reglages_groupbox=new GroupBox(sensor_tab->NewRow(),name);
    103         button_ref=new PushButton(reglages_groupbox->NewRow(),"set ref");
    104         nb_sat_label=new Label(reglages_groupbox->NewRow(),"nb_sat");
    105         fix_label=new Label(reglages_groupbox->LastRowLastCol(),"fix");
    106 
    107     position=new GeoCoordinate((IODevice*)this,"position",0,0,0);
    108 
    109     fix=FixQuality_t::Invalid;
    110     nb_sat=0;
    111     take_ref=false;
    112     nb_sat_label->SetText("nb_sat: %i",nb_sat);
    113     fix_label->SetText("fix: %i",fix);
    114 }
    115 
    116 Gps::~Gps()
    117 {
    118     nmea_parser_destroy(&parser);
    119     delete main_tab;
    120 }
    121 
    122 void Gps::UseDefaultPlot(void)
    123 {
    124     int index=0;
    125     plot_tab=new Tab(tab,"Mesures");
    126 
    127     if((NMEAFlags&GGA)!=0)
    128     {
    129         e_plot=new DataPlot1D(plot_tab->NewRow(),"e",-10,10);
    130             e_plot->AddCurve(output->Element(index));
    131         n_plot=new DataPlot1D(plot_tab->LastRowLastCol(),"n",-10,10);
    132             n_plot->AddCurve(output->Element(index+1));
    133         u_plot=new DataPlot1D(plot_tab->LastRowLastCol(),"u",-10,10);
    134             u_plot->AddCurve(output->Element(index+2));
    135         index+=3;
    136     }
    137     if((NMEAFlags&VTG)!=0)
    138     {
    139         ve_plot=new DataPlot1D(plot_tab->NewRow(),"ve",-10,10);
    140             ve_plot->AddCurve(output->Element(index));
    141         vn_plot=new DataPlot1D(plot_tab->LastRowLastCol(),"vn",-10,10);
    142             vn_plot->AddCurve(output->Element(index+1));
    143         index+=2;
    144     }
    145 
    146     Tab* map_tab=new Tab(tab,"carte");
    147         map=new Map(map_tab->NewRow(),"map");
    148             map->AddPoint(position,"drone");
    149 }
    150 
    151 DataPlot1D* Gps::EPlot(void) const
    152 {
    153     if((NMEAFlags&GGA)!=0)
    154     {
    155         return e_plot;
    156     }
    157     else
    158     {
    159         Err("GGA sentence not requested\n");
    160         return NULL;
    161     }
    162 }
    163 
    164 DataPlot1D* Gps::NPlot(void) const
    165 {
    166     if((NMEAFlags&GGA)!=0)
    167     {
    168         return n_plot;
    169     }
    170     else
    171     {
    172         Err("GGA sentence not requested\n");
    173         return NULL;
    174     }
    175 }
    176 
    177 DataPlot1D* Gps::UPlot(void) const
    178 {
    179     if((NMEAFlags&GGA)!=0)
    180     {
    181         return u_plot;
    182     }
    183     else
    184     {
    185         Err("GGA sentence not requested\n");
    186         return NULL;
    187     }
    188 }
    189 
    190 DataPlot1D* Gps::VEPlot(void) const
    191 {
    192     if((NMEAFlags&VTG)!=0)
    193     {
    194         return ve_plot;
    195     }
    196     else
    197     {
    198         Err("GGA sentence not requested\n");
    199         return NULL;
    200     }
    201 }
    202 
    203 DataPlot1D* Gps::VNPlot(void) const
    204 {
    205     if((NMEAFlags&VTG)!=0)
    206     {
    207         return vn_plot;
    208     }
    209     else
    210     {
    211         Err("GGA sentence not requested\n");
    212         return NULL;
    213     }
    214 }
    215 
    216 Layout* Gps::GetLayout(void) const
    217 {
    218     return sensor_tab;
    219 }
    220 
    221 Tab* Gps::GetPlotTab(void) const
    222 {
    223     return plot_tab;
    224 }
    225 
    226 TabWidget* Gps::GetTab(void) const
    227 {
    228     return tab;
    229 }
    230 
    231 uint16_t Gps::NbSat(void) const
    232 {
    233     output->GetMutex();
    234     uint16_t result=nb_sat;
    235     output->ReleaseMutex();
    236     return result;
    237 }
    238 
    239 Gps::FixQuality_t Gps::FixQuality(void) const
    240 {
    241     output->GetMutex();
    242     FixQuality_t result=fix;
    243     output->ReleaseMutex();
    244     return result;
    245 }
    246 
    247 void Gps::SetRef(void)
    248 {
    249     take_ref=true;
    250 }
    251 
    252 void Gps::GetENUPosition(Vector3D *point)
    253 {
    254     output->GetMutex();
    255     point->x=output->ValueNoMutex(0,0);
    256     point->y=output->ValueNoMutex(1,0);
    257     point->z=output->ValueNoMutex(2,0);
    258     output->ReleaseMutex();
    259 }
    260 
    261 void Gps::parseFrame(const char *frame, int frame_size){
    262 
    263     int result;
    264 
    265     result=nmea_parse(&parser, frame, frame_size, &info);
    266     if(result!=1)
    267     {
    268         Warn("unrecognized nmea sentence\n");
    269         Warn("%s\n",frame);
    270     }
    271 
    272     result=nmea_parse_GPGGA(frame, frame_size, &pack);
    273 
    274     if(result==1)
    275     {
    276         //Printf("%s\n",frame);
    277         //Printf("nb:%i fix:%i lat:%f long:%f alt:%f vel:%f angle:%f\n",pack.satinuse,pack.sig,info.lat,info.lon,info.elv,info.speed*1000./3600.,info.direction);
    278         //Printf("lat:%f long:%f\n",pos.lat,pos.lon);
    279         output->GetMutex();//on utilise le mutex de l'output pour fix et nb_sat
    280         if((int)fix!=pack.sig)
    281         {
    282             fix=(FixQuality_t)pack.sig;
    283             fix_label->SetText("fix: %i",fix);
    284         }
    285         if(nb_sat!=pack.satinuse)
    286         {
    287             nb_sat=pack.satinuse;
    288             nb_sat_label->SetText("nb_sat: %i",nb_sat);
    289         }
    290         output->ReleaseMutex();
    291 
    292 
    293         nmea_info2pos(&info,&pos);
    294         position->SetCoordinates(Euler::ToDegree(pos.lat),Euler::ToDegree(pos.lon),info.elv);
    295 
    296         if((info.sig==2 && alt_ref==0) || button_ref->Clicked()==true || take_ref==true)
    297         {
    298             Printf("prise pos ref\n");
    299             lat_ref=pos.lat;
    300             long_ref=pos.lon;
    301             alt_ref=info.elv;
    302             take_ref=false;
    303         }
    304         //if(alt_ref!=0)
    305         {
    306             double x,y,z;
    307             double e,n,u;
    308             Geographique_2_ECEF(pos.lon,pos.lat,info.elv,x,y,z);
    309             ECEF_2_ENU(x,y,z,e, n, u,long_ref,lat_ref,alt_ref);
    310             //Printf("lon:%f lat:%f elv:%f\n",pos.lon,pos.lat,info.elv);
    311 
    312             //on prend une fois pour toute le mutex et on fait des accès directs
    313             output->GetMutex();
    314             output->SetValueNoMutex( 0, 0,e);
    315             output->SetValueNoMutex( 1, 0,n);
    316             output->SetValueNoMutex( 2, 0,u);
    317 
    318             int index=3;
    319             if((NMEAFlags&VTG)!=0)
    320             {
    321                 output->SetValueNoMutex( index, 0,info.speed*1000./3600.*sin(Euler::ToRadian(info.direction)));
    322                 output->SetValueNoMutex( index+1, 0,info.speed*1000./3600.*cos(Euler::ToRadian(info.direction)));
    323                 index+=2;
    324             }
    325             if((NMEAFlags&GST)!=0)
    326             {
    327                 //Thread::Printf("dev_lon:%f dev_lat:%f dev_elv:%f\n",info.dev_lat,info.dev_lon,info.dev_elv);
    328                 output->SetValueNoMutex( index, 0,info.dev_lat);
    329                 output->SetValueNoMutex( index+1, 0,info.dev_lon);
    330                 output->SetValueNoMutex( index+2, 0,info.dev_elv);
    331                 index+=3;
    332             }
    333             output->ReleaseMutex();
    334 
    335             output->SetDataTime(GetTime());
    336             ProcessUpdate(output);
    337         }
    338     }
     254      double x, y, z;
     255      double e, n, u;
     256      Geographique_2_ECEF(pos.lon, pos.lat, info.elv, x, y, z);
     257      ECEF_2_ENU(x, y, z, e, n, u, long_ref, lat_ref, alt_ref);
     258      // Printf("lon:%f lat:%f elv:%f\n",pos.lon,pos.lat,info.elv);
     259
     260      // on prend une fois pour toute le mutex et on fait des accès directs
     261      output->GetMutex();
     262      output->SetValueNoMutex(0, 0, e);
     263      output->SetValueNoMutex(1, 0, n);
     264      output->SetValueNoMutex(2, 0, u);
     265
     266      int index = 3;
     267      if ((NMEAFlags & VTG) != 0) {
     268        output->SetValueNoMutex(index, 0,
     269                                info.speed * 1000. / 3600. *
     270                                    sin(Euler::ToRadian(info.direction)));
     271        output->SetValueNoMutex(index + 1, 0,
     272                                info.speed * 1000. / 3600. *
     273                                    cos(Euler::ToRadian(info.direction)));
     274        index += 2;
     275      }
     276      if ((NMEAFlags & GST) != 0) {
     277        // Thread::Printf("dev_lon:%f dev_lat:%f
     278        // dev_elv:%f\n",info.dev_lat,info.dev_lon,info.dev_elv);
     279        output->SetValueNoMutex(index, 0, info.dev_lat);
     280        output->SetValueNoMutex(index + 1, 0, info.dev_lon);
     281        output->SetValueNoMutex(index + 2, 0, info.dev_elv);
     282        index += 3;
     283      }
     284      output->ReleaseMutex();
     285
     286      output->SetDataTime(GetTime());
     287      ProcessUpdate(output);
     288    }
     289  }
    339290}
    340291
Note: See TracChangeset for help on using the changeset viewer.